Merge pull request #378 from nyx-space/fix/gh-377

Bound the proposed integrator steps

src/propagators/instance.rs

@@ -26,6 +26,7 @@ use crate::propagators::TrajectoryEventSnafu;
 use crate::time::{Duration, Epoch, Unit};
 use crate::State;
 use anise::almanac::Almanac;
+use anise::errors::MathError;
 use rayon::iter::ParallelBridge;
 use rayon::prelude::ParallelIterator;
 use snafu::ResultExt;
@@ -222,7 +223,10 @@ where
                             // Report status every minute
                             let cur_epoch = self.state.epoch();
                             let dur_to_go = (stop_time - cur_epoch).floor(Unit::Second * 1);
-                            info!("\t... current epoch {}, remaing {}", cur_epoch, dur_to_go);
+                            info!(
+                                "\t... current epoch {}, remaining {} (step size = {})",
+                                cur_epoch, dur_to_go, self.details.step
+                            );
                             prev_tick = Instant::now();
                         }
                     }
@@ -385,7 +389,7 @@ where
         // Reset the number of attempts used (we don't reset the error because it's set before it's read)
         self.details.attempts = 1;
         // Convert the step size to seconds -- it's mutable because we may change it below
-        let mut step_size = self.step_size.to_seconds();
+        let mut step_size_s = self.step_size.to_seconds();
         loop {
             let ki = self
                 .prop
@@ -411,8 +415,8 @@ where
                     .prop
                     .dynamics
                     .eom(
-                        ci * step_size,
-                        &(state_vec + step_size * wi),
+                        ci * step_size_s,
+                        &(state_vec + step_size_s * wi),
                         state_ctx,
                         self.almanac.clone(),
                     )
@@ -429,9 +433,9 @@ where
                 let b_i = self.prop.method.b_coeffs()[i];
                 if !self.fixed_step {
                     let b_i_star = self.prop.method.b_coeffs()[i + self.prop.method.stages()];
-                    error_est += step_size * (b_i - b_i_star) * ki;
+                    error_est += step_size_s * (b_i - b_i_star) * ki;
                 }
-                next_state += step_size * b_i * ki;
+                next_state += step_size_s * b_i * ki;
             }
 
             if self.fixed_step {
@@ -445,46 +449,66 @@ where
                         .opts
                         .error_ctrl
                         .estimate(&error_est, &next_state, state_vec);
+
                 if self.details.error <= self.prop.opts.tolerance
-                    || step_size <= self.prop.opts.min_step.to_seconds()
+                    || step_size_s <= self.prop.opts.min_step.to_seconds()
                     || self.details.attempts >= self.prop.opts.attempts
                 {
+                    if next_state.iter().any(|x| x.is_nan()) {
+                        return Err(PropagationError::PropMathError {
+                            source: MathError::DomainError {
+                                value: f64::NAN,
+                                msg: "try another integration method, or decrease step size; part of state vector is",
+                            },
+                        });
+                    }
                     if self.details.attempts >= self.prop.opts.attempts {
                         warn!(
                             "Could not further decrease step size: maximum number of attempts reached ({})",
                             self.details.attempts
                         );
                     }
 
-                    self.details.step = step_size * Unit::Second;
+                    self.details.step = step_size_s * Unit::Second;
                     if self.details.error < self.prop.opts.tolerance {
                         // Let's increase the step size for the next iteration.
                         // Error is less than tolerance, let's attempt to increase the step for the next iteration.
                         let proposed_step = 0.9
-                            * step_size
+                            * step_size_s
                             * (self.prop.opts.tolerance / self.details.error)
                                 .powf(1.0 / f64::from(self.prop.method.order()));
-                        step_size = if proposed_step > self.prop.opts.max_step.to_seconds() {
+
+                        step_size_s = if proposed_step > self.prop.opts.max_step.to_seconds() {
                             self.prop.opts.max_step.to_seconds()
                         } else {
                             proposed_step
                         };
                     }
                     // In all cases, let's update the step size to whatever was the adapted step size
-                    self.step_size = step_size * Unit::Second;
+                    self.step_size = step_size_s * Unit::Second;
+                    if self.step_size.abs() < self.prop.opts.min_step {
+                        // Custom signum in case the step size becomes zero.
+                        let signum = if self.step_size.is_negative() {
+                            -1.0
+                        } else {
+                            1.0
+                        };
+                        self.step_size = self.prop.opts.min_step * signum;
+                    }
                     return Ok((self.details.step, next_state));
                 } else {
                     // Error is too high and we aren't using the smallest step, and we haven't hit the max number of attempts.
                     // So let's adapt the step size.
                     self.details.attempts += 1;
-                    let proposed_step = 0.9
-                        * step_size
+                    let proposed_step_s = 0.9
+                        * step_size_s
                         * (self.prop.opts.tolerance / self.details.error)
                             .powf(1.0 / f64::from(self.prop.method.order() - 1));
-                    step_size = if proposed_step < self.prop.opts.min_step.to_seconds() {
+
+                    step_size_s = if proposed_step_s < self.prop.opts.min_step.to_seconds() {
                         self.prop.opts.min_step.to_seconds()
                     } else {
-                        proposed_step
+                        proposed_step_s
                     };
                     // Note that we don't set self.step_size, that will be updated right before we return
                 }

src/propagators/mod.rs

@@ -16,6 +16,7 @@
     along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 
+use anise::errors::MathError;
 use snafu::prelude::*;
 use std::fmt;
 
@@ -66,4 +67,6 @@ pub enum PropagationError {
     NthEventError { nth: usize, found: usize },
     #[snafu(display("propagation failed because {source}"))]
     PropConfigError { source: ConfigError },
+    #[snafu(display("propagation encountered a math error {source}"))]
+    PropMathError { source: MathError },
 }

tests/orbit_determination/spacecraft.rs

@@ -329,7 +329,7 @@ fn od_val_sc_srp_estimation(
     let setup = Propagator::dp78(
         sc_dynamics,
         IntegratorOptions::builder()
-            .max_step(1.minutes())
+            .max_step(30.seconds())
             .error_ctrl(ErrorControl::RSSCartesianStep)
             .build(),
     );