Mitigate the very rare cases where the Sk inversion fails

Author: ChristopherRabotin

src/od/filter/kalman.rs

@@ -300,10 +300,14 @@ where
         // Compute the prefit ratio for the automatic rejection.
         // The measurement covariance is the square of the measurement itself.
         // So we compute its Cholesky decomposition to return to the non squared values.
-        dbg!(epoch);
-        dbg!(&r_k);
-        dbg!(&s_k);
-        let r_k_chol = s_k.clone().cholesky().ok_or(ODError::SingularNoiseRk)?.l();
+        let r_k_chol = match s_k.clone().cholesky() {
+            Some(r_k_clone) => r_k_clone.l(),
+            None => {
+                // In very rare case, when there isn't enough noise in the measurements,
+                // the inverting of S_k fails. If so, we revert back to the nominal Kalman derivation.
+                r_k.clone().cholesky().ok_or(ODError::SingularNoiseRk)?.l()
+            }
+        };
 
         // Compute the ratio as the average of each component of the prefit over the square root of the measurement
         // matrix r_k. Refer to ODTK MathSpec equation 4.10.

tests/orbit_determination/simulator.rs

@@ -257,7 +257,9 @@ fn od_with_modulus_cov_test(
 
     println!("{estimate}");
 
-    let kf = KF::no_snc(estimate);
+    let sigma_q = 1e-12_f64.powi(2);
+    let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]);
+    let kf = KF::new(estimate, process_noise);
 
     let setup = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body()));
     let prop = setup.with(spacecraft.with_stm(), almanac.clone());