Update to ANISE 0.5.3

Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "nyx-space"
 build = "build.rs"
-version = "2.0.1"
+version = "2.0.2"
 edition = "2021"
 authors = ["Christopher Rabotin <[email protected]>"]
 description = "A high-fidelity space mission toolkit, with orbit propagation, estimation and some systems engineering"
@@ -38,7 +38,7 @@ github = { repository = "nyx-space/nyx", branch = "master" }
 nalgebra = "0.33"
 log = "0.4"
 hifitime = "4.0.0"
-anise = "0.5.2"
+anise = "0.5.3"
 flate2 = { version = "1.0", features = [
     "rust_backend",
 ], default-features = false }

src/md/trajectory/traj.rs

@@ -463,8 +463,8 @@ where
 
         // Build an array of all the RIC differences
         let mut ric_diff = Vec::with_capacity(other_states.len());
-        for (ii, other_state) in other_states.iter().enumerate() {
-            let self_orbit = self_states[ii].orbit();
+        for (other_state, self_state) in other_states.iter().zip(self_states.iter()) {
+            let self_orbit = self_state.orbit();
             let other_orbit = other_state.orbit();
 
             let this_ric_diff = self_orbit.ric_difference(&other_orbit).map_err(Box::new)?;
@@ -493,11 +493,13 @@ where
         // Add all of the fields
         for field in fields {
             let mut data = Float64Builder::new();
-            for (ii, self_state) in self_states.iter().enumerate() {
-                let self_val = self_state.value(field).unwrap();
-                let other_val = other_states[ii].value(field).unwrap();
+            for (other_state, self_state) in other_states.iter().zip(self_states.iter()) {
+                let self_val = self_state.value(field).map_err(Box::new)?;
+                let other_val = other_state.value(field).map_err(Box::new)?;
+
                 data.append_value(self_val - other_val);
             }
+
             record.push(Arc::new(data.finish()));
         }
 

src/od/filter/kalman.rs

@@ -326,7 +326,7 @@ where
                 let pred_est = self.time_update(nominal_state)?;
                 return Ok((
                     pred_est,
-                    Residual::rejected(epoch, prefit, ratio, r_k_chol.diagonal()),
+                    Residual::rejected(epoch, prefit, ratio, s_k.diagonal()),
                 ));
             }
         }

src/od/ground_station/trk_device.rs

@@ -48,13 +48,22 @@ impl TrackingDevice<Spacecraft> for GroundStation {
     ) -> Result<Option<Measurement>, ODError> {
         match self.integration_time {
             Some(integration_time) => {
+                // TODO: This should support measurement alignment
                 // If out of traj bounds, return None, else the whole strand is rejected.
-                let rx_0 = match traj.at(epoch - integration_time) {
+                let rx_0 = match traj.at(epoch - integration_time).context(ODTrajSnafu {
+                    details: format!(
+                        "fetching state {epoch} at start of ground station integration time {integration_time}"
+                    ),
+                }) {
                     Ok(rx) => rx,
                     Err(_) => return Ok(None),
                 };
 
-                let rx_1 = match traj.at(epoch).context(ODTrajSnafu) {
+                let rx_1 = match traj.at(epoch).context(ODTrajSnafu {
+                    details: format!(
+                        "fetching state {epoch} at end of ground station integration time"
+                    ),
+                }) {
                     Ok(rx) => rx,
                     Err(_) => return Ok(None),
                 };
@@ -106,7 +115,13 @@ impl TrackingDevice<Spacecraft> for GroundStation {
 
                 Ok(Some(msr))
             }
-            None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac),
+            None => self.measure_instantaneous(
+                traj.at(epoch).context(ODTrajSnafu {
+                    details: "fetching state for instantaneous measurement".to_string(),
+                })?,
+                rng,
+                almanac,
+            ),
         }
     }
 

src/od/mod.rs

@@ -18,6 +18,7 @@
 
 use crate::dynamics::DynamicsError;
 pub use crate::dynamics::{Dynamics, NyxError};
+use crate::errors::StateError;
 use crate::io::{ConfigError, InputOutputError};
 use crate::linalg::OVector;
 use crate::md::trajectory::TrajError;
@@ -120,8 +121,8 @@ pub enum ODError {
     NoiseNotConfigured { kind: String },
     #[snafu(display("measurement sim error: {details}"))]
     MeasurementSimError { details: String },
-    #[snafu(display("during an OD encountered {source}"))]
-    ODTrajError { source: TrajError },
+    #[snafu(display("during an OD encountered {source}: {details}"))]
+    ODTrajError { source: TrajError, details: String },
     #[snafu(display("OD failed because {source}"))]
     ODConfigError { source: ConfigError },
     #[snafu(display("OD failed because of an I/O error: {source}"))]
@@ -140,4 +141,10 @@ pub enum ODError {
     },
     #[snafu(display("not enough residuals to {action}"))]
     ODNoResiduals { action: &'static str },
+    #[snafu(display("Could not {action} OD results: {source}"))]
+    ODStateError {
+        #[snafu(source(from(StateError, Box::new)))]
+        source: Box<StateError>,
+        action: &'static str,
+    },
 }

src/od/msr/trackingdata/io_ccsds_tdm.rs

@@ -223,6 +223,7 @@ impl TrackingDataArc {
         freq_types.insert(MeasurementType::ReceiveFrequency);
         freq_types.insert(MeasurementType::TransmitFrequency);
         let mut latest_transmit_freq = None;
+        let mut malformed_warning = 0;
         for (epoch, measurement) in measurements.iter_mut() {
             if drop_freq_data {
                 for freq in &freq_types {
@@ -248,10 +249,13 @@ impl TrackingDataArc {
                 // if the receive frequency and the transmit frequency was previously set.
                 if latest_transmit_freq.is_some() && avail[0] {
                     use_prev_transmit_freq = true;
-                    warn!(
-                        "no transmit frequency at {epoch}, using previous value of {} Hz",
-                        latest_transmit_freq.unwrap()
-                    );
+                    if malformed_warning == 0 {
+                        warn!(
+                            "no transmit frequency at {epoch}, using previous value of {} Hz",
+                            latest_transmit_freq.unwrap()
+                        );
+                    }
+                    malformed_warning += 1;
                 } else {
                     warn!("only one of receive or transmit frequencies found at {epoch}, ignoring");
                     for freq in &freq_types {
@@ -294,12 +298,21 @@ impl TrackingDataArc {
                 .insert(MeasurementType::Doppler, rho_dot_km_s);
         }
 
+        if malformed_warning > 1 {
+            warn!("missing transmit frequency warning occured {malformed_warning} times",);
+        }
+
         let moduli = if let Some(range_modulus) = metadata.get("RANGE_MODULUS") {
             if let Ok(value) = range_modulus.parse::<f64>() {
-                let mut modulos = IndexMap::new();
-                modulos.insert(MeasurementType::Range, value);
-                // Only range modulus exists in TDM files.
-                Some(modulos)
+                if value > 0.0 {
+                    let mut modulos = IndexMap::new();
+                    modulos.insert(MeasurementType::Range, value);
+                    // Only range modulus exists in TDM files.
+                    Some(modulos)
+                } else {
+                    // Do not apply a modulus of zero.
+                    None
+                }
             } else {
                 warn!("could not parse RANGE_MODULUS of `{range_modulus}` as a double");
                 None
@@ -312,6 +325,7 @@ impl TrackingDataArc {
             measurements,
             source: Some(source),
             moduli,
+            force_reject: false,
         };
 
         if trk.unique_types().is_empty() {

src/od/msr/trackingdata/io_parquet.rs

@@ -215,6 +215,7 @@ impl TrackingDataArc {
             measurements,
             moduli: None,
             source: Some(path.as_ref().to_path_buf().display().to_string()),
+            force_reject: false,
         })
     }
     /// Store this tracking arc to a parquet file.

src/od/msr/trackingdata/mod.rs

@@ -18,10 +18,12 @@
 use super::{measurement::Measurement, MeasurementType};
 use core::fmt;
 use hifitime::prelude::{Duration, Epoch};
+use hifitime::Unit;
 use indexmap::{IndexMap, IndexSet};
+use std::collections::btree_map::Entry;
 use std::collections::BTreeMap;
-use std::ops::Bound::{Excluded, Included, Unbounded};
-use std::ops::RangeBounds;
+use std::ops::Bound::{self, Excluded, Included, Unbounded};
+use std::ops::{Add, AddAssign, RangeBounds};
 
 mod io_ccsds_tdm;
 mod io_parquet;
@@ -74,11 +76,17 @@ pub struct TrackingDataArc {
     pub source: Option<String>,
     /// Optionally provide a map of modulos (e.g. the RANGE_MODULO of CCSDS TDM).
     pub moduli: Option<IndexMap<MeasurementType, f64>>,
+    /// Reject all of the measurements, useful for debugging passes.
+    pub force_reject: bool,
 }
 
 impl TrackingDataArc {
     /// Set (or overwrites) the modulus of the provided measurement type.
     pub fn set_moduli(&mut self, msr_type: MeasurementType, modulus: f64) {
+        if modulus.is_nan() || modulus.abs() < f64::EPSILON {
+            warn!("cannot set modulus for {msr_type:?} to {modulus}");
+            return;
+        }
         if self.moduli.is_none() {
             self.moduli = Some(IndexMap::new());
         }
@@ -132,6 +140,14 @@ impl TrackingDataArc {
         self.measurements.last_key_value().map(|(k, _)| *k)
     }
 
+    /// Returns the duration this tracking arc
+    pub fn duration(&self) -> Option<Duration> {
+        match self.start_epoch() {
+            Some(start) => self.end_epoch().map(|end| end - start),
+            None => None,
+        }
+    }
+
     /// Returns the number of measurements in this data arc
     pub fn len(&self) -> usize {
         self.measurements.len()
@@ -142,8 +158,7 @@ impl TrackingDataArc {
         self.measurements.is_empty()
     }
 
-    /// Returns the minimum duration between two subsequent measurements.
-    /// This is important to correctly set up the propagator and not miss any measurement.
+    /// Returns the minimum duration between two subsequent measurements, flooring at 10 seconds.
     pub fn min_duration_sep(&self) -> Option<Duration> {
         if self.is_empty() {
             None
@@ -155,7 +170,7 @@ impl TrackingDataArc {
                 min_sep = min_sep.min(this_sep);
                 prev_epoch = *epoch;
             }
-            Some(min_sep)
+            Some(min_sep.max(Unit::Second * 10))
         }
     }
 
@@ -204,6 +219,52 @@ impl TrackingDataArc {
         self
     }
 
+    /// Returns a new tracking arc that contains measurements from all trackers except the one provided
+    pub fn exclude_tracker(mut self, excluded_tracker: String) -> Self {
+        info!("Excluding tracker {excluded_tracker}");
+
+        self.measurements = self
+            .measurements
+            .iter()
+            .filter_map(|(epoch, msr)| {
+                if msr.tracker != excluded_tracker {
+                    Some((*epoch, msr.clone()))
+                } else {
+                    None
+                }
+            })
+            .collect::<BTreeMap<Epoch, Measurement>>();
+        self
+    }
+
+    /// Returns a new tracking arc that excludes measurements within the given epoch range.
+    pub fn exclude_by_epoch<R: RangeBounds<Epoch>>(mut self, bound: R) -> Self {
+        info!(
+            "Excluding measurements from {:?} to {:?}",
+            bound.start_bound(),
+            bound.end_bound()
+        );
+
+        let mut new_measurements = BTreeMap::new();
+
+        // Include entries before the excluded range
+        new_measurements.extend(
+            self.measurements
+                .range((Bound::Unbounded, bound.start_bound()))
+                .map(|(epoch, msr)| (*epoch, msr.clone())),
+        );
+
+        // Include entries after the excluded range
+        new_measurements.extend(
+            self.measurements
+                .range((bound.end_bound(), Bound::Unbounded))
+                .map(|(epoch, msr)| (*epoch, msr.clone())),
+        );
+
+        self.measurements = new_measurements;
+        self
+    }
+
     /// Downsamples the tracking data to a lower frequency using a simple moving average low-pass filter followed by decimation,
     /// returning new `TrackingDataArc` with downsampled measurements.
     ///
@@ -325,3 +386,26 @@ impl PartialEq for TrackingDataArc {
         self.measurements == other.measurements
     }
 }
+
+impl Add for TrackingDataArc {
+    type Output = Self;
+
+    fn add(mut self, rhs: Self) -> Self::Output {
+        self.force_reject = false;
+        for (epoch, msr) in rhs.measurements {
+            if let Entry::Vacant(e) = self.measurements.entry(epoch) {
+                e.insert(msr);
+            } else {
+                error!("merging tracking data with overlapping epoch is not supported");
+            }
+        }
+
+        self
+    }
+}
+
+impl AddAssign for TrackingDataArc {
+    fn add_assign(&mut self, rhs: Self) {
+        *self = self.clone() + rhs;
+    }
+}

src/od/process/export.rs

@@ -289,7 +289,11 @@ where
         for field in fields {
             let mut data = Float64Builder::new();
             for s in &estimates {
-                data.append_value(s.state().value(field).unwrap());
+                data.append_value(
+                    s.state()
+                        .value(field)
+                        .context(ODStateSnafu { action: "export" })?,
+                );
             }
             record.push(Arc::new(data.finish()));
         }