Merge pull request #145 from nyx-space/127-online-measurement-rejection

Add online measurement rejection

.github/workflows/tests.yaml

@@ -137,6 +137,7 @@ jobs:
           cargo test --lib
           cargo test stop_cond_nrho_apo
           cargo test od_robust
+          cargo test od_resid_reject_
           grcov . --binary-path ./target/debug/ -t lcov -s . > lcov.txt
 
       - name: Upload coverage report

Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "nyx-space"
 build = "build.rs"
-version = "2.0.0-alpha.2"
+version = "2.0.0-alpha.3"
 edition = "2021"
 authors = ["Christopher Rabotin <christopher.rabotin@gmail.com>"]
 description = "A high-fidelity space mission toolkit, with orbit propagation, estimation and some systems engineering"
@@ -48,10 +48,10 @@ pyo3 = {version = "0.17.3", optional = true, features = ["extension-module"]}
 pyo3-log = {version = "0.8.1", optional = true}
 numpy = {version = "0.17", optional = true}
 indicatif = {version = "0.17", features = ["rayon"]}
-rstats = "1.2.49"
+rstats = "1.2.50"
 thiserror = "1.0"
-parquet = "38.0.0"
-arrow = "38.0.0"
+parquet = "39.0.0"
+arrow = "39.0.0"
 shadow-rs = "0.21.0"
 serde_yaml = "0.9.21"
 whoami = "1.3.0"

src/od/kalman.rs

@@ -319,18 +319,21 @@ where
         nominal_state: T,
         real_obs: &OVector<f64, M>,
         computed_obs: &OVector<f64, M>,
+        resid_ratio_check: Option<f64>,
     ) -> Result<(Self::Estimate, Residual<M>), NyxError> {
         if !self.h_tilde_updated {
             return Err(NyxError::SensitivityNotUpdated);
         }
 
         let stm = nominal_state.stm()?;
 
+        let epoch = nominal_state.epoch();
+
         let mut covar_bar = stm * self.prev_estimate.covar * stm.transpose();
         let mut snc_used = false;
         // Try to apply an SNC, if applicable
         for (i, snc) in self.process_noise.iter().enumerate().rev() {
-            if let Some(snc_matrix) = snc.to_matrix(nominal_state.epoch()) {
+            if let Some(snc_matrix) = snc.to_matrix(epoch) {
                 // Check if we're using another SNC than the one before
                 if self.prev_used_snc != i {
                     info!("Switched to {}-th {}", i, snc);
@@ -340,7 +343,7 @@ where
                 // Let's compute the Gamma matrix, an approximation of the time integral
                 // which assumes that the acceleration is constant between these two measurements.
                 let mut gamma = OMatrix::<f64, <T as State>::Size, A>::zeros();
-                let delta_t = (nominal_state.epoch() - self.prev_estimate.epoch()).to_seconds();
+                let delta_t = (epoch - self.prev_estimate.epoch()).to_seconds();
                 for blk in 0..A::dim() / 3 {
                     for i in 0..3 {
                         let idx_i = i + A::dim() * blk;
@@ -373,7 +376,7 @@ where
         }
 
         if !snc_used {
-            debug!("@{} No SNC", nominal_state.epoch());
+            debug!("@{} No SNC", epoch);
         }
 
         let h_tilde_t = &self.h_tilde.transpose();
@@ -383,7 +386,19 @@ where
         let prefit = real_obs - computed_obs;
 
         // Compute the prefit ratio
-        let ratio = prefit.transpose() * &h_p_ht * &prefit;
+        let ratio_mat = prefit.transpose() * &h_p_ht * &prefit;
+        let ratio = ratio_mat[0];
+
+        if let Some(ratio_thresh) = resid_ratio_check {
+            if ratio > ratio_thresh {
+                warn!("{epoch} msr rejected: residual ratio {ratio} > {ratio_thresh}");
+                // Perform only a time update and return
+                let pred_est = self.time_update(nominal_state)?;
+                return Ok((pred_est, Residual::rejected(epoch, prefit, ratio)));
+            } else {
+                debug!("{epoch} msr accepted: residual ratio {ratio} < {ratio_thresh}");
+            }
+        }
 
         // Compute the Kalman gain but first adding the measurement noise to H⋅P⋅H^T
         let mut invertible_part = h_p_ht + &self.measurement_noise;
@@ -397,17 +412,14 @@ where
         let (state_hat, res) = if self.ekf {
             let state_hat = &gain * &prefit;
             let postfit = &prefit - (&self.h_tilde * state_hat);
-            (
-                state_hat,
-                Residual::new(nominal_state.epoch(), prefit, postfit, ratio[0]),
-            )
+            (state_hat, Residual::new(epoch, prefit, postfit, ratio))
         } else {
             // Must do a time update first
             let state_bar = stm * self.prev_estimate.state_deviation;
             let postfit = &prefit - (&self.h_tilde * state_bar);
             (
                 state_bar + &gain * &postfit,
-                Residual::new(nominal_state.epoch(), prefit, postfit, ratio[0]),
+                Residual::new(epoch, prefit, postfit, ratio),
             )
         };
 

src/od/mod.rs

@@ -108,12 +108,24 @@ where
 
     /// Computes the measurement update with a provided real observation and computed observation.
     ///
-    ///Returns an error if the STM or sensitivity matrices were not updated.
+    /// The nominal state is the state used for the computed observation.
+    /// The real observation is the observation that was actually measured.
+    /// The computed observation is the observation that was computed from the nominal state.
+    ///
+    /// Returns the updated estimate and the residual. The residual may be zero if the residual ratio check prevented the ingestion of this measurement.
+    ///
+    /// # Arguments
+    ///
+    /// * `nominal_state`: the nominal state at which the observation was computed.
+    /// * `real_obs`: the real observation that was measured.
+    /// * `computed_obs`: the computed observation from the nominal state.
+    /// * `resid_ratio_check`: the ratio below which the measurement is considered to be valid.
     fn measurement_update(
         &mut self,
         nominal_state: T,
         real_obs: &OVector<f64, M>,
         computed_obs: &OVector<f64, M>,
+        resid_ratio_check: Option<f64>,
     ) -> Result<(Self::Estimate, residual::Residual<M>), NyxError>;
 
     /// Returns whether the filter is an extended filter (e.g. EKF)

src/od/msr/mod.rs

@@ -21,6 +21,7 @@ mod range;
 mod range_doppler;
 mod rangerate;
 
+pub use arc::TrackingArc;
 pub use range::RangeMsr;
 pub use range_doppler::RangeDoppler;
 pub use rangerate::RangeRate;

src/od/process/mod.rs

@@ -35,12 +35,14 @@ mod conf;
 pub use conf::{IterationConf, SmoothingArc};
 mod trigger;
 pub use trigger::{CkfTrigger, EkfTrigger, KfTrigger};
+mod rejectcrit;
 
 use std::collections::HashMap;
 use std::marker::PhantomData;
 use std::ops::Add;
 
 use self::msr::arc::TrackingArc;
+pub use self::rejectcrit::FltResid;
 
 /// An orbit determination process. Note that everything passed to this structure is moved.
 #[allow(clippy::upper_case_acronyms)]
@@ -85,6 +87,8 @@ pub struct ODProcess<
     /// Vector of residuals available after a pass
     pub residuals: Vec<Residual<Msr::MeasurementSize>>,
     pub ekf_trigger: T,
+    /// Residual rejection criteria allows preventing bad measurements from affecting the estimation.
+    pub resid_crit: Option<FltResid>,
     pub cosm: Arc<Cosm>,
     init_state: D::StateType,
     _marker: PhantomData<A>,
@@ -126,14 +130,21 @@ where
         + Allocator<f64, <S as State>::Size, A>
         + Allocator<f64, A, <S as State>::Size>,
 {
-    pub fn ekf(prop: PropInstance<'a, D, E>, kf: K, trigger: T, cosm: Arc<Cosm>) -> Self {
+    pub fn ekf(
+        prop: PropInstance<'a, D, E>,
+        kf: K,
+        trigger: T,
+        resid_crit: Option<FltResid>,
+        cosm: Arc<Cosm>,
+    ) -> Self {
         let init_state = prop.state;
         Self {
             prop,
             kf,
             estimates: Vec::with_capacity(10_000),
             residuals: Vec::with_capacity(10_000),
             ekf_trigger: trigger,
+            resid_crit,
             cosm,
             init_state,
             _marker: PhantomData::<A>,
@@ -440,6 +451,8 @@ where
         // We'll build a trajectory of the estimated states. This will be used to compute the measurements.
         let mut traj = Traj::new();
 
+        let mut msr_accepted_cnt = 0;
+
         for (msr_cnt, (device_name, msr)) in measurements.iter().enumerate() {
             let next_msr_epoch = msr.epoch();
 
@@ -502,13 +515,30 @@ where
 
                                 self.kf.update_h_tilde(h_tilde);
 
+                                let resid_ratio_check = match self.resid_crit {
+                                    None => None,
+                                    Some(flt) => {
+                                        if msr_accepted_cnt < flt.min_accepted {
+                                            None
+                                        } else {
+                                            Some(flt.num_sigmas)
+                                        }
+                                    }
+                                };
+
                                 match self.kf.measurement_update(
                                     nominal_state,
                                     &msr.observation(),
                                     &computed_meas.observation(),
+                                    resid_ratio_check,
                                 ) {
                                     Ok((estimate, residual)) => {
-                                        debug!("msr update #{msr_cnt} @ {epoch}");
+                                        debug!("processed msr #{msr_cnt} @ {epoch}");
+
+                                        if !residual.rejected {
+                                            msr_accepted_cnt += 1;
+                                        }
+
                                         // Switch to EKF if necessary, and update the dynamics and such
                                         // Note: we call enable_ekf first to ensure that the trigger gets
                                         // called in case it needs to save some information (e.g. the
@@ -546,8 +576,8 @@ where
                     let msr_prct = (10.0 * (msr_cnt as f64) / (num_msrs as f64)) as usize;
                     if !reported[msr_prct] {
                         info!(
-                            "{:>3}% done ({msr_cnt:.0} measurements processed)",
-                            10 * msr_prct,
+                            "{:>3}% done ({msr_accepted_cnt:.0} measurements accepted, {:.0} rejected)",
+                            10 * msr_prct, msr_cnt - (msr_accepted_cnt - 1)
                         );
                         reported[msr_prct] = true;
                     }
@@ -571,7 +601,10 @@ where
 
         // Always report the 100% mark
         if !reported[10] {
-            info!("100% done ({num_msrs:.0} measurements processed)");
+            info!(
+                "100% done ({msr_accepted_cnt:.0} measurements accepted, {:.0} rejected)",
+                num_msrs - msr_accepted_cnt
+            );
         }
 
         Ok(())
@@ -672,16 +705,22 @@ where
         + Allocator<f64, <S as State>::Size, A>
         + Allocator<f64, A, <S as State>::Size>,
 {
-    pub fn ckf(prop: PropInstance<'a, D, E>, kf: K, cosm: Arc<Cosm>) -> Self {
+    pub fn ckf(
+        prop: PropInstance<'a, D, E>,
+        kf: K,
+        resid_crit: Option<FltResid>,
+        cosm: Arc<Cosm>,
+    ) -> Self {
         let init_state = prop.state;
         Self {
             prop,
             kf,
             estimates: Vec::with_capacity(10_000),
             residuals: Vec::with_capacity(10_000),
-            cosm,
+            resid_crit,
             ekf_trigger: CkfTrigger {},
             init_state,
+            cosm,
             _marker: PhantomData::<A>,
         }
     }

src/od/process/rejectcrit.rs

@@ -0,0 +1,94 @@
+/*
+    Nyx, blazing fast astrodynamics
+    Copyright (C) 2023 Christopher Rabotin <christopher.rabotin@gmail.com>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Affero General Public License as published
+    by the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+use crate::cosmic::Cosm;
+use crate::io::{ConfigError, ConfigRepr, Configurable};
+#[cfg(feature = "python")]
+use pyo3::prelude::*;
+use serde_derive::{Deserialize, Serialize};
+use std::sync::Arc;
+
+/// Reject measurements with a residual ratio greater than the provided value.
+#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
+#[cfg_attr(feature = "python", pyclass)]
+pub struct FltResid {
+    /// Minimum number of accepted measurements before applying the rejection criteria.
+    pub min_accepted: usize,
+    /// Number of sigmas for a measurement to be considered an outlier.
+    pub num_sigmas: f64,
+}
+
+#[cfg(feature = "python")]
+#[pymethods]
+impl FltResid {
+    #[getter]
+    fn get_min_accepted(&self) -> usize {
+        self.min_accepted
+    }
+
+    #[setter(orbit)]
+    fn py_set_min_accepted(&mut self, min_accepted: usize) -> PyResult<()> {
+        self.min_accepted = min_accepted;
+        Ok(())
+    }
+
+    #[getter]
+    fn get_num_sigmas(&self) -> f64 {
+        self.num_sigmas
+    }
+
+    #[setter(orbit)]
+    fn py_set_num_sigmas(&mut self, num_sigmas: f64) -> PyResult<()> {
+        self.num_sigmas = num_sigmas;
+        Ok(())
+    }
+
+    fn __repr__(&self) -> String {
+        format!("{self:?}")
+    }
+
+    fn __str__(&self) -> String {
+        format!("{self:?}")
+    }
+}
+
+impl Default for FltResid {
+    fn default() -> Self {
+        Self {
+            min_accepted: 10,
+            num_sigmas: 3.0,
+        }
+    }
+}
+
+impl ConfigRepr for FltResid {}
+
+impl Configurable for FltResid {
+    type IntermediateRepr = Self;
+
+    fn from_config(cfg: Self, _cosm: Arc<Cosm>) -> Result<Self, ConfigError>
+    where
+        Self: Sized,
+    {
+        Ok(cfg)
+    }
+
+    fn to_config(&self) -> Result<Self::IntermediateRepr, ConfigError> {
+        Ok(*self)
+    }
+}