From 84e4f43696066b79feb915430f624eb8db4c0fff Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Tue, 14 Jan 2025 15:42:50 -0600 Subject: [PATCH] Mitigate the very rare cases where the Sk inversion fails --- src/od/filter/kalman.rs | 12 ++++++++---- tests/orbit_determination/simulator.rs | 4 +++- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 7b94c170..df3e066b 100644 --- a/src/od/filter/kalman.rs +++ b/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. diff --git a/tests/orbit_determination/simulator.rs b/tests/orbit_determination/simulator.rs index 277702a8..cf25d0cd 100644 --- a/tests/orbit_determination/simulator.rs +++ b/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());