Account for state covariance in measurement noise

.cargo/config.toml

@@ -9,3 +9,8 @@ rustflags = [
   "-C", "link-arg=-undefined",
   "-C", "link-arg=dynamic_lookup",
 ]
+
+[target.x86_64-unknown-linux-gnu]
+linker = "clang"
+rustflags = ["-C", "link-arg=-fuse-ld=lld"]
+

data/tests/config/high-prec-network.yaml

@@ -1,9 +1,7 @@
 range_noise_model:
   tau: 12 h 159 ms
-  bias_sigma: 5.0e-3 # 5 m
-  steady_state_sigma: 0.1e-3 # 0.1 m
+  process_noise: 5.0e-3 # 5 m
 
 doppler_noise_model:
   tau: 11 h 59 min
-  bias_sigma: 50.0e-6 # 5 cm/s
-  steady_state_sigma: 1.5e-6 # 0.15 cm/s
+  process_noise: 50.0e-6 # 5 cm/s

data/tests/config/many_ground_stations.yaml

@@ -6,13 +6,13 @@
     shape: null
   elevation_mask_deg: 5.0
   range_noise_km:
-    tau: 24 h
-    bias_sigma: 5.0e-3 # 5 m
-    steady_state_sigma: 0.1e-3 # 0.1 m
+    bias:
+      tau: 24 h
+      process_noise: 5.0e-3 # 5 m
   doppler_noise_km_s:
-    tau: 24 h
-    bias_sigma: 50.0e-6 # 5 cm/s
-    steady_state_sigma: 1.5e-6 # 0.15 cm/s
+    bias:
+      tau: 24 h
+      process_noise: 50.0e-6 # 5 cm/s
   light_time_correction: false
   latitude_deg: 2.3522
   longitude_deg: 48.8566
@@ -29,11 +29,11 @@
   height_km: 0.691750
   elevation_mask_deg: 5.0
   range_noise_km:
-    tau: 24 h
-    bias_sigma: 5.0e-3 # 5 m
-    steady_state_sigma: 0.1e-3 # 0.1 m
+    bias:
+      tau: 24 h
+      process_noise: 5.0e-3 # 5 m
   doppler_noise_km_s:
-    tau: 24 h
-    bias_sigma: 50.0e-6 # 5 cm/s
-    steady_state_sigma: 1.5e-6 # 0.15 cm/s
+    bias:
+      tau: 24 h
+      process_noise: 50.0e-6 # 5 cm/s
   light_time_correction: false

data/tests/config/one_ground_station.yaml

@@ -9,11 +9,11 @@ latitude_deg: 2.3522
 longitude_deg: 48.8566
 height_km: 0.4
 range_noise_km:
-  tau: 24 h
-  bias_sigma: 5.0e-3 # 5 m
-  steady_state_sigma: 0.1e-3 # 0.1 m
+  bias:
+    tau: 24 h
+    process_noise: 5.0e-3 # 5 m
 doppler_noise_km_s:
-  tau: 24 h
-  bias_sigma: 50.0e-6 # 5 cm/s
-  steady_state_sigma: 1.5e-6 # 0.15 cm/s
+  bias:
+    tau: 24 h
+    process_noise: 50.0e-6 # 5 cm/s
 light_time_correction: false

examples/02_jwst_covar_monte_carlo/main.rs

@@ -14,7 +14,6 @@ use nyx::{
     cosmic::{eclipse::EclipseLocator, Frame, MetaAlmanac, SrpConfig},
     dynamics::{guidance::LocalFrame, OrbitalDynamics, SolarPressure, SpacecraftDynamics},
     io::ExportCfg,
-    linalg::{Matrix2, Vector2},
     mc::MonteCarlo,
     od::{prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess},
     propagators::Propagator,
@@ -112,8 +111,7 @@ fn main() -> Result<(), Box<dyn Error>> {
     // For the covariance mapping / prediction, we'll use the common orbit determination approach.
     // This is done by setting up a spacecraft OD process, and predicting for the analysis duration.
 
-    let measurement_noise = Matrix2::from_diagonal(&Vector2::new(1e-6, 1e-3));
-    let ckf = KF::no_snc(jwst_estimate, measurement_noise);
+    let ckf = KF::no_snc(jwst_estimate);
 
     // Build the propagation instance for the OD process.
     let prop = setup.with(jwst.with_stm(), almanac.clone());

python/nyx_space/plots/gauss_markov.py

@@ -17,6 +17,7 @@
 """
 
 import plotly.express as px
+import plotly.graph_objects as go
 
 from .utils import finalize_plot
 
@@ -29,6 +30,7 @@ def plot_gauss_markov(df, title="Gauss Markov Process", tau=None):
     # Grab the column
     try:
         col_name = [col for col in df.columns if "Bias" in col][0]
+        variance_name = [col for col in df.columns if "Variance" in col][0]
     except IndexError:
         raise ValueError("No bias column found in the provided data frame")
 
@@ -41,8 +43,6 @@ def plot_gauss_markov(df, title="Gauss Markov Process", tau=None):
         marginal_y="rug",
     )
 
-    # TODO: Consider adding the autocorrelation data for clarity
-
     if tau:
         fig.add_vline(
             x=tau,
@@ -53,6 +53,26 @@ def plot_gauss_markov(df, title="Gauss Markov Process", tau=None):
             col=1,
         )
 
-    finalize_plot(fig, title=title)
+    for run in df["Run"].unique():
+        this_df = df[df["Run"] == run]
+        fig.add_trace(
+            go.Scatter(
+                x=this_df["Delta Time (s)"],
+                y=this_df[variance_name],
+                mode='lines',
+                name=f'Variance for {run}'
+            )
+        )
+
+
+    # fig = px.line(
+    #     df,
+    #     x="Delta Time (s)",
+    #     y=variance_name,
+    #     color="Run",
+    #     opacity=0.5,
+    # )
+
+    # finalize_plot(fig, title=title)
 
     fig.show()

src/mc/multivariate.rs

@@ -69,11 +69,13 @@ where
         + Allocator<f64, S::Size, <S::Size as DimMin<S::Size>>::Output>
         + Allocator<f64, <S::Size as DimMin<S::Size>>::Output, S::Size>
         + Allocator<f64, <S::Size as DimSub<Const<1>>>::Output>
-        + Allocator<f64, S::Size, <S::Size as DimSub<Const<1>>>::Output>,
+        + Allocator<f64, S::Size, <S::Size as DimSub<Const<1>>>::Output>
+        + Allocator<(f64, usize), <S::Size as DimMin<S::Size>>::Output>,
     <DefaultAllocator as Allocator<f64, S::VecLength>>::Buffer: Send,
     S::Size: DimMin<S::Size>,
     <S::Size as DimMin<S::Size>>::Output: DimSub<Const<1>>,
     S::Size: DimSub<Const<1>>,
+    DefaultAllocator: Allocator<(usize, usize), <S::Size as na::DimMin<S::Size>>::Output>,
 {
     /// Creates a new Monte Carlos state generator from a mean and covariance which must be of the same size as the state vector
     /// The covariance must be positive semi definite. The algorithm is the one from numpy
@@ -96,7 +98,7 @@ where
             }
         };
 
-        let svd = cov.svd_unordered(false, true);
+        let svd = cov.svd(false, true);
         if svd.v_t.is_none() {
             return Err(NyxError::CovarianceMatrixNotPsd);
         }
@@ -154,8 +156,10 @@ where
     <S::Size as DimMin<S::Size>>::Output: DimName,
 {
     fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> DispersedState<S> {
+        // TODO: Switch to nalgebra-mvm
         // Generate the vector representing the state
         let x_rng = OVector::<f64, S::Size>::from_fn(|_, _| self.std_norm_distr.sample(rng));
+        println!("{x_rng}\n{:.6}", self.sqrt_s_v);
         let x = self.sqrt_s_v.transpose() * x_rng + &self.mean;
         let mut state = self.template;
 

src/od/estimate/kfestimate.rs

@@ -303,7 +303,14 @@ where
         };
         let mut fmt_cov = Vec::with_capacity(dim);
         for i in 0..dim {
-            fmt_cov.push(format!("{:e}", &self.covar[(i, i)]));
+            let unit = if i < 3 {
+                "km"
+            } else if i < 6 {
+                "km/s"
+            } else {
+                ""
+            };
+            fmt_cov.push(format!("{:.6} {unit}", &self.covar[(i, i)]));
         }
         write!(
             f,
@@ -312,7 +319,7 @@ where
             &self.epoch(),
             self.within_3sigma(),
             &self.state(),
-            fmt_cov.join(",")
+            fmt_cov.join(", ")
         )
     }
 }

src/od/estimate/residual.rs

@@ -36,8 +36,12 @@ where
     pub postfit: OVector<f64, M>,
     /// The prefit residual ratio, i.e. `r' * (H*P*H')^-1 * r`, where `r` is the prefit residual, `H` is the sensitivity matrix, and `P` is the covariance matrix.
     pub ratio: f64,
+    /// The tracker measurement noise (variance)) for this tracker at this time.
+    pub tracker_msr_noise: OVector<f64, M>,
     /// Whether or not this was rejected
     pub rejected: bool,
+    /// Name of the tracker that caused this residual
+    pub tracker: Option<String>,
 }
 
 impl<M> Residual<M>
@@ -51,34 +55,46 @@ where
             epoch: Epoch::from_tai_seconds(0.0),
             prefit: OVector::<f64, M>::zeros(),
             postfit: OVector::<f64, M>::zeros(),
+            tracker_msr_noise: OVector::<f64, M>::zeros(),
             ratio: 0.0,
             rejected: true,
+            tracker: None,
         }
     }
 
     /// Flags a Residual as rejected.
-    pub fn rejected(epoch: Epoch, prefit: OVector<f64, M>, ratio: f64) -> Self {
+    pub fn rejected(
+        epoch: Epoch,
+        prefit: OVector<f64, M>,
+        ratio: f64,
+        tracker_msr_noise: OVector<f64, M>,
+    ) -> Self {
         Self {
             epoch,
             prefit,
             postfit: OVector::<f64, M>::zeros(),
             ratio,
+            tracker_msr_noise,
             rejected: true,
+            tracker: None,
         }
     }
 
-    pub fn new(
+    pub fn accepted(
         epoch: Epoch,
         prefit: OVector<f64, M>,
         postfit: OVector<f64, M>,
         ratio: f64,
+        tracker_msr_noise: OVector<f64, M>,
     ) -> Self {
         Self {
             epoch,
             prefit,
             postfit,
             ratio,
+            tracker_msr_noise,
             rejected: false,
+            tracker: None,
         }
     }
 }
@@ -89,7 +105,15 @@ where
     DefaultAllocator: Allocator<f64, M> + Allocator<usize, M>,
 {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "Prefit {} Postfit {}", &self.prefit, &self.postfit)
+        write!(
+            f,
+            "Residual from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}",
+            self.tracker.as_ref().unwrap_or(&"Unknown".to_string()),
+            self.epoch,
+            self.ratio,
+            &self.prefit,
+            &self.postfit
+        )
     }
 }
 

src/od/estimate/sc_uncertainty.rs

@@ -143,17 +143,17 @@ impl fmt::Display for SpacecraftUncertainty {
         writeln!(f, "{}", self.nominal)?;
         writeln!(
             f,
-            "{frame}  Σ_x = {} km  Σ_y = {} km  Σ_z = {} km",
+            "{frame}  σ_x = {} km  σ_y = {} km  σ_z = {} km",
             self.x_km, self.y_km, self.z_km
         )?;
         writeln!(
             f,
-            "{frame}  Σ_vx = {} km/s  Σ_vy = {} km/s  Σ_vz = {} km/s",
+            "{frame}  σ_vx = {} km/s  σ_vy = {} km/s  σ_vz = {} km/s",
             self.vx_km_s, self.vy_km_s, self.vz_km_s
         )?;
         writeln!(
             f,
-            "Σ_cr = {}  Σ_cd = {}  Σ_mass = {} kg",
+            "σ_cr = {}  σ_cd = {}  σ_mass = {} kg",
             self.cr, self.cd, self.mass_kg
         )
     }
@@ -216,9 +216,9 @@ mod ut_sc_uncertainty {
             for j in 0..6 {
                 if i == j {
                     if i < 3 {
-                        assert!(estimate.covar[(i, j)] - 0.5_f64.powi(2) < f64::EPSILON);
+                        assert!((estimate.covar[(i, j)] - 0.5_f64.powi(2)).abs() < f64::EPSILON);
                     } else {
-                        assert!(estimate.covar[(i, j)] - 0.5e-3_f64.powi(2) < f64::EPSILON);
+                        assert!((estimate.covar[(i, j)] - 0.5e-3_f64.powi(2)).abs() < f64::EPSILON);
                     }
                 } else {
                     assert_eq!(estimate.covar[(i, j)], 0.0);
@@ -278,7 +278,7 @@ mod ut_sc_uncertainty {
         let orbit_cov = estimate.covar.fixed_view::<6, 6>(0, 0);
 
         // Rotate back into the RIC frame
-        let ric_covar = &dcm_ric2inertial * orbit_cov * &dcm_ric2inertial.transpose();
+        let ric_covar = dcm_ric2inertial * orbit_cov * dcm_ric2inertial.transpose();
 
         println!("{:.9}", ric_covar);
         for i in 0..6 {