Errors still aren't quite right

src/controller.rs

@@ -35,6 +35,7 @@ where
         let factor = self.factor_c2.max(self.factor_c1.min(factor_11 * self.factor_old.powf(-self.beta) / self.safety_factor));
         let mut h_new = h / factor;
         if err <= 1.0.into() {
+            println!("{:?}", f64::from(h));
             // Accept the stepsize
             self.factor_old = err.max(1.0e-4.into());
             if h_new.abs() > self.h_max {
@@ -42,6 +43,7 @@ where
                 h_new = self.h_max.copysign(h_new);
             }
             (true, h_new)
+            // (true, h_new)
         } else {
             // Reject the stepsize and propose a smaller one
             h_new = h / (self.factor_c1.min(factor_11 / self.safety_factor));

src/integrator.rs

@@ -48,8 +48,8 @@ where
             k0 * self.tableau().b[0] +
             k1 * self.tableau().b[1];
         let err = match self.tableau().b_star {
-            Some(b_star) => Some(sum + (k0 * b_star[0] +
+            Some(b_star) => Some(sum + ((k0 * b_star[0] +
-                                        k1 * b_star[1]) * T::from(-1.0_f64)),
+                                        k1 * b_star[1]) * T::from(-1.0_f64)) * h),
             None => None
         };
         (ode.y + (sum * h), err)
@@ -73,9 +73,9 @@ where
             k1 * self.tableau().b[1] +
             k2 * self.tableau().b[2];
         let err = match self.tableau().b_star {
-            Some(b_star) => Some(sum + (k0 * b_star[0] +
+            Some(b_star) => Some(sum + ((k0 * b_star[0] +
                                         k1 * b_star[1] +
-                                        k2 * b_star[2]) * T::from(-1.0_f64)),
+                                        k2 * b_star[2]) * T::from(-1.0_f64)) * h),
             None => None
         };
         (ode.y + (sum * h), err)
@@ -108,10 +108,10 @@ where
             k2 * self.tableau().b[2] +
             k3 * self.tableau().b[3];
         let err = match self.tableau().b_star {
-            Some(b_star) => Some(sum + (k0 * b_star[0] +
+            Some(b_star) => Some(sum + ((k0 * b_star[0] +
                                         k1 * b_star[1] +
                                         k2 * b_star[2] +
-                                        k3 * b_star[3]) * T::from(-1.0_f64)),
+                                        k3 * b_star[3]) * T::from(-1.0_f64)) * h),
             None => None
         };
         (ode.y + (sum * h), err)
@@ -154,11 +154,11 @@ where
             k3 * self.tableau().b[3] +
             k4 * self.tableau().b[4];
         let err = match self.tableau().b_star {
-            Some(b_star) => Some(sum + (k0 * b_star[0] +
+            Some(b_star) => Some(sum + ((k0 * b_star[0] +
                                         k1 * b_star[1] +
                                         k2 * b_star[2] +
                                         k3 * b_star[3] +
-                                        k4 * b_star[4]) * T::from(-1.0_f64)),
+                                        k4 * b_star[4]) * T::from(-1.0_f64)) * h),
             None => None
         };
         (ode.y + (sum * h), err)
@@ -212,12 +212,12 @@ where
             k4 * self.tableau().b[4] +
             k5 * self.tableau().b[5];
         let err = match self.tableau().b_star {
-            Some(b_star) => Some(sum + (k0 * b_star[0] +
+            Some(b_star) => Some((sum + (k0 * b_star[0] +
                                         k1 * b_star[1] +
                                         k2 * b_star[2] +
                                         k3 * b_star[3] +
                                         k4 * b_star[4] +
-                                        k5 * b_star[5]) * T::from(-1.0_f64)),
+                                        k5 * b_star[5]) * T::from(-1.0_f64)) * h),
             None => None
         };
         (ode.y + (sum * h), err)
@@ -285,7 +285,7 @@ where
             let tol = self.atol + yi.abs().max(next_yi.abs()) * self.rtol;
             err += (err_array[i] / tol).powi(2);
         });
-        (next_y, Some(err))
+        (next_y, Some(err.sqrt()))
     }
 }
 
@@ -347,7 +347,7 @@ mod tests {
 
         let system = System {};
         let y0 = Vector3::new(1.0, 1.0, 1.0);
-        let mut ode = ODE::new(system, 0.0, 10.0, y0);
+        let mut ode = ODE::new(system, 0.0, 30.0, y0);
 
         let rk4 = FixedFourthOrder { tableau: RUNGE_KUTTA_4_TABLEAU };
 

src/lib.rs

@@ -11,38 +11,55 @@ pub mod problem;
 mod tests {
     use nalgebra::Vector6;
     use approx::assert_relative_eq;
-    use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU};
+    use crate::integrator::*;
     use crate::controller::{PIController};
     use crate::ode::{SystemTrait, ODE};
     use crate::problem::Problem;
+    use std::f64::consts::PI;
 
     #[test]
-    fn test_ode_creation() {
+    fn test_orbit() {
         struct System {
             mu: f64,
         }
 
         impl SystemTrait<f64,6> for System {
             fn derivative(&self, _t: f64, state: Vector6<f64>) -> Vector6<f64> {
-                let acc = -(self.mu * state.fixed_rows<3>(0)) / (state.fixed_rows<3>(0).norm.powi(3));
+                let acc = -(self.mu * state.fixed_rows::<3>(0)) / (state.fixed_rows::<3>(0).norm().powi(3));
                 Vector6::new(state[3], state[4], state[5], acc[0], acc[1], acc[2])
             }
         }
 
         let system = System { mu: 3.98600441500000e14 };
-        let y0 = Vector3::new(1.0, 0.0, 0.0);
+        let y0 = Vector6::new(
-        let ode = ODE::new(system, 0.0, 10.0, y0);
+            4.26387250e+06,
-        let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4);
+            5.14619397e+06,
+            1.13102192e+06,
+            -5.92345023e+03,
+            4.49679662e+03,
+            1.87038714e+03,
+        );
+        let a = 6.7781363e6_f64;
+        let period = 2.0 * PI * (a.powi(3)/system.mu).sqrt();
+        println!("{}", period);
+        let ode = ODE::new(system, 0.0, period, y0);
+        let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10000.0, 0.9, 1.0);
         let rkf54 = AdaptiveSixthOrder {
             tableau: RUNGE_KUTTA_FEHLBERG_54_TABLEAU,
-            atol: 1e-8_f64,
+            atol: 1e-12_f64,
-            rtol: 1e-8_f64,
+            rtol: 1e-7_f64,
         };
         let mut problem = Problem::new(ode, rkf54, controller);
 
         let solution = problem.solve();
-        solution.times.iter().zip(solution.states.iter()).for_each(|(time, state)| {
+        println!("{}", solution.times.len());
-            assert_relative_eq!(state[0], time.exp(), max_relative=1e-7);
+        // panic!();
-        })
+        assert_relative_eq!(solution.times[solution.states.len()-1], period, max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][0], y0[0], max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][1], y0[1], max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][2], y0[2], max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][3], y0[3], max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][4], y0[4], max_relative=1e-7);
+        assert_relative_eq!(solution.states[solution.states.len()-1][5], y0[5], max_relative=1e-7);
     }
 }

src/problem.rs

@@ -67,10 +67,10 @@ where
         let mut times: Vec::<T> = Vec::new();
         let mut states: Vec::<SVector<T,D1>> = Vec::new();
         let mut step: T = self.controller.old_h;
-        while self.ode.t < self.ode.t_end {
-            let (mut new_y, mut err_option) = self.integrator.step(&self.ode, step);
         times.push(self.ode.t);
         states.push(self.ode.y);
+        while self.ode.t < self.ode.t_end {
+            let (mut new_y, mut err_option) = self.integrator.step(&self.ode, step);
             match err_option {
                 Some(mut err) => {
                     // Adaptive Step Size
@@ -81,15 +81,14 @@ where
                         err = err_option.unwrap();
                     }
                     self.controller.old_h = step;
+                    self.controller.h_max = self.ode.t_end - self.ode.t - step;
+                },
+                None => {},
+            };
             self.ode.y = new_y;
             self.ode.t += step;
-                },
+            times.push(self.ode.t);
-                None => {
+            states.push(self.ode.y);
-                    // Fixed Step Size
-                    self.ode.y = new_y;
-                    self.ode.t += self.controller.old_h;
-                },
-            };
         }
         Solution {
             times: times,
@@ -111,8 +110,8 @@ mod tests {
     use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU};
     use crate::controller::{PIController};
 
-    #[test]
+    // #[test]
-    fn test_ode_creation() {
+    fn test_problem() {
         struct System {}
 
         impl SystemTrait<f64,3> for System {