From 7ee816958061ee0c4e547ffa8b8ae7d85343830c Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Mon, 18 Nov 2024 23:05:31 -0700 Subject: [PATCH 01/38] Initial work on new tracking arc and measurement serialization. --- src/od/msr/measurement.rs | 201 ++++++++++++++++++++++++++++++++++++++ src/od/msr/mod.rs | 3 + src/od/msr/types.rs | 33 +++++++ 3 files changed, 237 insertions(+) create mode 100644 src/od/msr/measurement.rs create mode 100644 src/od/msr/types.rs diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs new file mode 100644 index 00000000..b9090b01 --- /dev/null +++ b/src/od/msr/measurement.rs @@ -0,0 +1,201 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use arrow::{ + array::{Float64Array, StringArray}, + record_batch::RecordBatchReader, +}; +use core::fmt; +use std::collections::{BTreeMap, HashMap, HashSet}; +use std::fs::File; +use std::path::Path; + +use hifitime::Epoch; +use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; +use snafu::{ensure, ResultExt}; + +use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; + +use super::MeasurementType; + +#[derive(Clone, Debug)] +pub struct Measurement { + pub epoch: Epoch, + pub data: HashMap, + pub tracker: String, +} + +pub struct TrackingArc { + measurements: BTreeMap, +} + +impl TrackingArc { + pub fn from_parquet>(path: P) -> Result { + // Read the file since we closed it earlier + let file = File::open(&path).context(StdIOSnafu { + action: "opening file for tracking arc", + })?; + let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); + + let reader = builder.build().context(ParquetSnafu { + action: "reading tracking arc", + })?; + + // Check the schema + let mut has_epoch = false; + let mut has_tracking_dev = false; + let mut range_avail = false; + let mut rate_avail = false; + for field in &reader.schema().fields { + match field.name().as_str() { + "Epoch (UTC)" => has_epoch = true, + "Tracking device" => has_tracking_dev = true, + "Range (km)" => range_avail = true, + "Doppler (km/s)" => rate_avail = true, + _ => {} + } + } + + ensure!( + has_epoch, + MissingDataSnafu { + which: "Epoch (UTC)" + } + ); + + ensure!( + has_tracking_dev, + MissingDataSnafu { + which: "Tracking device" + } + ); + + ensure!( + range_avail || rate_avail, + MissingDataSnafu { + which: "`Range (km)` or `Doppler (km/s)`" + } + ); + + let mut measurements = BTreeMap::new(); + + // Now convert each batch on the fly + for maybe_batch in reader { + let batch = maybe_batch.context(ArrowSnafu { + action: "reading batch of tracking data", + })?; + + let tracking_device = batch + .column_by_name("Tracking device") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let epochs = batch + .column_by_name("Epoch (UTC)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let range_data = batch + .column_by_name("Range (km)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let doppler_data = batch + .column_by_name("Doppler (km/s)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + // Set the measurements in the tracking arc + for i in 0..batch.num_rows() { + let epoch = Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?; + + let mut measurement = Measurement { + epoch, + tracker: tracking_device.value(i).to_string(), + data: HashMap::new(), + }; + + if range_avail { + measurement + .data + .insert(MeasurementType::Range, range_data.value(i)); + } + + if rate_avail { + measurement + .data + .insert(MeasurementType::Doppler, doppler_data.value(i)); + } + + measurements.insert(epoch, measurement); + } + } + + Ok(Self { measurements }) + } + + /// Compute the list of unique aliases in this tracking arc + pub fn unique_aliases(&self) -> HashSet { + let mut aliases = HashSet::new(); + for msr in self.measurements.values() { + aliases.insert(msr.tracker.clone()); + } + aliases + } +} + +impl fmt::Display for TrackingArc { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + if self.measurements.is_empty() { + write!(f, "Empty tracking arc") + } else { + let (start, _) = self.measurements.first_key_value().unwrap(); + let (end, _) = self.measurements.last_key_value().unwrap(); + write!( + f, + "Tracking arc with {} measurements over {} (from {start} to {end}) with trackers {:?}", + self.measurements.len(), + *end - *start, + self.unique_aliases() + ) + } + } +} + +#[cfg(test)] +mod ut_tracker { + use super::TrackingArc; + + #[test] + fn test_lro_data() { + let trk = TrackingArc::from_parquet("04_lro_simulated_tracking.parquet").unwrap(); + println!("{trk}"); + } +} diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 8b216d60..ec048713 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -17,11 +17,14 @@ */ mod arc; +mod measurement; mod range; mod range_doppler; mod rangerate; +mod types; pub use arc::TrackingArc; pub use range::RangeMsr; pub use range_doppler::RangeDoppler; pub use rangerate::RangeRate; +pub use types::MeasurementType; diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs new file mode 100644 index 00000000..0298cd5a --- /dev/null +++ b/src/od/msr/types.rs @@ -0,0 +1,33 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] +pub enum MeasurementType { + Range, + Doppler, +} + +impl MeasurementType { + /// Returns the expected unit of this measurement type + pub fn unit(self) -> &'static str { + match self { + MeasurementType::Range => "km", + MeasurementType::Doppler => "km/s", + } + } +} From 3d60740e83c86b1e3bbd2f0dcd56b20ff1a3f905 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Wed, 20 Nov 2024 20:31:39 -0700 Subject: [PATCH 02/38] Usability improvements to new generic tracking data arc --- src/od/msr/data_arc.rs | 239 ++++++++++++++++++++++++++++++++++++++ src/od/msr/measurement.rs | 183 ++--------------------------- src/od/msr/mod.rs | 1 + 3 files changed, 247 insertions(+), 176 deletions(-) create mode 100644 src/od/msr/data_arc.rs diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs new file mode 100644 index 00000000..b099d798 --- /dev/null +++ b/src/od/msr/data_arc.rs @@ -0,0 +1,239 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ +use arrow::{ + array::{Float64Array, PrimitiveArray, StringArray}, + datatypes, + record_batch::RecordBatchReader, +}; +use core::fmt; +use std::collections::{BTreeMap, HashMap, HashSet}; +use std::fs::File; +use std::path::Path; + +use hifitime::Epoch; +use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; +use snafu::{ensure, ResultExt}; + +use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; + +use super::{measurement::Measurement, MeasurementType}; + +/// Tracking data storing all of measurements as a B-Tree. +pub struct TrackingDataArc { + /// All measurements in this data arc + pub measurements: BTreeMap, + /// Source file if loaded from a file + pub source: Option, +} + +impl TrackingDataArc { + /// Loads a tracking arc from its serialization in parquet. + pub fn from_parquet + fmt::Display>(path: P) -> Result { + // Read the file since we closed it earlier + let file = File::open(&path).context(StdIOSnafu { + action: "opening file for tracking arc", + })?; + let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); + + let reader = builder.build().context(ParquetSnafu { + action: "reading tracking arc", + })?; + + // Check the schema + let mut has_epoch = false; + let mut has_tracking_dev = false; + let mut range_avail = false; + let mut rate_avail = false; + for field in &reader.schema().fields { + match field.name().as_str() { + "Epoch (UTC)" => has_epoch = true, + "Tracking device" => has_tracking_dev = true, + "Range (km)" => range_avail = true, + "Doppler (km/s)" => rate_avail = true, + _ => {} + } + } + + ensure!( + has_epoch, + MissingDataSnafu { + which: "Epoch (UTC)" + } + ); + + ensure!( + has_tracking_dev, + MissingDataSnafu { + which: "Tracking device" + } + ); + + ensure!( + range_avail || rate_avail, + MissingDataSnafu { + which: "`Range (km)` or `Doppler (km/s)`" + } + ); + + let mut measurements = BTreeMap::new(); + + // Now convert each batch on the fly + for maybe_batch in reader { + let batch = maybe_batch.context(ArrowSnafu { + action: "reading batch of tracking data", + })?; + + let tracking_device = batch + .column_by_name("Tracking device") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let epochs = batch + .column_by_name("Epoch (UTC)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let range_data: Option<&PrimitiveArray> = if range_avail { + Some( + batch + .column_by_name("Range (km)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + let doppler_data: Option<&PrimitiveArray> = if rate_avail { + Some( + batch + .column_by_name("Doppler (km/s)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + // Set the measurements in the tracking arc + for i in 0..batch.num_rows() { + let epoch = Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?; + + let mut measurement = Measurement { + epoch, + tracker: tracking_device.value(i).to_string(), + data: HashMap::new(), + }; + + if range_avail { + measurement + .data + .insert(MeasurementType::Range, range_data.unwrap().value(i)); + } + + if rate_avail { + measurement + .data + .insert(MeasurementType::Doppler, doppler_data.unwrap().value(i)); + } + + measurements.insert(epoch, measurement); + } + } + + Ok(Self { + measurements, + source: Some(path.to_string()), + }) + } + + /// Compute the list of unique aliases in this tracking arc + pub fn unique_aliases(&self) -> HashSet { + let mut aliases = HashSet::new(); + for msr in self.measurements.values() { + aliases.insert(msr.tracker.clone()); + } + aliases + } + + /// Returns the start epoch of this tracking arc + pub fn start_epoch(&self) -> Option { + self.measurements.first_key_value().map(|(k, _)| *k) + } + + /// Returns the end epoch of this tracking arc + pub fn end_epoch(&self) -> Option { + self.measurements.last_key_value().map(|(k, _)| *k) + } + + /// Returns the number of measurements in this data arc + pub fn len(&self) -> usize { + self.measurements.len() + } + + /// Returns whether this arc has no measurements. + pub fn is_empty(&self) -> bool { + self.measurements.is_empty() + } +} + +impl fmt::Display for TrackingDataArc { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + if self.is_empty() { + write!(f, "Empty tracking arc") + } else { + let start = self.start_epoch().unwrap(); + let end = self.end_epoch().unwrap(); + let src = match &self.source { + Some(src) => format!(" (source: {src})"), + None => String::new(), + }; + write!( + f, + "Tracking arc with {} measurements over {} (from {start} to {end}) with trackers {:?}{src}", + self.len(), + end - start, + self.unique_aliases() + ) + } + } +} + +#[cfg(test)] +mod ut_tracker { + use super::TrackingDataArc; + + #[test] + fn test_lro_data() { + let trk = TrackingDataArc::from_parquet("04_lro_simulated_tracking.parquet").unwrap(); + println!("{trk}"); + // TODO: Add test nulls in specific sets, and missing columns. + } +} diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs index b9090b01..47833991 100644 --- a/src/od/msr/measurement.rs +++ b/src/od/msr/measurement.rs @@ -16,186 +16,17 @@ along with this program. If not, see . */ -use arrow::{ - array::{Float64Array, StringArray}, - record_batch::RecordBatchReader, -}; -use core::fmt; -use std::collections::{BTreeMap, HashMap, HashSet}; -use std::fs::File; -use std::path::Path; - -use hifitime::Epoch; -use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; -use snafu::{ensure, ResultExt}; - -use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; - use super::MeasurementType; +use hifitime::Epoch; +use std::collections::HashMap; +/// A type-agnostic simultaneous measurement storage structure. Allows storing any number of simultaneous measurement of a given taker. #[derive(Clone, Debug)] pub struct Measurement { + /// Tracker alias which made this measurement + pub tracker: String, + /// Epoch of the measurement pub epoch: Epoch, + /// All measurements made simultaneously pub data: HashMap, - pub tracker: String, -} - -pub struct TrackingArc { - measurements: BTreeMap, -} - -impl TrackingArc { - pub fn from_parquet>(path: P) -> Result { - // Read the file since we closed it earlier - let file = File::open(&path).context(StdIOSnafu { - action: "opening file for tracking arc", - })?; - let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); - - let reader = builder.build().context(ParquetSnafu { - action: "reading tracking arc", - })?; - - // Check the schema - let mut has_epoch = false; - let mut has_tracking_dev = false; - let mut range_avail = false; - let mut rate_avail = false; - for field in &reader.schema().fields { - match field.name().as_str() { - "Epoch (UTC)" => has_epoch = true, - "Tracking device" => has_tracking_dev = true, - "Range (km)" => range_avail = true, - "Doppler (km/s)" => rate_avail = true, - _ => {} - } - } - - ensure!( - has_epoch, - MissingDataSnafu { - which: "Epoch (UTC)" - } - ); - - ensure!( - has_tracking_dev, - MissingDataSnafu { - which: "Tracking device" - } - ); - - ensure!( - range_avail || rate_avail, - MissingDataSnafu { - which: "`Range (km)` or `Doppler (km/s)`" - } - ); - - let mut measurements = BTreeMap::new(); - - // Now convert each batch on the fly - for maybe_batch in reader { - let batch = maybe_batch.context(ArrowSnafu { - action: "reading batch of tracking data", - })?; - - let tracking_device = batch - .column_by_name("Tracking device") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let epochs = batch - .column_by_name("Epoch (UTC)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let range_data = batch - .column_by_name("Range (km)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let doppler_data = batch - .column_by_name("Doppler (km/s)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - // Set the measurements in the tracking arc - for i in 0..batch.num_rows() { - let epoch = Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { - InputOutputError::Inconsistency { - msg: format!("{e} when parsing epoch"), - } - })?; - - let mut measurement = Measurement { - epoch, - tracker: tracking_device.value(i).to_string(), - data: HashMap::new(), - }; - - if range_avail { - measurement - .data - .insert(MeasurementType::Range, range_data.value(i)); - } - - if rate_avail { - measurement - .data - .insert(MeasurementType::Doppler, doppler_data.value(i)); - } - - measurements.insert(epoch, measurement); - } - } - - Ok(Self { measurements }) - } - - /// Compute the list of unique aliases in this tracking arc - pub fn unique_aliases(&self) -> HashSet { - let mut aliases = HashSet::new(); - for msr in self.measurements.values() { - aliases.insert(msr.tracker.clone()); - } - aliases - } -} - -impl fmt::Display for TrackingArc { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - if self.measurements.is_empty() { - write!(f, "Empty tracking arc") - } else { - let (start, _) = self.measurements.first_key_value().unwrap(); - let (end, _) = self.measurements.last_key_value().unwrap(); - write!( - f, - "Tracking arc with {} measurements over {} (from {start} to {end}) with trackers {:?}", - self.measurements.len(), - *end - *start, - self.unique_aliases() - ) - } - } -} - -#[cfg(test)] -mod ut_tracker { - use super::TrackingArc; - - #[test] - fn test_lro_data() { - let trk = TrackingArc::from_parquet("04_lro_simulated_tracking.parquet").unwrap(); - println!("{trk}"); - } } diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index ec048713..4ec4726c 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -17,6 +17,7 @@ */ mod arc; +mod data_arc; mod measurement; mod range; mod range_doppler; From 105a0cb37bf7a06fbbd9cc055fd0d8be265d8898 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Wed, 20 Nov 2024 22:10:37 -0700 Subject: [PATCH 03/38] Computation of sensitivity in new framework --- src/od/msr/data_arc.rs | 19 +++++- src/od/msr/measurement.rs | 32 +++++++++- src/od/msr/mod.rs | 1 + src/od/msr/sensitivity.rs | 129 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 179 insertions(+), 2 deletions(-) create mode 100644 src/od/msr/sensitivity.rs diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index b099d798..c4da5a3b 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -25,7 +25,7 @@ use std::collections::{BTreeMap, HashMap, HashSet}; use std::fs::File; use std::path::Path; -use hifitime::Epoch; +use hifitime::{Duration, Epoch}; use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; use snafu::{ensure, ResultExt}; @@ -202,6 +202,23 @@ impl TrackingDataArc { pub fn is_empty(&self) -> bool { 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. + pub fn min_duration_sep(&self) -> Option { + if self.is_empty() { + None + } else { + let mut min_sep = Duration::MAX; + let mut prev_epoch = self.start_epoch().unwrap(); + for (epoch, _) in self.measurements.iter().skip(1) { + let this_sep = *epoch - prev_epoch; + min_sep = min_sep.min(this_sep); + prev_epoch = *epoch; + } + Some(min_sep) + } + } } impl fmt::Display for TrackingDataArc { diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs index 47833991..5f8e4607 100644 --- a/src/od/msr/measurement.rs +++ b/src/od/msr/measurement.rs @@ -18,7 +18,8 @@ use super::MeasurementType; use hifitime::Epoch; -use std::collections::HashMap; +use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OVector}; +use std::collections::{HashMap, HashSet}; /// A type-agnostic simultaneous measurement storage structure. Allows storing any number of simultaneous measurement of a given taker. #[derive(Clone, Debug)] @@ -30,3 +31,32 @@ pub struct Measurement { /// All measurements made simultaneously pub data: HashMap, } + +impl Measurement { + /// Builds an observation vector for this measurement provided a set of measurement types. + /// If the requested measurement type is not available, then that specific row is set to zero. + /// The caller must set the appropriate sensitivity matrix rows to zero. + pub fn observation(&self, types: HashSet) -> OVector + where + DefaultAllocator: Allocator, + { + let mut obs = OVector::zeros(); + for (i, t) in types.iter().enumerate() { + if let Some(msr_value) = self.data.get(t) { + obs[i] = *msr_value; + } + } + obs + } + + /// Returns a vector specifying which measurement types are available. + pub fn availability(&self, types: HashSet) -> Vec { + let mut rtn = Vec::with_capacity(types.len()); + for (i, t) in types.iter().enumerate() { + if self.data.contains_key(t) { + rtn[i] = true; + } + } + rtn + } +} diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 4ec4726c..0a00d0a5 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -22,6 +22,7 @@ mod measurement; mod range; mod range_doppler; mod rangerate; +mod sensitivity; mod types; pub use arc::TrackingArc; diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs new file mode 100644 index 00000000..02666e51 --- /dev/null +++ b/src/od/msr/sensitivity.rs @@ -0,0 +1,129 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use crate::linalg::allocator::Allocator; +use crate::linalg::{DefaultAllocator, OVector}; +use crate::od::{GroundStation, TrackingDeviceSim}; +use crate::{Spacecraft, State}; +use anise::prelude::Almanac; +use std::marker::PhantomData; +use std::sync::Arc; + +use super::measurement::Measurement; +use super::MeasurementType; + +trait Sensitivity +where + Self: Sized, + DefaultAllocator: Allocator + + Allocator + + Allocator, +{ + fn new( + msr_type: MeasurementType, + msr: &Measurement, + rx: &Rx, + tx: &Tx, + almanac: Arc, + ) -> Result; +} + +struct ScalarSensitivity +where + DefaultAllocator: Allocator + + Allocator + + Allocator, +{ + sensitivity_row: OVector, + msr_type: MeasurementType, + _rx: PhantomData, + _tx: PhantomData, +} + +impl Sensitivity + for ScalarSensitivity +{ + fn new( + msr_type: MeasurementType, + msr: &Measurement, + rx: &Spacecraft, + tx: &GroundStation, + almanac: Arc, + ) -> Result { + let receiver = rx.orbit; + // Compute the device location + let transmitter = tx + .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) + .unwrap(); + + let delta_r = receiver.radius_km - transmitter.radius_km; + let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + + match msr_type { + MeasurementType::Doppler => { + // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range. + let ρ_km = match msr.data.get(&MeasurementType::Range) { + Some(range_km) => *range_km, + None => { + tx.azimuth_elevation_of(receiver, None, &almanac) + .unwrap() + .range_km + } + }; + + let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).unwrap(); + let m11 = delta_r.x / ρ_km; + let m12 = delta_r.y / ρ_km; + let m13 = delta_r.z / ρ_km; + let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2); + let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2); + let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2); + + let sensitivity_row = + OVector::::Size>::from_row_slice(&[ + m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + msr_type, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + MeasurementType::Range => { + let ρ_km = msr.data.get(&MeasurementType::Doppler).unwrap(); + let m11 = delta_r.x / ρ_km; + let m12 = delta_r.y / ρ_km; + let m13 = delta_r.z / ρ_km; + + let sensitivity_row = + OVector::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + msr_type, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + } + } +} From 9aa07c0265232e4728b86add41eac9a043972d07 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Wed, 20 Nov 2024 22:42:09 -0700 Subject: [PATCH 04/38] Time to test this new sensitivity formulation --- src/od/msr/sensitivity.rs | 64 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 62 insertions(+), 2 deletions(-) diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 02666e51..cfb0c9e8 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -21,13 +21,15 @@ use crate::linalg::{DefaultAllocator, OVector}; use crate::od::{GroundStation, TrackingDeviceSim}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; +use nalgebra::{DimName, OMatrix}; +use std::collections::HashSet; use std::marker::PhantomData; use std::sync::Arc; use super::measurement::Measurement; use super::MeasurementType; -trait Sensitivity +trait ScalarSensitivityT where Self: Sized, DefaultAllocator: Allocator @@ -43,6 +45,26 @@ where ) -> Result; } +trait Sensitivity +where + Self: Sized, + DefaultAllocator: Allocator + + Allocator + + Allocator, +{ + /// Returns the sensitivity matrix of size MxS where M is the number of simultaneous measurements + /// and S is the size of the state being solved for. + fn h_tilde( + &self, + msr_types: HashSet, // Consider switching to array + rx: &Rx, + tx: &Tx, + almanac: Arc, + ) -> Result, String> + where + DefaultAllocator: Allocator + Allocator; +} + struct ScalarSensitivity where DefaultAllocator: Allocator @@ -55,7 +77,45 @@ where _tx: PhantomData, } -impl Sensitivity +impl Sensitivity for Measurement +where + DefaultAllocator: Allocator<::Size> + + Allocator<::VecLength> + + Allocator<::Size, ::Size>, +{ + fn h_tilde( + &self, + msr_types: HashSet, + rx: &Spacecraft, + tx: &GroundStation, + almanac: Arc, + ) -> Result::Size>, String> + where + DefaultAllocator: Allocator + Allocator::Size>, + { + // Rebuild each row of the scalar sensitivities. + let mut mat = OMatrix::::Size>::zeros(); + for (ith_row, msr_type) in msr_types.iter().enumerate() { + if !self.data.contains_key(msr_type) { + // Skip computation, this row is zero anyway. + continue; + } + let scalar_h = + as ScalarSensitivityT< + Spacecraft, + Spacecraft, + GroundStation, + >>::new(*msr_type, self, rx, tx, almanac.clone())?; + + // TODO: Check that I should be taking the transpose here! + mat.set_row(ith_row, &scalar_h.sensitivity_row.transpose()); + } + // ScalarSensitivity + Ok(mat) + } +} + +impl ScalarSensitivityT for ScalarSensitivity { fn new( From c2713002054024d88eb75fb9572b731cf1447c04 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Wed, 20 Nov 2024 23:03:13 -0700 Subject: [PATCH 05/38] Superb! Sensitivity matrix computation matches previous computation --- src/od/msr/mod.rs | 4 +-- src/od/msr/sensitivity.rs | 9 +++--- tests/orbit_determination/measurements.rs | 39 +++++++++++++++++++++-- 3 files changed, 43 insertions(+), 9 deletions(-) diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 0a00d0a5..20609d94 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -18,11 +18,11 @@ mod arc; mod data_arc; -mod measurement; +pub mod measurement; mod range; mod range_doppler; mod rangerate; -mod sensitivity; +pub mod sensitivity; mod types; pub use arc::TrackingArc; diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index cfb0c9e8..4eae6518 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -22,7 +22,6 @@ use crate::od::{GroundStation, TrackingDeviceSim}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; use nalgebra::{DimName, OMatrix}; -use std::collections::HashSet; use std::marker::PhantomData; use std::sync::Arc; @@ -45,7 +44,7 @@ where ) -> Result; } -trait Sensitivity +pub trait Sensitivity where Self: Sized, DefaultAllocator: Allocator @@ -56,7 +55,7 @@ where /// and S is the size of the state being solved for. fn h_tilde( &self, - msr_types: HashSet, // Consider switching to array + msr_types: &[MeasurementType], // Consider switching to array rx: &Rx, tx: &Tx, almanac: Arc, @@ -85,7 +84,7 @@ where { fn h_tilde( &self, - msr_types: HashSet, + msr_types: &[MeasurementType], rx: &Spacecraft, tx: &GroundStation, almanac: Arc, @@ -167,7 +166,7 @@ impl ScalarSensitivityT }) } MeasurementType::Range => { - let ρ_km = msr.data.get(&MeasurementType::Doppler).unwrap(); + let ρ_km = msr.data.get(&MeasurementType::Range).unwrap(); let m11 = delta_r.x / ρ_km; let m12 = delta_r.y / ρ_km; let m13 = delta_r.z / ρ_km; diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index 0317ddd6..e2df05ec 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -3,8 +3,10 @@ extern crate nyx_space as nyx; use anise::constants::celestial_objects::{EARTH, MOON, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; +use nalgebra::U2; use nyx::cosmic::Orbit; use nyx::dynamics::SpacecraftDynamics; +use nyx::od::msr::measurement::Measurement as NewMeasurement; use nyx::od::prelude::*; use nyx::time::Epoch; use nyx::{dynamics::OrbitalDynamics, propagators::Propagator}; @@ -14,6 +16,8 @@ use rand_pcg::Pcg64Mcg; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; +use sensitivity::Sensitivity; +use std::collections::HashMap; use std::sync::Arc; #[fixture] @@ -238,12 +242,43 @@ fn val_measurements_topo(almanac: Arc) { // Now iterate the trajectory to count the measurements. for state in traj2.every(1 * Unit::Minute) { - if dss65_madrid + if let Some(msr) = dss65_madrid .measure(state.epoch(), &traj2, Some(&mut rng), almanac.clone()) .unwrap() - .is_some() { traj2_msr_cnt += 1; + let exp_h = Spacecraft::sensitivity( + &msr, + state, + dss65_madrid + .location(state.epoch(), state.orbit.frame, almanac.clone()) + .unwrap(), + ); + + // Rebuild the measurement and recompute the sensitivity. + let mut data = HashMap::new(); + data.insert(MeasurementType::Range, msr.observation()[0]); + data.insert(MeasurementType::Doppler, msr.observation()[1]); + let n_msr = NewMeasurement { + epoch: state.epoch(), + tracker: "DSS 65".to_string(), + data, + }; + let got_h = n_msr + .h_tilde::( + &[MeasurementType::Range, MeasurementType::Doppler], + &state, + &dss65_madrid, + almanac.clone(), + ) + .unwrap(); + let err = exp_h - got_h; + if err.norm() > 0.0 { + println!("ERR = {:.3e}", exp_h - got_h); + println!("EXP = {exp_h:.6}"); + println!("GOT = {got_h:.6}"); + panic!(); + } } } From 6aca68f53ad120eb1042c228945a4e151ac08aba Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Wed, 20 Nov 2024 23:27:27 -0700 Subject: [PATCH 06/38] Impl snafu for measurement type --- src/od/msr/data_arc.rs | 24 ++++++++++++++++++++---- src/od/msr/sensitivity.rs | 39 +++++++++++++++++++++------------------ 2 files changed, 41 insertions(+), 22 deletions(-) diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index c4da5a3b..08b4ec27 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -92,7 +92,7 @@ impl TrackingDataArc { let mut measurements = BTreeMap::new(); - // Now convert each batch on the fly + // We can safely unwrap the columns since we've checked for their existance just before. for maybe_batch in reader { let batch = maybe_batch.context(ArrowSnafu { action: "reading batch of tracking data", @@ -174,13 +174,27 @@ impl TrackingDataArc { }) } - /// Compute the list of unique aliases in this tracking arc + /// Returns the unique list of aliases in this tracking data arc pub fn unique_aliases(&self) -> HashSet { + self.unique().0 + } + + /// Returns the unique measurement types in this tracking data arc + pub fn unique_types(&self) -> HashSet { + self.unique().1 + } + + /// Returns the unique trackers and unique measurement types in this data arc + pub fn unique(&self) -> (HashSet, HashSet) { let mut aliases = HashSet::new(); + let mut types = HashSet::new(); for msr in self.measurements.values() { aliases.insert(msr.tracker.clone()); + for k in msr.data.keys() { + types.insert(*k); + } } - aliases + (aliases, types) } /// Returns the start epoch of this tracking arc @@ -234,8 +248,9 @@ impl fmt::Display for TrackingDataArc { }; write!( f, - "Tracking arc with {} measurements over {} (from {start} to {end}) with trackers {:?}{src}", + "Tracking arc with {} measurements of type {:?} over {} (from {start} to {end}) with trackers {:?}{src}", self.len(), + self.unique_types(), end - start, self.unique_aliases() ) @@ -251,6 +266,7 @@ mod ut_tracker { fn test_lro_data() { let trk = TrackingDataArc::from_parquet("04_lro_simulated_tracking.parquet").unwrap(); println!("{trk}"); + println!("min step = {}", trk.min_duration_sep().unwrap()); // TODO: Add test nulls in specific sets, and missing columns. } } diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 4eae6518..f5ffc3e0 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -17,11 +17,12 @@ */ use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, OVector}; -use crate::od::{GroundStation, TrackingDeviceSim}; +use crate::linalg::DefaultAllocator; +use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDeviceSim}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; -use nalgebra::{DimName, OMatrix}; +use nalgebra::{DimName, OMatrix, U1}; +use snafu::ResultExt; use std::marker::PhantomData; use std::sync::Arc; @@ -41,9 +42,10 @@ where rx: &Rx, tx: &Tx, almanac: Arc, - ) -> Result; + ) -> Result; } +/// Trait required to build a triplet of a solve-for state, a receiver, and a transmitter. pub trait Sensitivity where Self: Sized, @@ -59,7 +61,7 @@ where rx: &Rx, tx: &Tx, almanac: Arc, - ) -> Result, String> + ) -> Result, ODError> where DefaultAllocator: Allocator + Allocator; } @@ -68,10 +70,10 @@ struct ScalarSensitivity where DefaultAllocator: Allocator + Allocator - + Allocator, + + Allocator + + Allocator, { - sensitivity_row: OVector, - msr_type: MeasurementType, + sensitivity_row: OMatrix, _rx: PhantomData, _tx: PhantomData, } @@ -88,7 +90,7 @@ where rx: &Spacecraft, tx: &GroundStation, almanac: Arc, - ) -> Result::Size>, String> + ) -> Result::Size>, ODError> where DefaultAllocator: Allocator + Allocator::Size>, { @@ -106,8 +108,7 @@ where GroundStation, >>::new(*msr_type, self, rx, tx, almanac.clone())?; - // TODO: Check that I should be taking the transpose here! - mat.set_row(ith_row, &scalar_h.sensitivity_row.transpose()); + mat.set_row(ith_row, &scalar_h.sensitivity_row); } // ScalarSensitivity Ok(mat) @@ -123,12 +124,14 @@ impl ScalarSensitivityT rx: &Spacecraft, tx: &GroundStation, almanac: Arc, - ) -> Result { + ) -> Result { let receiver = rx.orbit; // Compute the device location let transmitter = tx .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) - .unwrap(); + .context(ODAlmanacSnafu { + action: "computing transmitter location when computing sensitivity matrix", + })?; let delta_r = receiver.radius_km - transmitter.radius_km; let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; @@ -140,7 +143,9 @@ impl ScalarSensitivityT Some(range_km) => *range_km, None => { tx.azimuth_elevation_of(receiver, None, &almanac) - .unwrap() + .context(ODAlmanacSnafu { + action: "computing range for Doppler measurement", + })? .range_km } }; @@ -154,13 +159,12 @@ impl ScalarSensitivityT let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2); let sensitivity_row = - OVector::::Size>::from_row_slice(&[ + OMatrix::::Size>::from_row_slice(&[ m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0, ]); Ok(Self { sensitivity_row, - msr_type, _rx: PhantomData::<_>, _tx: PhantomData::<_>, }) @@ -172,13 +176,12 @@ impl ScalarSensitivityT let m13 = delta_r.z / ρ_km; let sensitivity_row = - OVector::::Size>::from_row_slice(&[ + OMatrix::::Size>::from_row_slice(&[ m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ]); Ok(Self { sensitivity_row, - msr_type, _rx: PhantomData::<_>, _tx: PhantomData::<_>, }) From e5368864c39c3f53c343c7e5d38199ab18503186 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Thu, 21 Nov 2024 22:51:29 -0700 Subject: [PATCH 07/38] Simulated measurements now match the original version --- src/io/mod.rs | 2 + src/od/ground_station.rs | 32 +++++ src/od/msr/arc.rs | 1 + src/od/msr/data_arc.rs | 161 ++++++++++++++++++++++-- src/od/msr/measurement.rs | 19 ++- src/od/msr/mod.rs | 1 + src/od/msr/types.rs | 18 +++ src/od/simulator/arc.rs | 91 +++++++++++++- src/od/simulator/trackdata.rs | 17 +++ tests/orbit_determination/multi_body.rs | 7 ++ 10 files changed, 337 insertions(+), 12 deletions(-) diff --git a/src/io/mod.rs b/src/io/mod.rs index 0031377e..0484d198 100644 --- a/src/io/mod.rs +++ b/src/io/mod.rs @@ -204,6 +204,8 @@ pub enum InputOutputError { ParseDhall { data: String, err: String }, #[snafu(display("error serializing {what} to Dhall: {err}"))] SerializeDhall { what: String, err: String }, + #[snafu(display("empty dataset error when (de)serializing for {action}"))] + EmptyDataset { action: &'static str }, } impl PartialEq for InputOutputError { diff --git a/src/od/ground_station.rs b/src/od/ground_station.rs index 3d8f753f..9904e2af 100644 --- a/src/od/ground_station.rs +++ b/src/od/ground_station.rs @@ -20,6 +20,7 @@ use anise::astro::{Aberration, AzElRange, PhysicsResult}; use anise::errors::AlmanacResult; use anise::prelude::{Almanac, Frame, Orbit}; +use super::msr::measurement::Measurement; use super::msr::RangeDoppler; use super::noise::StochasticNoise; use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDeviceSim}; @@ -374,6 +375,37 @@ impl TrackingDeviceSim for GroundStation { Ok(msr_noises) } + + fn measurement_covar_new( + &self, + msr_type: super::prelude::MeasurementType, + epoch: Epoch, + ) -> Result { + Ok(match msr_type { + super::msr::MeasurementType::Range => self + .range_noise_km + .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? + .covariance(epoch), + super::msr::MeasurementType::Doppler => self + .doppler_noise_km_s + .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? + .covariance(epoch), + }) + } + + fn measure_new( + &mut self, + epoch: Epoch, + traj: &Traj, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError> { + Ok(self.measure(epoch, traj, rng, almanac)?.map(|msr| { + Measurement::new(self.name.clone(), epoch) + .with(super::msr::MeasurementType::Range, msr.obs[0]) + .with(super::msr::MeasurementType::Doppler, msr.obs[1]) + })) + } } impl fmt::Display for GroundStation { diff --git a/src/od/msr/arc.rs b/src/od/msr/arc.rs index 3c259431..59d86a42 100644 --- a/src/od/msr/arc.rs +++ b/src/od/msr/arc.rs @@ -32,6 +32,7 @@ use crate::md::trajectory::Interpolatable; use crate::od::prelude::TrkConfig; use crate::od::{Measurement, TrackingDeviceSim}; use crate::State; + use arrow::array::{Array, Float64Builder, StringBuilder}; use arrow::datatypes::{DataType, Field, Schema}; use arrow::record_batch::RecordBatch; diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index 08b4ec27..e2a37849 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -15,29 +15,36 @@ You should have received a copy of the GNU Affero General Public License along with this program. If not, see . */ +use super::{measurement::Measurement, MeasurementType}; +use crate::io::watermark::pq_writer; +use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; +use crate::io::{EmptyDatasetSnafu, ExportCfg}; +use arrow::array::{Array, Float64Builder, StringBuilder}; +use arrow::datatypes::{DataType, Field, Schema}; +use arrow::record_batch::RecordBatch; use arrow::{ array::{Float64Array, PrimitiveArray, StringArray}, datatypes, record_batch::RecordBatchReader, }; use core::fmt; -use std::collections::{BTreeMap, HashMap, HashSet}; -use std::fs::File; -use std::path::Path; - -use hifitime::{Duration, Epoch}; +use hifitime::prelude::{Duration, Epoch}; +use hifitime::TimeScale; use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; +use parquet::arrow::ArrowWriter; use snafu::{ensure, ResultExt}; - -use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; - -use super::{measurement::Measurement, MeasurementType}; +use std::collections::{BTreeMap, HashMap, HashSet}; +use std::error::Error; +use std::fs::File; +use std::path::{Path, PathBuf}; +use std::sync::Arc; /// Tracking data storing all of measurements as a B-Tree. +#[derive(Clone)] pub struct TrackingDataArc { /// All measurements in this data arc pub measurements: BTreeMap, - /// Source file if loaded from a file + /// Source file if loaded from a file or saved to a file. pub source: Option, } @@ -233,6 +240,128 @@ impl TrackingDataArc { Some(min_sep) } } + + /// Store this tracking arc to a parquet file. + pub fn to_parquet_simple + fmt::Display>( + &self, + path: P, + ) -> Result> { + self.to_parquet(path, ExportCfg::default()) + } + + /// Store this tracking arc to a parquet file, with optional metadata and a timestamp appended to the filename. + pub fn to_parquet + fmt::Display>( + &self, + path: P, + cfg: ExportCfg, + ) -> Result> { + ensure!( + !self.is_empty(), + EmptyDatasetSnafu { + action: "exporting tracking data arc" + } + ); + + let path_buf = cfg.actual_path(path); + + if cfg.step.is_some() { + warn!("The `step` parameter in the export is not supported for tracking arcs."); + } + + if cfg.fields.is_some() { + warn!("The `fields` parameter in the export is not supported for tracking arcs."); + } + + // Build the schema + let mut hdrs = vec![ + Field::new("Epoch (UTC)", DataType::Utf8, false), + Field::new("Tracking device", DataType::Utf8, false), + ]; + + let msr_types = self.unique_types(); + let mut msr_fields = msr_types + .iter() + .map(|msr_type| msr_type.to_field()) + .collect::>(); + + hdrs.append(&mut msr_fields); + + // Build the schema + let schema = Arc::new(Schema::new(hdrs)); + let mut record: Vec> = Vec::new(); + + // Build the measurement iterator + + let measurements = + if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { + let start = cfg + .start_epoch + .unwrap_or_else(|| self.start_epoch().unwrap()); + let end = cfg.end_epoch.unwrap_or_else(|| self.end_epoch().unwrap()); + + info!("Exporting measurements from {start} to {end}."); + + self.measurements + .range(start..end) + .map(|(k, v)| (*k, v.clone())) + .collect() + } else { + self.measurements.clone() + }; + + // Build all of the records + + // Epochs + let mut utc_epoch = StringBuilder::new(); + for epoch in measurements.keys() { + utc_epoch.append_value(epoch.to_time_scale(TimeScale::UTC).to_isoformat()); + } + record.push(Arc::new(utc_epoch.finish())); + + // Device names + let mut device_names = StringBuilder::new(); + for m in measurements.values() { + device_names.append_value(m.tracker.clone()); + } + record.push(Arc::new(device_names.finish())); + + // Measurement data, column by column + for msr_type in msr_types { + let mut data_builder = Float64Builder::new(); + + for m in measurements.values() { + match m.data.get(&msr_type) { + Some(value) => data_builder.append_value(*value), + None => data_builder.append_null(), + }; + } + record.push(Arc::new(data_builder.finish())); + } + + // Serialize all of the devices and add that to the parquet file too. + let mut metadata = HashMap::new(); + metadata.insert("Purpose".to_string(), "Tracking Arc Data".to_string()); + if let Some(add_meta) = cfg.metadata { + for (k, v) in add_meta { + metadata.insert(k, v); + } + } + + let props = pq_writer(Some(metadata)); + + let file = File::create(&path_buf)?; + + let mut writer = ArrowWriter::try_new(file, schema.clone(), props).unwrap(); + + let batch = RecordBatch::try_new(schema, record)?; + writer.write(&batch)?; + writer.close()?; + + info!("Serialized {self} to {}", path_buf.display()); + + // Return the path this was written to + Ok(path_buf) + } } impl fmt::Display for TrackingDataArc { @@ -258,6 +387,18 @@ impl fmt::Display for TrackingDataArc { } } +impl fmt::Debug for TrackingDataArc { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{self} @ {self:p}") + } +} + +impl PartialEq for TrackingDataArc { + fn eq(&self, other: &Self) -> bool { + self.measurements == other.measurements + } +} + #[cfg(test)] mod ut_tracker { use super::TrackingDataArc; diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs index 5f8e4607..b8d9cb8a 100644 --- a/src/od/msr/measurement.rs +++ b/src/od/msr/measurement.rs @@ -22,7 +22,7 @@ use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OVector}; use std::collections::{HashMap, HashSet}; /// A type-agnostic simultaneous measurement storage structure. Allows storing any number of simultaneous measurement of a given taker. -#[derive(Clone, Debug)] +#[derive(Clone, Debug, PartialEq)] pub struct Measurement { /// Tracker alias which made this measurement pub tracker: String, @@ -33,6 +33,23 @@ pub struct Measurement { } impl Measurement { + pub fn new(tracker: String, epoch: Epoch) -> Self { + Self { + tracker, + epoch, + data: HashMap::new(), + } + } + + pub fn push(&mut self, msr_type: MeasurementType, msr_value: f64) { + self.data.insert(msr_type, msr_value); + } + + pub fn with(mut self, msr_type: MeasurementType, msr_value: f64) -> Self { + self.push(msr_type, msr_value); + self + } + /// Builds an observation vector for this measurement provided a set of measurement types. /// If the requested measurement type is not available, then that specific row is set to zero. /// The caller must set the appropriate sensitivity matrix rows to zero. diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 20609d94..528dd339 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -26,6 +26,7 @@ pub mod sensitivity; mod types; pub use arc::TrackingArc; +pub use data_arc::TrackingDataArc; pub use range::RangeMsr; pub use range_doppler::RangeDoppler; pub use rangerate::RangeRate; diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index 0298cd5a..3b40212d 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -16,6 +16,10 @@ along with this program. If not, see . */ +use std::collections::HashMap; + +use arrow::datatypes::{DataType, Field}; + #[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] pub enum MeasurementType { Range, @@ -30,4 +34,18 @@ impl MeasurementType { MeasurementType::Doppler => "km/s", } } + + /// Returns the fields for this kind of measurement. The metadata includes a `unit` field with the unit. + /// Column is nullable in case there is no such measurement at a given epoch. + pub fn to_field(&self) -> Field { + let mut meta = HashMap::new(); + meta.insert("unit".to_string(), self.unit().to_string()); + + Field::new( + format!("{self:?} ({})", self.unit()), + DataType::Float64, + true, + ) + .with_metadata(meta) + } } diff --git a/src/od/simulator/arc.rs b/src/od/simulator/arc.rs index 6302ee33..3327c7e4 100644 --- a/src/od/simulator/arc.rs +++ b/src/od/simulator/arc.rs @@ -25,7 +25,7 @@ use rand_pcg::Pcg64Mcg; use crate::dynamics::NyxError; use crate::io::ConfigError; use crate::md::trajectory::Interpolatable; -use crate::od::msr::{RangeDoppler, TrackingArc}; +use crate::od::msr::{RangeDoppler, TrackingArc, TrackingDataArc}; use crate::od::prelude::Strand; use crate::od::simulator::Cadence; use crate::od::{GroundStation, Measurement}; @@ -256,6 +256,95 @@ where Ok(trk) } + + /// Generates measurements for the tracking arc using the defined strands + /// + /// # Warning + /// This function will return an error if any of the devices defines as a scheduler. + /// You must create the schedule first using `build_schedule` first. + /// + /// # Notes + /// Although mutable, this function may be called several times to generate different measurements. + /// + /// # Algorithm + /// For each tracking device, and for each strand within that device, sample the trajectory at the sample + /// rate of the tracking device, adding a measurement whenever the spacecraft is visible. + /// Build the measurements as a vector, ordered chronologically. + /// + pub fn simulate_measurements( + &mut self, + almanac: Arc, + ) -> Result { + let mut measurements = BTreeMap::new(); + + for (name, device) in self.devices.iter_mut() { + let cfg = &self.configs[name]; + if cfg.scheduler.is_some() { + if cfg.strands.is_none() { + return Err(NyxError::CustomError { + msg: format!( + "schedule for {name} must be built before generating measurements" + ), + }); + } else { + warn!("scheduler for {name} is ignored, using the defined tracking strands instead") + } + } + + let init_msr_count = measurements.len(); + let tick = Epoch::now().unwrap(); + + match cfg.strands.as_ref() { + Some(strands) => { + // Strands are defined at this point + 'strands: for (ii, strand) in strands.iter().enumerate() { + // Build the time series for this strand, sampling at the correct rate + for epoch in TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) { + match device.measure_new( + epoch, + &self.trajectory, + Some(&mut self.rng), + almanac.clone(), + ) { + Ok(msr_opt) => { + if let Some(msr) = msr_opt { + measurements.insert(epoch, msr); + } + } + Err(e) => { + if epoch != strand.end { + warn!( + "Skipping the remaining strand #{ii} ending on {}: {e}", + strand.end + ); + } + continue 'strands; + } + } + } + } + + info!( + "Simulated {} measurements for {name} for {} tracking strands in {}", + measurements.len() - init_msr_count, + strands.len(), + (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) + ); + } + None => { + warn!("No tracking strands defined for {name}, skipping"); + } + } + } + + // Build the tracking arc. + let trk_data = TrackingDataArc { + measurements, + source: None, + }; + + Ok(trk_data) + } } impl Display for TrackingArcSim diff --git a/src/od/simulator/trackdata.rs b/src/od/simulator/trackdata.rs index 06b8f28c..24d6857b 100644 --- a/src/od/simulator/trackdata.rs +++ b/src/od/simulator/trackdata.rs @@ -28,6 +28,8 @@ use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, OMatrix}; use crate::md::prelude::{Frame, Traj}; use crate::md::trajectory::Interpolatable; +use crate::od::msr::measurement::Measurement as NewMeasurement; +use crate::od::msr::MeasurementType; use crate::od::{Measurement, ODError}; use crate::Orbit; @@ -79,4 +81,19 @@ where &mut self, epoch: Epoch, ) -> Result, ODError>; + + fn measure_new( + &mut self, + epoch: Epoch, + traj: &Traj, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError>; + + // Return the noise statistics of this tracking device for the provided measurement type at the requested epoch. + fn measurement_covar_new( + &self, + msr_type: MeasurementType, + epoch: Epoch, + ) -> Result; } diff --git a/tests/orbit_determination/multi_body.rs b/tests/orbit_determination/multi_body.rs index 97a5ba65..0da5273c 100644 --- a/tests/orbit_determination/multi_body.rs +++ b/tests/orbit_determination/multi_body.rs @@ -125,6 +125,13 @@ fn od_val_multi_body_ckf_perfect_stations( let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.set_devices(proc_devices, configs).unwrap(); + arc.to_parquet_simple("multi_body.parquet").unwrap(); + + let new_arc = arc_sim.simulate_measurements(almanac.clone()).unwrap(); + new_arc.to_parquet_simple("multi_body_new.parquet").unwrap(); + let new_arc_reloaded = TrackingDataArc::from_parquet("multi_body_new.parquet").unwrap(); + + assert_eq!(new_arc_reloaded, new_arc); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on From 5c22717d8f20097781531bd49396b1e23a32a063 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 17:30:15 -0700 Subject: [PATCH 08/38] Finish refactoring OD processor for new measurement structure --- .github/workflows/coverage.yaml | 5 +- Cargo.toml | 1 + data/tests/config/many_ground_stations.yaml | 6 + data/tests/config/one_ground_station.yaml | 3 + examples/02_jwst_covar_monte_carlo/main.rs | 12 +- examples/03_geo_analysis/stationkeeping.rs | 4 +- examples/04_lro_od/main.rs | 18 +- src/io/mod.rs | 2 +- src/io/tracking_data.rs | 2 +- src/mc/mod.rs | 2 +- src/mc/multivariate.rs | 18 +- src/md/opti/mod.rs | 3 - src/od/estimate/kfestimate.rs | 12 +- src/od/estimate/param.rs | 274 --------------- .../mod.rs} | 217 ++++++------ src/od/mod.rs | 177 +++++----- src/od/msr/arc.rs | 323 ------------------ src/od/msr/data_arc.rs | 57 +++- src/od/msr/measurement.rs | 21 +- src/od/msr/mod.rs | 9 +- src/od/msr/range.rs | 131 ------- src/od/msr/range_doppler.rs | 175 ---------- src/od/msr/rangerate.rs | 132 ------- src/od/msr/sensitivity.rs | 21 +- src/od/msr/types.rs | 34 +- src/od/process/export.rs | 64 ++-- src/od/process/mod.rs | 264 ++++++-------- src/od/simulator/arc.rs | 157 ++------- src/od/simulator/mod.rs | 2 +- src/od/simulator/trackdata.rs | 57 ++-- tests/mission_design/multishoot/mod.rs | 2 +- tests/mission_design/targeter/b_plane.rs | 5 +- tests/mission_design/targeter/finite_burns.rs | 1 - tests/mission_design/targeter/mod.rs | 2 - tests/mission_design/targeter/multi_oe.rs | 2 - tests/mission_design/targeter/multi_oe_vnc.rs | 1 - tests/mission_design/targeter/single_oe.rs | 5 +- tests/monte_carlo/framework.rs | 2 +- tests/orbit_determination/measurements.rs | 46 +-- tests/orbit_determination/multi_body.rs | 68 ++-- tests/orbit_determination/resid_reject.rs | 73 ++-- tests/orbit_determination/robust.rs | 46 ++- tests/orbit_determination/simulator.rs | 20 +- tests/orbit_determination/spacecraft.rs | 53 +-- tests/orbit_determination/trackingarc.rs | 105 ++---- tests/orbit_determination/two_body.rs | 131 +++---- tests/orbit_determination/xhat_dev.rs | 229 ++++--------- 47 files changed, 841 insertions(+), 2153 deletions(-) delete mode 100644 src/od/estimate/param.rs rename src/od/{ground_station.rs => ground_station/mod.rs} (79%) delete mode 100644 src/od/msr/arc.rs delete mode 100644 src/od/msr/range.rs delete mode 100644 src/od/msr/range_doppler.rs delete mode 100644 src/od/msr/rangerate.rs diff --git a/.github/workflows/coverage.yaml b/.github/workflows/coverage.yaml index 3d868ec1..8780f0ae 100644 --- a/.github/workflows/coverage.yaml +++ b/.github/workflows/coverage.yaml @@ -31,7 +31,10 @@ jobs: - name: Install cargo-llvm-cov uses: taiki-e/install-action@cargo-llvm-cov - name: Generate full code coverage - run: cargo llvm-cov --lcov --output-path lcov.info + run: | + cargo llvm-cov test --release --lib --no-report + cargo llvm-cov test --release cov_test --no-report + cargo llvm-cov report --lcov --output-path lcov.info - name: Upload coverage to Codecov uses: codecov/codecov-action@v3 with: diff --git a/Cargo.toml b/Cargo.toml index 9b5f34e0..6b108063 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -73,6 +73,7 @@ getrandom = { version = "0.2", features = ["js"] } typed-builder = "0.20.0" snafu = { version = "0.8.3", features = ["backtrace"] } serde_dhall = "0.12" +indexmap = {version = "2.6.0", features = ["serde"]} [dev-dependencies] diff --git a/data/tests/config/many_ground_stations.yaml b/data/tests/config/many_ground_stations.yaml index e21eec8c..b1379047 100644 --- a/data/tests/config/many_ground_stations.yaml +++ b/data/tests/config/many_ground_stations.yaml @@ -17,6 +17,9 @@ latitude_deg: 2.3522 longitude_deg: 48.8566 height_km: 0.4 + measurement_types: + - Range + - Doppler - name: Canberra frame: @@ -37,3 +40,6 @@ tau: 24 h process_noise: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - Range + - Doppler \ No newline at end of file diff --git a/data/tests/config/one_ground_station.yaml b/data/tests/config/one_ground_station.yaml index 98bd716a..8522ee2e 100644 --- a/data/tests/config/one_ground_station.yaml +++ b/data/tests/config/one_ground_station.yaml @@ -17,3 +17,6 @@ doppler_noise_km_s: tau: 24 h process_noise: 50.0e-6 # 5 cm/s light_time_correction: false +measurement_types: + - Range + - Doppler \ No newline at end of file diff --git a/examples/02_jwst_covar_monte_carlo/main.rs b/examples/02_jwst_covar_monte_carlo/main.rs index cf58f687..a5cbfc3f 100644 --- a/examples/02_jwst_covar_monte_carlo/main.rs +++ b/examples/02_jwst_covar_monte_carlo/main.rs @@ -16,12 +16,12 @@ use nyx::{ dynamics::{guidance::LocalFrame, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, io::ExportCfg, mc::MonteCarlo, - od::{prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess}, + od::{msr::TrackingDataArc, prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess}, propagators::Propagator, Spacecraft, State, }; -use std::{error::Error, sync::Arc}; +use std::{collections::BTreeMap, error::Error, sync::Arc}; fn main() -> Result<(), Box> { pel::init(); @@ -116,13 +116,17 @@ fn main() -> Result<(), Box> { // Build the propagation instance for the OD process. let prop = setup.with(jwst.with_stm(), almanac.clone()); - let mut odp = SpacecraftODProcess::ckf(prop, ckf, None, almanac.clone()); + let mut odp = SpacecraftODProcess::ckf(prop, ckf, BTreeMap::new(), None, almanac.clone()); // Define the prediction step, i.e. how often we want to know the covariance. let step = 1_i64.minutes(); // Finally, predict, and export the trajectory with covariance to a parquet file. odp.predict_for(step, prediction_duration)?; - odp.to_parquet("./02_jwst_covar_map.parquet", ExportCfg::default())?; + odp.to_parquet( + &TrackingDataArc::default(), + "./02_jwst_covar_map.parquet", + ExportCfg::default(), + )?; // === Monte Carlo framework === // Nyx comes with a complete multi-threaded Monte Carlo frame. It's blazing fast. diff --git a/examples/03_geo_analysis/stationkeeping.rs b/examples/03_geo_analysis/stationkeeping.rs index eef6a45d..8fdc730c 100644 --- a/examples/03_geo_analysis/stationkeeping.rs +++ b/examples/03_geo_analysis/stationkeeping.rs @@ -18,7 +18,7 @@ use nyx::{ Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, }, io::{gravity::HarmonicsMem, ExportCfg}, - mc::{MonteCarlo, MultivariateNormal, StateDispersion}, + mc::{MonteCarlo, MvnSpacecraft, StateDispersion}, md::{prelude::Objective, StateParameter}, propagators::{ErrorControl, IntegratorOptions, Propagator}, Spacecraft, State, @@ -88,7 +88,7 @@ fn main() -> Result<(), Box> { // Let's start by defining the dispersion. // The MultivariateNormal structure allows us to define the dispersions in any of the orbital parameters, but these are applied directly in the Cartesian state space. // Note that additional validation on the MVN is in progress -- https://github.com/nyx-space/nyx/issues/339. - let mc_rv = MultivariateNormal::new( + let mc_rv = MvnSpacecraft::new( sc, vec![StateDispersion::zero_mean(StateParameter::SMA, 3.0)], )?; diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs index 3a2a724f..c948a0e3 100644 --- a/examples/04_lro_od/main.rs +++ b/examples/04_lro_od/main.rs @@ -19,11 +19,10 @@ use nyx::{ io::{ConfigRepr, ExportCfg}, md::prelude::{HarmonicsMem, Traj}, od::{ - msr::RangeDoppler, prelude::{TrackingArcSim, TrkConfig, KF}, - process::{Estimate, NavSolution, ODProcess, ResidRejectCrit, SpacecraftUncertainty}, + process::{Estimate, NavSolution, ResidRejectCrit, SpacecraftUncertainty}, snc::SNC3, - GroundStation, + GroundStation, SpacecraftODProcess, }, propagators::Propagator, Orbit, Spacecraft, State, @@ -198,7 +197,7 @@ fn main() -> Result<(), Box> { .iter() .collect(); - let devices = GroundStation::load_many(ground_station_file)?; + let devices = GroundStation::load_named(ground_station_file)?; // Typical OD software requires that you specify your own tracking schedule or you'll have overlapping measurements. // Nyx can build a tracking schedule for you based on the first station with access. @@ -214,8 +213,8 @@ fn main() -> Result<(), Box> { let configs: BTreeMap = TrkConfig::load_named(trkconfg_yaml)?; // Build the tracking arc simulation to generate a "standard measurement". - let mut trk = TrackingArcSim::::new( - devices, + let mut trk = TrackingArcSim::::new( + devices.clone(), traj_as_flown.clone(), configs, )?; @@ -258,14 +257,15 @@ fn main() -> Result<(), Box> { ); // We'll set up the OD process to reject measurements whose residuals are mover than 4 sigmas away from what we expect. - let mut odp = ODProcess::ckf( + let mut odp = SpacecraftODProcess::ckf( setup.with(initial_estimate.state().with_stm(), almanac.clone()), kf, + devices, Some(ResidRejectCrit::default()), almanac.clone(), ); - odp.process_arc::(&arc)?; + odp.process_arc(&arc)?; let ric_err = traj_as_flown .at(odp.estimates.last().unwrap().epoch())? @@ -275,7 +275,7 @@ fn main() -> Result<(), Box> { println!("RIC Position (m): {}", ric_err.radius_km * 1e3); println!("RIC Velocity (m/s): {}", ric_err.velocity_km_s * 1e3); - odp.to_parquet("./04_lro_od_results.parquet", ExportCfg::default())?; + odp.to_parquet(&arc, "./04_lro_od_results.parquet", ExportCfg::default())?; // In our case, we have the truth trajectory from NASA. // So we can compute the RIC state difference between the real LRO ephem and what we've just estimated. diff --git a/src/io/mod.rs b/src/io/mod.rs index 0484d198..55cdcb55 100644 --- a/src/io/mod.rs +++ b/src/io/mod.rs @@ -46,7 +46,7 @@ pub mod estimate; /// Handles loading of gravity models using files of NASA PDS and GMAT COF. Several gunzipped files are provided with nyx. pub mod gravity; pub mod matrices; -pub mod tracking_data; +// pub mod tracking_data; pub mod trajectory_data; use std::io; diff --git a/src/io/tracking_data.rs b/src/io/tracking_data.rs index 1ac59bfa..227796a3 100644 --- a/src/io/tracking_data.rs +++ b/src/io/tracking_data.rs @@ -19,7 +19,7 @@ use crate::{ io::{MissingDataSnafu, ParquetSnafu}, linalg::{allocator::Allocator, DefaultAllocator, OVector}, - od::{msr::TrackingArc, Measurement}, + od::{msr::TrackingDataArc, Measurement}, }; #[cfg(feature = "python")] diff --git a/src/mc/mod.rs b/src/mc/mod.rs index 9a298869..8494ec91 100644 --- a/src/mc/mod.rs +++ b/src/mc/mod.rs @@ -32,7 +32,7 @@ mod generator; pub use generator::{DispersedState, Dispersion}; mod multivariate; -pub use multivariate::MultivariateNormal; +pub use multivariate::MvnSpacecraft; mod results; pub use results::{Results, Stats}; diff --git a/src/mc/multivariate.rs b/src/mc/multivariate.rs index 3aa1dd82..fd8c94b8 100644 --- a/src/mc/multivariate.rs +++ b/src/mc/multivariate.rs @@ -27,10 +27,8 @@ use nalgebra::{DMatrix, DVector, SMatrix, SVector}; use rand_distr::{Distribution, Normal}; use snafu::ResultExt; -/// A multivariate state generator for Monte Carlo analyses. Ensures that the covariance is properly applied on all provided state variables. -pub struct MultivariateNormal -// TODO: Rename to MultivariateNormalSpacecraft ?? -{ +/// A multivariate spacecraft state generator for Monte Carlo analyses. Ensures that the covariance is properly applied on all provided state variables. +pub struct MvnSpacecraft { /// The template state pub template: Spacecraft, pub dispersions: Vec, @@ -42,7 +40,7 @@ pub struct MultivariateNormal pub std_norm_distr: Normal, } -impl MultivariateNormal { +impl MvnSpacecraft { /// Creates a new mulivariate state generator from a mean and covariance on the set of state parameters. /// The covariance must be positive semi definite. /// @@ -266,7 +264,7 @@ impl MultivariateNormal { } } -impl Distribution> for MultivariateNormal { +impl Distribution> for MvnSpacecraft { fn sample(&self, rng: &mut R) -> DispersedState { // Generate the vector representing the state let x_rng = SVector::::from_fn(|_, _| self.std_norm_distr.sample(rng)); @@ -331,7 +329,7 @@ mod multivariate_ut { // Check that we can modify the radius magnitude let std_dev = 1.0; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![StateDispersion::builder() .param(StateParameter::Rmag) @@ -380,7 +378,7 @@ mod multivariate_ut { let std_dev = [10.0, 10.0, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.0]; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( Spacecraft { orbit: state, ..Default::default() @@ -462,7 +460,7 @@ mod multivariate_ut { let angle_sigma_deg = 0.2; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![StateDispersion::zero_mean( StateParameter::RAAN, @@ -530,7 +528,7 @@ mod multivariate_ut { let inc_sigma_deg = 0.15; let angle_sigma_deg = 0.02; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![ StateDispersion::zero_mean(StateParameter::SMA, sma_sigma_km), diff --git a/src/md/opti/mod.rs b/src/md/opti/mod.rs index 1524a795..51485570 100644 --- a/src/md/opti/mod.rs +++ b/src/md/opti/mod.rs @@ -25,9 +25,6 @@ pub mod raphson_finite_diff; pub mod raphson_hyperdual; pub mod solution; pub mod target_variable; -/// Uses a Levenberg Marquardt minimizer to solve the damped least squares problem. -// #[cfg(feature = "broken-donotuse")] -// pub mod minimize_lm; pub mod targeter; #[derive(Copy, Clone, Debug, PartialEq, Eq)] diff --git a/src/od/estimate/kfestimate.rs b/src/od/estimate/kfestimate.rs index f2b3ea00..b141e95d 100644 --- a/src/od/estimate/kfestimate.rs +++ b/src/od/estimate/kfestimate.rs @@ -20,7 +20,7 @@ use super::{Estimate, State}; use crate::cosmic::AstroError; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, Matrix, OMatrix, OVector}; -use crate::mc::{MultivariateNormal, StateDispersion}; +use crate::mc::{MvnSpacecraft, StateDispersion}; use crate::md::prelude::OrbitDual; use crate::md::StateParameter; use crate::Spacecraft; @@ -108,7 +108,7 @@ impl KfEstimate { dispersions: Vec, seed: Option, ) -> Result> { - let generator = MultivariateNormal::new(nominal_state, dispersions)?; + let generator = MvnSpacecraft::new(nominal_state, dispersions)?; let mut rng = match seed { Some(seed) => Pcg64Mcg::new(seed), @@ -149,12 +149,8 @@ impl KfEstimate { } /// Builds a multivariate random variable from this estimate's nominal state and covariance, zero mean. - pub fn to_random_variable(&self) -> Result> { - MultivariateNormal::from_spacecraft_cov( - self.nominal_state, - self.covar, - self.state_deviation, - ) + pub fn to_random_variable(&self) -> Result> { + MvnSpacecraft::from_spacecraft_cov(self.nominal_state, self.covar, self.state_deviation) } /// Returns the 1-sigma uncertainty for a given parameter, in that parameter's unit diff --git a/src/od/estimate/param.rs b/src/od/estimate/param.rs deleted file mode 100644 index 4c647ba3..00000000 --- a/src/od/estimate/param.rs +++ /dev/null @@ -1,274 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::md::StateParameter; - -use super::EpochFormat; -use std::cmp::PartialEq; -use std::fmt; - -/// Allowed headers, with an optional frame. -/// TODO: Support units -#[allow(non_camel_case_types)] -#[derive(Clone, Debug, PartialEq)] -pub enum EstimateParameter { - /// The epoch in the specified format - Epoch(EpochFormat), - /// Parameters of the estimated state - EstimatedState(Vec), - /// Parameters of the nominal state - NominalState(Vec), - /// Orbit deviation X (km) - Delta_x, - /// Orbit deviation Y (km) - Delta_y, - /// Orbit deviation Z (km) - Delta_z, - /// Orbit deviation VX (km/s) - Delta_vx, - /// Orbit deviation VY (km/s) - Delta_vy, - /// Orbit deviation VZ (km/s) - Delta_vz, - /// Covariance matrix [1,1] - Cx_x { frame: Option }, - /// Covariance matrix [2,1] - Cy_x { frame: Option }, - /// Covariance matrix [2,2] - Cy_y { frame: Option }, - /// Covariance matrix [3,1] - Cz_x { frame: Option }, - /// Covariance matrix [3,2] - Cz_y { frame: Option }, - /// Covariance matrix [3,3] - Cz_z { frame: Option }, - /// Covariance matrix [4,1] - Cx_dot_x { frame: Option }, - /// Covariance matrix [4,2] - Cx_dot_y { frame: Option }, - /// Covariance matrix [4,3] - Cx_dot_z { frame: Option }, - /// Covariance matrix [4,4] - Cx_dot_x_dot { frame: Option }, - /// Covariance matrix [5,1] - Cy_dot_x { frame: Option }, - /// Covariance matrix [5,2] - Cy_dot_y { frame: Option }, - /// Covariance matrix [5,3] - Cy_dot_z { frame: Option }, - /// Covariance matrix [5,4] - Cy_dot_x_dot { frame: Option }, - /// Covariance matrix [5,5] - Cy_dot_y_dot { frame: Option }, - /// Covariance matrix [6,1] - Cz_dot_x { frame: Option }, - /// Covariance matrix [6,2] - Cz_dot_y { frame: Option }, - /// Covariance matrix [6,3] - Cz_dot_z { frame: Option }, - /// Covariance matrix [6,4] - Cz_dot_x_dot { frame: Option }, - /// Covariance matrix [6,5] - Cz_dot_y_dot { frame: Option }, - /// Covariance matrix [6,6] - Cz_dot_z_dot { frame: Option }, - /// Boolean specifying whether this is a prediction or not - Prediction, - /// Norm of the position items of the covariance (Cx_x, Cy_y, Cz_z) - Covar_pos, - /// Norm of the velocity items of the covariance (Cx_dot_x_dot, Cy_dot_y_dot, Cz_dot_z_dot) - Covar_vel, -} - -impl fmt::Display for EstimateParameter { - fn fmt(&self, fh: &mut fmt::Formatter) -> fmt::Result { - match self { - EstimateParameter::Epoch(efmt) => write!(fh, "Epoch:{efmt:?}"), - EstimateParameter::EstimatedState(hdr) => { - let mut seq = Vec::with_capacity(hdr.len()); - for element in hdr { - seq.push(format!("Estimate:{element}")); - } - write!(fh, "{}", seq.join(",")) - } - EstimateParameter::NominalState(hdr) => { - let mut seq = Vec::with_capacity(hdr.len()); - for element in hdr { - seq.push(format!("Nominal:{element}")); - } - write!(fh, "{}", seq.join(",")) - } - EstimateParameter::Delta_x => write!(fh, "delta_x"), - EstimateParameter::Delta_y => write!(fh, "delta_y"), - EstimateParameter::Delta_z => write!(fh, "delta_z"), - EstimateParameter::Delta_vx => write!(fh, "delta_vx"), - EstimateParameter::Delta_vy => write!(fh, "delta_vy"), - EstimateParameter::Delta_vz => write!(fh, "delta_vz"), - EstimateParameter::Cx_x { frame } => { - if let Some(f) = frame { - write!(fh, "cx_x:{f}") - } else { - write!(fh, "cx_x") - } - } - EstimateParameter::Cy_x { frame } => { - if let Some(f) = frame { - write!(fh, "cy_x:{f}") - } else { - write!(fh, "cy_x") - } - } - EstimateParameter::Cy_y { frame } => { - if let Some(f) = frame { - write!(fh, "cy_y:{f}") - } else { - write!(fh, "cy_y") - } - } - EstimateParameter::Cz_x { frame } => { - if let Some(f) = frame { - write!(fh, "cz_x:{f}") - } else { - write!(fh, "cz_x") - } - } - EstimateParameter::Cz_y { frame } => { - if let Some(f) = frame { - write!(fh, "cz_y:{f}") - } else { - write!(fh, "cz_y") - } - } - EstimateParameter::Cz_z { frame } => { - if let Some(f) = frame { - write!(fh, "cz_z:{f}") - } else { - write!(fh, "cz_z") - } - } - EstimateParameter::Cx_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_x:{f}") - } else { - write!(fh, "cx_dot_x") - } - } - EstimateParameter::Cx_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_y:{f}") - } else { - write!(fh, "cx_dot_y") - } - } - EstimateParameter::Cx_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_z:{f}") - } else { - write!(fh, "cx_dot_z") - } - } - EstimateParameter::Cx_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_x_dot:{f}") - } else { - write!(fh, "cx_dot_x_dot") - } - } - EstimateParameter::Cy_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_x:{f}") - } else { - write!(fh, "cy_dot_x") - } - } - EstimateParameter::Cy_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_y:{f}") - } else { - write!(fh, "cy_dot_y") - } - } - EstimateParameter::Cy_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_z:{f}") - } else { - write!(fh, "cy_dot_z") - } - } - EstimateParameter::Cy_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_x_dot:{f}") - } else { - write!(fh, "cy_dot_x_dot") - } - } - EstimateParameter::Cy_dot_y_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_y_dot:{f}") - } else { - write!(fh, "cy_dot_y_dot") - } - } - EstimateParameter::Cz_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_x:{f}") - } else { - write!(fh, "cz_dot_x") - } - } - EstimateParameter::Cz_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_y:{f}") - } else { - write!(fh, "cz_dot_y") - } - } - EstimateParameter::Cz_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_z:{f}") - } else { - write!(fh, "cz_dot_z") - } - } - EstimateParameter::Cz_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_x_dot:{f}") - } else { - write!(fh, "cz_dot_x_dot") - } - } - EstimateParameter::Cz_dot_y_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_y_dot:{f}") - } else { - write!(fh, "cz_dot_y_dot") - } - } - EstimateParameter::Cz_dot_z_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_z_dot:{f}") - } else { - write!(fh, "cz_dot_z_dot") - } - } - EstimateParameter::Prediction => write!(fh, "prediction"), - EstimateParameter::Covar_pos => write!(fh, "covar_position"), - EstimateParameter::Covar_vel => write!(fh, "covar_velocity"), - } - } -} diff --git a/src/od/ground_station.rs b/src/od/ground_station/mod.rs similarity index 79% rename from src/od/ground_station.rs rename to src/od/ground_station/mod.rs index 9904e2af..1b585cd0 100644 --- a/src/od/ground_station.rs +++ b/src/od/ground_station/mod.rs @@ -17,21 +17,23 @@ */ use anise::astro::{Aberration, AzElRange, PhysicsResult}; +use anise::constants::frames::EARTH_J2000; use anise::errors::AlmanacResult; use anise::prelude::{Almanac, Frame, Orbit}; +use indexmap::IndexSet; use super::msr::measurement::Measurement; -use super::msr::RangeDoppler; +use super::msr::MeasurementType; use super::noise::StochasticNoise; -use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDeviceSim}; +use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; use crate::errors::EventError; use crate::io::ConfigRepr; use crate::md::prelude::{Interpolatable, Traj}; use crate::md::EventEvaluator; use crate::time::Epoch; use crate::Spacecraft; -use hifitime::{Duration, Unit}; -use nalgebra::{allocator::Allocator, DefaultAllocator, OMatrix}; +use hifitime::{Duration, TimeUnits, Unit}; +use nalgebra::{allocator::Allocator, DefaultAllocator}; use rand_pcg::Pcg64Mcg; use serde_derive::{Deserialize, Serialize}; use snafu::ResultExt; @@ -56,8 +58,11 @@ pub struct GroundStation { /// in km pub height_km: f64, pub frame: Frame, + pub measurement_types: IndexSet, /// Duration needed to generate a measurement (if unset, it is assumed to be instantaneous) - #[serde(skip)] + /// TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. + /// Without it, we're stuck having to rebuild them from scratch. + /// https://github.com/nyx-space/nyx/issues/140 pub integration_time: Option, /// Whether to correct for light travel time pub light_time_correction: bool, @@ -86,6 +91,7 @@ impl GroundStation { longitude_deg, height_km, frame, + measurement_types: IndexSet::new(), integration_time: None, light_time_correction: false, timestamp_noise_s: None, @@ -100,6 +106,9 @@ impl GroundStation { doppler_noise_km_s: StochasticNoise, iau_earth: Frame, ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); Self { name: "Madrid".to_string(), elevation_mask_deg: elevation_mask, @@ -107,6 +116,7 @@ impl GroundStation { longitude_deg: 4.250_556, height_km: 0.834_939, frame: iau_earth, + measurement_types, integration_time: None, light_time_correction: false, timestamp_noise_s: None, @@ -121,6 +131,9 @@ impl GroundStation { doppler_noise_km_s: StochasticNoise, iau_earth: Frame, ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); Self { name: "Canberra".to_string(), elevation_mask_deg: elevation_mask, @@ -128,6 +141,7 @@ impl GroundStation { longitude_deg: 148.981_944, height_km: 0.691_750, frame: iau_earth, + measurement_types, integration_time: None, light_time_correction: false, timestamp_noise_s: None, @@ -142,6 +156,9 @@ impl GroundStation { doppler_noise_km_s: StochasticNoise, iau_earth: Frame, ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); Self { name: "Goldstone".to_string(), elevation_mask_deg: elevation_mask, @@ -149,6 +166,7 @@ impl GroundStation { longitude_deg: 243.205, height_km: 1.071_149_04, frame: iau_earth, + measurement_types, integration_time: None, light_time_correction: false, timestamp_noise_s: None, @@ -191,51 +209,64 @@ impl GroundStation { ) } - /// Returns the timestamp noise, range noise, and doppler noise for this ground station at the provided epoch. - fn noises( - &mut self, - epoch: Epoch, - rng: Option<&mut Pcg64Mcg>, - ) -> Result<(f64, f64, f64), ODError> { - let timestamp_noise_s; - let range_noise_km; - let doppler_noise_km_s; - - match rng { - Some(rng) => { - // Add the range noise, or return an error if it's not configured. - range_noise_km = self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .sample(epoch, rng); - - // Add the Doppler noise, or return an error if it's not configured. - doppler_noise_km_s = self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .sample(epoch, rng); - - // Only add the epoch noise if it's configured, it's valid to not have any noise on the clock. - if let Some(mut timestamp_noise) = self.timestamp_noise_s { - timestamp_noise_s = timestamp_noise.sample(epoch, rng); - } else { - timestamp_noise_s = 0.0; - } + /// Returns the noises for all measurement types configured for this ground station at the provided epoch, timestamp noise is the first entry. + fn noises(&mut self, epoch: Epoch, rng: Option<&mut Pcg64Mcg>) -> Result, ODError> { + let mut noises = vec![0.0; self.measurement_types.len() + 1]; + + if let Some(rng) = rng { + // Add the timestamp noise first + + if let Some(mut timestamp_noise) = self.timestamp_noise_s { + noises[0] = timestamp_noise.sample(epoch, rng); } - None => { - timestamp_noise_s = 0.0; - range_noise_km = 0.0; - doppler_noise_km_s = 0.0; + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + noises[ii + 1] = match msr_type { + MeasurementType::Range => self + .range_noise_km + .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? + .sample(epoch, rng), + MeasurementType::Doppler => self + .doppler_noise_km_s + .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? + .sample(epoch, rng), + }; } - }; + } - Ok((timestamp_noise_s, range_noise_km, doppler_noise_km_s)) + Ok(noises) + } +} + +impl Default for GroundStation { + fn default() -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + Self { + name: "UNDEFINED".to_string(), + measurement_types, + elevation_mask_deg: 0.0, + latitude_deg: 0.0, + longitude_deg: 0.0, + height_km: 0.0, + frame: EARTH_J2000, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + range_noise_km: None, + doppler_noise_km_s: None, + } } } impl ConfigRepr for GroundStation {} -impl TrackingDeviceSim for GroundStation { +impl TrackingDevice for GroundStation { + fn measurement_types(&self) -> &IndexSet { + &self.measurement_types + } + /// Perform a measurement from the ground station to the receiver (rx). fn measure( &mut self, @@ -243,9 +274,10 @@ impl TrackingDeviceSim for GroundStation { traj: &Traj, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError> { + ) -> Result, ODError> { match self.integration_time { Some(integration_time) => { + // If out of traj bounds, return None. let rx_0 = traj.at(epoch - integration_time).context(ODTrajSnafu)?; let rx_1 = traj.at(epoch).context(ODTrajSnafu)?; @@ -277,16 +309,16 @@ impl TrackingDeviceSim for GroundStation { } // Noises are computed at the midpoint of the integration time. - let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = - self.noises(epoch - integration_time * 0.5, rng)?; - - Ok(Some(RangeDoppler::two_way( - aer_t0, - aer_t1, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ))) + let noises = self.noises(epoch - integration_time * 0.5, rng)?; + + let mut msr = Measurement::new(self.name.clone(), epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_two_way(aer_t0, aer_t1, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) } None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac), } @@ -305,7 +337,7 @@ impl TrackingDeviceSim for GroundStation { rx: Spacecraft, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError> { + ) -> Result, ODError> { let obstructing_body = if !self.frame.ephem_origin_match(rx.frame()) { Some(rx.frame()) } else { @@ -320,15 +352,16 @@ impl TrackingDeviceSim for GroundStation { if aer.elevation_deg >= self.elevation_mask_deg { // Only update the noises if the measurement is valid. - let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = - self.noises(rx.orbit.epoch, rng)?; - - Ok(Some(RangeDoppler::one_way( - aer, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ))) + let noises = self.noises(rx.orbit.epoch, rng)?; + + let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_one_way(aer, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) } else { debug!( "{} {} (el. mask {:.3} deg), object at {:.3} deg -- no measurement", @@ -346,37 +379,6 @@ impl TrackingDeviceSim for GroundStation { /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. fn measurement_covar( - &mut self, - epoch: Epoch, - ) -> Result< - OMatrix< - f64, - ::MeasurementSize, - ::MeasurementSize, - >, - ODError, - > { - let range_noise_km2 = self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .covariance(epoch); - let doppler_noise_km2_s2 = self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .covariance(epoch); - - let mut msr_noises = OMatrix::< - f64, - ::MeasurementSize, - ::MeasurementSize, - >::zeros(); - msr_noises[(0, 0)] = range_noise_km2; - msr_noises[(1, 1)] = doppler_noise_km2_s2; - - Ok(msr_noises) - } - - fn measurement_covar_new( &self, msr_type: super::prelude::MeasurementType, epoch: Epoch, @@ -392,20 +394,6 @@ impl TrackingDeviceSim for GroundStation { .covariance(epoch), }) } - - fn measure_new( - &mut self, - epoch: Epoch, - traj: &Traj, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, - ) -> Result, ODError> { - Ok(self.measure(epoch, traj, rng, almanac)?.map(|msr| { - Measurement::new(self.name.clone(), epoch) - .with(super::msr::MeasurementType::Range, msr.obs[0]) - .with(super::msr::MeasurementType::Doppler, msr.obs[1]) - })) - } } impl fmt::Display for GroundStation { @@ -475,7 +463,9 @@ where #[cfg(test)] mod gs_ut { + use anise::constants::frames::IAU_EARTH_FRAME; + use indexmap::IndexSet; use crate::io::ConfigRepr; use crate::od::prelude::*; @@ -504,9 +494,14 @@ mod gs_ut { dbg!(&gs); + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + let expected_gs = GroundStation { name: "Demo ground station".to_string(), frame: IAU_EARTH_FRAME, + measurement_types, elevation_mask_deg: 5.0, range_noise_km: Some(StochasticNoise { bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), @@ -549,10 +544,15 @@ mod gs_ut { dbg!(&stations); + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + let expected = vec![ GroundStation { name: "Demo ground station".to_string(), frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), + measurement_types: measurement_types.clone(), elevation_mask_deg: 5.0, range_noise_km: Some(StochasticNoise { bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), @@ -572,6 +572,7 @@ mod gs_ut { GroundStation { name: "Canberra".to_string(), frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), + measurement_types, elevation_mask_deg: 5.0, range_noise_km: Some(StochasticNoise { bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), diff --git a/src/od/mod.rs b/src/od/mod.rs index c017b587..ecac0a61 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -19,12 +19,10 @@ use crate::dynamics::DynamicsError; pub use crate::dynamics::{Dynamics, NyxError}; use crate::io::{ConfigError, InputOutputError}; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector}; +use crate::linalg::OVector; use crate::md::trajectory::TrajError; use crate::propagators::PropagationError; use crate::time::Epoch; -use crate::Orbit; pub use crate::{State, TimeTagged}; use anise::almanac::planetary::PlanetaryDataError; use anise::errors::AlmanacError; @@ -54,8 +52,7 @@ pub mod simulator; /// Provides the interfaces to the orbit determination process pub mod process; -use arrow::datatypes::Field; -pub use simulator::TrackingDeviceSim; +pub use simulator::TrackingDevice; /// Provides all state noise compensation functionality pub mod snc; @@ -64,10 +61,10 @@ pub mod snc; pub type SpacecraftODProcess<'a> = self::process::ODProcess< 'a, crate::md::prelude::SpacecraftDynamics, - msr::RangeDoppler, + nalgebra::Const<2>, nalgebra::Const<3>, - crate::Spacecraft, filter::kalman::KF, nalgebra::Const<2>>, + GroundStation, >; #[allow(unused_imports)] @@ -86,88 +83,88 @@ pub mod prelude { pub use crate::time::{Duration, Epoch, TimeUnits, Unit}; } -/// A trait defining a measurement that can be used in the orbit determination process. -pub trait Measurement: Copy + TimeTagged { - /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2). - type MeasurementSize: DimName; - - /// Returns the fields for this kind of measurement. - /// The metadata must include a `unit` field with the unit. - fn fields() -> Vec; - - /// Initializes a new measurement from the provided data. - fn from_observation(epoch: Epoch, obs: OVector) -> Self - where - DefaultAllocator: Allocator; - - /// Returns the measurement/observation as a vector. - fn observation(&self) -> OVector - where - DefaultAllocator: Allocator; -} - -/// The Estimate trait defines the interface that is the opposite of a `SolveFor`. -/// For example, `impl EstimateFrom for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`. -/// -/// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and -/// the ground station bias information. Then, the `impl EstimateFrom for OrbitAndBias` will be added, where `OrbitAndBias` is the -/// new State that includes the orbit and the bias of one ground station. -pub trait EstimateFrom -where - Self: State, - DefaultAllocator: Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator, -{ - /// From the state extract the state to be estimated - fn extract(from: O) -> Self; - - /// Returns the measurement sensitivity (often referred to as H tilde). - /// - /// # Limitations - /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero. - /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use. - /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator. - fn sensitivity( - msr: &M, - receiver: Self, - transmitter: Orbit, - ) -> OMatrix - where - DefaultAllocator: Allocator; -} - -/// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state. -/// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame. -impl EstimateFrom for O -where - O: State + Measurement, - Self: State, - DefaultAllocator: Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator, -{ - fn extract(from: O) -> Self { - from - } - - fn sensitivity( - _full_state_msr: &O, - _receiver: Self, - _transmitter: Orbit, - ) -> OMatrix::MeasurementSize, Self::Size> - where - DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, - { - OMatrix::::identity() - } -} +// /// A trait defining a measurement that can be used in the orbit determination process. +// pub trait Measurement: Copy + TimeTagged { +// /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2). +// type MeasurementSize: DimName; + +// /// Returns the fields for this kind of measurement. +// /// The metadata must include a `unit` field with the unit. +// fn fields() -> Vec; + +// /// Initializes a new measurement from the provided data. +// fn from_observation(epoch: Epoch, obs: OVector) -> Self +// where +// DefaultAllocator: Allocator; + +// /// Returns the measurement/observation as a vector. +// fn observation(&self) -> OVector +// where +// DefaultAllocator: Allocator; +// } + +// /// The Estimate trait defines the interface that is the opposite of a `SolveFor`. +// /// For example, `impl EstimateFrom for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`. +// /// +// /// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and +// /// the ground station bias information. Then, the `impl EstimateFrom for OrbitAndBias` will be added, where `OrbitAndBias` is the +// /// new State that includes the orbit and the bias of one ground station. +// pub trait EstimateFrom +// where +// Self: State, +// DefaultAllocator: Allocator<::Size> +// + Allocator<::VecLength> +// + Allocator<::Size, ::Size> +// + Allocator +// + Allocator +// + Allocator, +// { +// /// From the state extract the state to be estimated +// fn extract(from: O) -> Self; + +// /// Returns the measurement sensitivity (often referred to as H tilde). +// /// +// /// # Limitations +// /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero. +// /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use. +// /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator. +// fn sensitivity( +// msr: &M, +// receiver: Self, +// transmitter: Orbit, +// ) -> OMatrix +// where +// DefaultAllocator: Allocator; +// } + +// /// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state. +// /// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame. +// impl EstimateFrom for O +// where +// O: State + Measurement, +// Self: State, +// DefaultAllocator: Allocator<::Size> +// + Allocator<::VecLength> +// + Allocator<::Size, ::Size> +// + Allocator +// + Allocator +// + Allocator, +// { +// fn extract(from: O) -> Self { +// from +// } + +// fn sensitivity( +// _full_state_msr: &O, +// _receiver: Self, +// _transmitter: Orbit, +// ) -> OMatrix::MeasurementSize, Self::Size> +// where +// DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, +// { +// OMatrix::::identity() +// } +// } #[derive(Debug, PartialEq, Snafu)] #[snafu(visibility(pub(crate)))] @@ -194,6 +191,8 @@ pub enum ODError { SingularNoiseRk, #[snafu(display("{kind} noise not configured"))] NoiseNotConfigured { kind: &'static str }, + #[snafu(display("measurement sim error: {details}"))] + MeasurementSimError { details: String }, #[snafu(display("during an OD encountered {source}"))] ODTrajError { source: TrajError }, #[snafu(display("OD failed because {source}"))] diff --git a/src/od/msr/arc.rs b/src/od/msr/arc.rs deleted file mode 100644 index 59d86a42..00000000 --- a/src/od/msr/arc.rs +++ /dev/null @@ -1,323 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use std::collections::{BTreeMap, HashMap, HashSet}; -use std::error::Error; -use std::fmt::{Debug, Display}; -use std::fs::File; -use std::ops::RangeBounds; -use std::path::{Path, PathBuf}; -use std::sync::Arc; - -use crate::io::watermark::pq_writer; -use crate::io::{ConfigError, ExportCfg}; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, DimName}; -use crate::md::trajectory::Interpolatable; -use crate::od::prelude::TrkConfig; -use crate::od::{Measurement, TrackingDeviceSim}; -use crate::State; - -use arrow::array::{Array, Float64Builder, StringBuilder}; -use arrow::datatypes::{DataType, Field, Schema}; -use arrow::record_batch::RecordBatch; -use hifitime::prelude::{Duration, Epoch, Unit}; -use hifitime::TimeScale; -use parquet::arrow::ArrowWriter; - -/// Tracking arc contains the tracking data generated by the tracking devices defined in this structure. -/// This structure is shared between both simulated and real tracking arcs. -#[derive(Clone, Default, Debug)] -pub struct TrackingArc -where - Msr: Measurement, - DefaultAllocator: Allocator, -{ - /// The YAML configuration to set up these devices - pub device_cfg: String, - /// A chronological list of the measurements to the devices used to generate these measurements. If the name of the device does not appear in the list of devices, then the measurement will be ignored. - pub measurements: Vec<(String, Msr)>, -} - -impl Display for TrackingArc -where - Msr: Measurement, - DefaultAllocator: - Allocator + Allocator, -{ - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!( - f, - "{} measurements from {:?}", - self.measurements.len(), - self.device_names() - ) - } -} - -impl TrackingArc -where - Msr: Measurement, - DefaultAllocator: - Allocator + Allocator, -{ - /// Store this tracking arc to a parquet file. - pub fn to_parquet_simple + Debug>( - &self, - path: P, - ) -> Result> { - self.to_parquet(path, ExportCfg::default()) - } - - /// Store this tracking arc to a parquet file, with optional metadata and a timestamp appended to the filename. - pub fn to_parquet + Debug>( - &self, - path: P, - cfg: ExportCfg, - ) -> Result> { - let path_buf = cfg.actual_path(path); - - if cfg.step.is_some() { - warn!("The `step` parameter in the export is not supported for tracking arcs."); - } - - if cfg.fields.is_some() { - warn!("The `fields` parameter in the export is not supported for tracking arcs."); - } - - // Build the schema - let mut hdrs = vec![ - Field::new("Epoch (UTC)", DataType::Utf8, false), - Field::new("Tracking device", DataType::Utf8, false), - ]; - - let mut msr_fields = Msr::fields(); - - hdrs.append(&mut msr_fields); - - // Build the schema - let schema = Arc::new(Schema::new(hdrs)); - let mut record: Vec> = Vec::new(); - - // Build the measurement iterator - - let measurements = - if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { - let start = cfg - .start_epoch - .unwrap_or_else(|| self.measurements.first().unwrap().1.epoch()); - let end = cfg - .end_epoch - .unwrap_or_else(|| self.measurements.last().unwrap().1.epoch()); - - info!("Exporting measurements from {start} to {end}."); - - self.measurements - .iter() - .filter(|msr| msr.1.epoch() >= start && msr.1.epoch() <= end) - .cloned() - .collect() - } else { - self.measurements.to_vec() - }; - - // Build all of the records - - // Epochs - let mut utc_epoch = StringBuilder::new(); - for m in &measurements { - utc_epoch.append_value(m.1.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); - } - record.push(Arc::new(utc_epoch.finish())); - - // Device names - let mut device_names = StringBuilder::new(); - for m in &measurements { - device_names.append_value(m.0.clone()); - } - record.push(Arc::new(device_names.finish())); - - // Measurement data - for obs_no in 0..Msr::MeasurementSize::USIZE { - let mut data_builder = Float64Builder::new(); - - for m in &measurements { - data_builder.append_value(m.1.observation()[obs_no]); - } - record.push(Arc::new(data_builder.finish())); - } - - // Serialize all of the devices and add that to the parquet file too. - let mut metadata = HashMap::new(); - metadata.insert("devices".to_string(), self.device_cfg.clone()); - metadata.insert("Purpose".to_string(), "Tracking Arc Data".to_string()); - if let Some(add_meta) = cfg.metadata { - for (k, v) in add_meta { - metadata.insert(k, v); - } - } - - let props = pq_writer(Some(metadata)); - - let file = File::create(&path_buf)?; - - let mut writer = ArrowWriter::try_new(file, schema.clone(), props).unwrap(); - - let batch = RecordBatch::try_new(schema, record)?; - writer.write(&batch)?; - writer.close()?; - - info!("Serialized {self} to {}", path_buf.display()); - - // Return the path this was written to - Ok(path_buf) - } - - /// Returns the set of devices from which measurements were taken. This accounts for the availability of measurements, so if a device was not available, it will not appear in this set. - pub fn device_names(&self) -> HashSet<&String> { - let mut set = HashSet::new(); - self.measurements.iter().for_each(|(name, _msr)| { - set.insert(name); - }); - set - } - - /// Returns the minimum duration between two subsequent measurements. - /// This is important to correctly set up the propagator and not miss any measurement. - pub fn min_duration_sep(&self) -> Option { - if self.measurements.is_empty() { - None - } else { - let mut windows = self.measurements.windows(2); - let first_window = windows.next()?; - // Ensure that the first interval isn't zero - let mut min_interval = - (first_window[1].1.epoch() - first_window[0].1.epoch()).max(2 * Unit::Second); - for window in windows { - let interval = window[1].1.epoch() - window[0].1.epoch(); - if interval > Duration::ZERO && interval < min_interval { - min_interval = interval; - } - } - Some(min_interval) - } - } - - /// If this tracking arc has devices that can be used to generate simulated measurements, - /// then this function can be used to rebuild said measurement devices - pub fn rebuild_devices(&self) -> Result, ConfigError> - where - MsrIn: Interpolatable, - D: TrackingDeviceSim, - DefaultAllocator: Allocator<::Size> - + Allocator<::Size, ::Size> - + Allocator<::VecLength>, - { - let devices = D::loads_named(&self.device_cfg)?; - - for device in devices.keys() { - if !self.device_names().contains(device) { - info!("no measurements from {device} in loaded arc"); - continue; - } - } - - Ok(devices) - } - - /// Returns a new tracking arc that only contains measurements that fall within the given epoch range. - pub fn filter_by_epoch>(&self, bound: R) -> Self { - let mut measurements = Vec::new(); - for (name, msr) in &self.measurements { - if bound.contains(&msr.epoch()) { - measurements.push((name.clone(), *msr)); - } - } - - Self { - measurements, - device_cfg: self.device_cfg.clone(), - } - } - - /// Returns a new tracking arc that only contains measurements that fall within the given offset from the first epoch - pub fn filter_by_offset>(&self, bound: R) -> Self { - if self.measurements.is_empty() { - return Self { - device_cfg: self.device_cfg.clone(), - measurements: Vec::new(), - }; - } - let ref_epoch = self.measurements[0].1.epoch(); - let mut measurements = Vec::new(); - for (name, msr) in &self.measurements { - if bound.contains(&(msr.epoch() - ref_epoch)) { - measurements.push((name.clone(), *msr)); - } - } - - Self { - measurements, - device_cfg: self.device_cfg.clone(), - } - } - - /// If this tracking arc has devices that can be used to generate simulated measurements, - /// then this function can be used to rebuild said measurement devices - pub fn set_devices( - &mut self, - devices: Vec, - configs: BTreeMap, - ) -> Result<(), Box> - where - MsrIn: Interpolatable, - D: TrackingDeviceSim, - DefaultAllocator: Allocator<::Size> - + Allocator<::Size, ::Size> - + Allocator<::VecLength>, - { - let mut devices_map = BTreeMap::new(); - let mut sampling_rates_ns = Vec::with_capacity(devices.len()); - for device in devices { - if let Some(cfg) = configs.get(&device.name()) { - if let Err(e) = cfg.sanity_check() { - warn!("Ignoring device {}: {e}", device.name()); - continue; - } - sampling_rates_ns.push(cfg.sampling.truncated_nanoseconds()); - } else { - warn!( - "Ignoring device {}: no associated tracking configuration", - device.name() - ); - continue; - } - devices_map.insert(device.name(), device); - } - - if devices_map.is_empty() { - return Err(Box::new(ConfigError::InvalidConfig { - msg: "None of the devices are properly configured".to_string(), - })); - } - - self.device_cfg = serde_yaml::to_string(&devices_map).unwrap(); - - Ok(()) - } -} diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index e2a37849..5477ed0f 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -30,17 +30,20 @@ use arrow::{ use core::fmt; use hifitime::prelude::{Duration, Epoch}; use hifitime::TimeScale; +use indexmap::IndexSet; use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; use parquet::arrow::ArrowWriter; use snafu::{ensure, ResultExt}; -use std::collections::{BTreeMap, HashMap, HashSet}; +use std::collections::{BTreeMap, HashMap}; use std::error::Error; use std::fs::File; +use std::ops::Bound::{Excluded, Included, Unbounded}; +use std::ops::RangeBounds; use std::path::{Path, PathBuf}; use std::sync::Arc; /// Tracking data storing all of measurements as a B-Tree. -#[derive(Clone)] +#[derive(Clone, Default)] pub struct TrackingDataArc { /// All measurements in this data arc pub measurements: BTreeMap, @@ -50,7 +53,7 @@ pub struct TrackingDataArc { impl TrackingDataArc { /// Loads a tracking arc from its serialization in parquet. - pub fn from_parquet + fmt::Display>(path: P) -> Result { + pub fn from_parquet>(path: P) -> Result { // Read the file since we closed it earlier let file = File::open(&path).context(StdIOSnafu { action: "opening file for tracking arc", @@ -177,24 +180,24 @@ impl TrackingDataArc { Ok(Self { measurements, - source: Some(path.to_string()), + source: Some(path.as_ref().to_path_buf().display().to_string()), }) } /// Returns the unique list of aliases in this tracking data arc - pub fn unique_aliases(&self) -> HashSet { + pub fn unique_aliases(&self) -> IndexSet { self.unique().0 } /// Returns the unique measurement types in this tracking data arc - pub fn unique_types(&self) -> HashSet { + pub fn unique_types(&self) -> IndexSet { self.unique().1 } /// Returns the unique trackers and unique measurement types in this data arc - pub fn unique(&self) -> (HashSet, HashSet) { - let mut aliases = HashSet::new(); - let mut types = HashSet::new(); + pub fn unique(&self) -> (IndexSet, IndexSet) { + let mut aliases = IndexSet::new(); + let mut types = IndexSet::new(); for msr in self.measurements.values() { aliases.insert(msr.tracker.clone()); for k in msr.data.keys() { @@ -241,16 +244,42 @@ impl TrackingDataArc { } } + /// Returns a new tracking arc that only contains measurements that fall within the given epoch range. + pub fn filter_by_epoch>(mut self, bound: R) -> Self { + self.measurements = self + .measurements + .range(bound) + .map(|(epoch, msr)| (*epoch, msr.clone())) + .collect::>(); + self + } + + /// Returns a new tracking arc that only contains measurements that fall within the given offset from the first epoch + pub fn filter_by_offset>(self, bound: R) -> Self { + if self.is_empty() { + return self; + } + // Rebuild an epoch bound. + let start = match bound.start_bound() { + Unbounded => self.start_epoch().unwrap(), + Included(offset) | Excluded(offset) => self.start_epoch().unwrap() + *offset, + }; + + let end = match bound.end_bound() { + Unbounded => self.end_epoch().unwrap(), + Included(offset) | Excluded(offset) => self.end_epoch().unwrap() - *offset, + }; + + self.filter_by_epoch(start..end) + } + /// Store this tracking arc to a parquet file. - pub fn to_parquet_simple + fmt::Display>( - &self, - path: P, - ) -> Result> { + pub fn to_parquet_simple>(&self, path: P) -> Result> { self.to_parquet(path, ExportCfg::default()) } /// Store this tracking arc to a parquet file, with optional metadata and a timestamp appended to the filename. - pub fn to_parquet + fmt::Display>( + pub fn to_parquet>( &self, path: P, cfg: ExportCfg, diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs index b8d9cb8a..9a03792f 100644 --- a/src/od/msr/measurement.rs +++ b/src/od/msr/measurement.rs @@ -18,8 +18,10 @@ use super::MeasurementType; use hifitime::Epoch; +use indexmap::IndexSet; use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OVector}; -use std::collections::{HashMap, HashSet}; +use std::collections::HashMap; +use std::fmt; /// A type-agnostic simultaneous measurement storage structure. Allows storing any number of simultaneous measurement of a given taker. #[derive(Clone, Debug, PartialEq)] @@ -53,7 +55,7 @@ impl Measurement { /// Builds an observation vector for this measurement provided a set of measurement types. /// If the requested measurement type is not available, then that specific row is set to zero. /// The caller must set the appropriate sensitivity matrix rows to zero. - pub fn observation(&self, types: HashSet) -> OVector + pub fn observation(&self, types: &IndexSet) -> OVector where DefaultAllocator: Allocator, { @@ -67,7 +69,7 @@ impl Measurement { } /// Returns a vector specifying which measurement types are available. - pub fn availability(&self, types: HashSet) -> Vec { + pub fn availability(&self, types: &IndexSet) -> Vec { let mut rtn = Vec::with_capacity(types.len()); for (i, t) in types.iter().enumerate() { if self.data.contains_key(t) { @@ -77,3 +79,16 @@ impl Measurement { rtn } } + +impl fmt::Display for Measurement { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let msrs = self + .data + .iter() + .map(|(msr_type, msr_value)| format!("{msr_type:?} = {msr_value} {}", msr_type.unit())) + .collect::>() + .join(", "); + + write!(f, "{} measured {} on {}", self.tracker, msrs, self.epoch) + } +} diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 528dd339..61d2414f 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -16,18 +16,11 @@ along with this program. If not, see . */ -mod arc; mod data_arc; pub mod measurement; -mod range; -mod range_doppler; -mod rangerate; pub mod sensitivity; mod types; -pub use arc::TrackingArc; pub use data_arc::TrackingDataArc; -pub use range::RangeMsr; -pub use range_doppler::RangeDoppler; -pub use rangerate::RangeRate; +pub use measurement::Measurement; pub use types::MeasurementType; diff --git a/src/od/msr/range.rs b/src/od/msr/range.rs deleted file mode 100644 index 1828f61e..00000000 --- a/src/od/msr/range.rs +++ /dev/null @@ -1,131 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::{DimName, Matrix1x6, OVector, Vector1, U1, U6, U7}; -use crate::od::Measurement; -use crate::time::Epoch; -use crate::TimeTagged; -use arrow::datatypes::{DataType, Field}; -use hyperdual::linalg::norm; -use hyperdual::{hyperspace_from_vector, OHyperdual}; -use serde::ser::SerializeSeq; -use serde::{Serialize, Serializer}; -use std::collections::HashMap; - -/// Stores a standard measurement of range (km) -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeMsr { - pub dt: Epoch, - pub obs: Vector1, - visible: bool, - h_tilde: Matrix1x6, -} - -impl RangeMsr { - pub fn range(&self) -> f64 { - self.obs[(0, 0)] - } - - fn compute_sensitivity( - state: &OVector, U6>, - ) -> (Vector1, Matrix1x6) { - // Extract data from hyperspace - let range_vec = state.fixed_rows::<3>(0).into_owned(); - - // Code up math as usual - let range = norm(&range_vec); - - // Extract result into Vector2 and Matrix2x6 - let fx = Vector1::new(range.real()); - let mut pmat = Matrix1x6::zeros(); - - for j in 1..U7::dim() { - pmat[j - 1] = range[j]; - } - - (fx, pmat) - } - - pub fn new(tx: Orbit, rx: Orbit, visible: bool) -> RangeMsr { - assert_eq!(tx.frame, rx.frame, "tx and rx in different frames"); - assert_eq!(tx.epoch, rx.epoch, "tx and rx states have different times"); - - let dt = tx.epoch; - let hyperstate = - hyperspace_from_vector(&(rx.to_cartesian_pos_vel() - tx.to_cartesian_pos_vel())); - let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate); - - RangeMsr { - dt, - obs, - visible, - h_tilde, - } - } -} - -impl Measurement for RangeMsr { - type MeasurementSize = U1; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km - fn observation(&self) -> Vector1 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km".to_string()); - vec![Field::new("Range (km)", DataType::Float64, false).with_metadata(meta)] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { - dt: epoch, - obs, - visible: true, - h_tilde: Matrix1x6::zeros(), - } - } -} - -impl TimeTagged for RangeMsr { - fn epoch(&self) -> Epoch { - self.dt - } - - fn set_epoch(&mut self, dt: Epoch) { - self.dt = dt - } -} - -impl Serialize for RangeMsr { - /// Serializes the observation vector at the given time. - fn serialize(&self, serializer: S) -> Result - where - S: Serializer, - { - let mut seq = serializer.serialize_seq(Some(3))?; - seq.serialize_element(&self.dt.to_mjd_tai_days())?; - let obs = self.observation(); - seq.serialize_element(&obs[(0, 0)])?; - seq.end() - } -} diff --git a/src/od/msr/range_doppler.rs b/src/od/msr/range_doppler.rs deleted file mode 100644 index aa57f53b..00000000 --- a/src/od/msr/range_doppler.rs +++ /dev/null @@ -1,175 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, OMatrix, OVector, Vector2, U2}; -use crate::od::msr::RangeMsr; -use crate::od::{EstimateFrom, Measurement}; -use crate::{Spacecraft, TimeTagged}; -use anise::astro::AzElRange; -use arrow::datatypes::{DataType, Field}; -use hifitime::{Epoch, Unit}; -use std::collections::HashMap; - -/// A simultaneous range and Doppler measurement in units of km and km/s, available both in one way and two way measurement. -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeDoppler { - /// Epoch of the observation - pub epoch: Epoch, - /// Observation vector in km and km/s - pub obs: Vector2, -} - -impl RangeDoppler { - /// Initialize a new one-way range and Doppler measurement from the provided states and the effective noises. - /// - /// # Panics - /// + If the epochs of the two states differ. - /// + If the frames of the two states differ. - pub fn one_way( - aer: AzElRange, - timestamp_noise_s: f64, - range_noise_km: f64, - doppler_noise_km_s: f64, - ) -> Self { - Self { - epoch: aer.epoch + timestamp_noise_s * Unit::Second, - obs: Vector2::new( - aer.range_km + range_noise_km, - aer.range_rate_km_s + doppler_noise_km_s, - ), - } - } - - /// Initialize a new two-way range and Doppler measurement from the provided states as times t_1 and t_2 and the effective noises. - /// - /// The measurement is time-tagged at realization, i.e. at the end of the integration time (plus timestamp noise). - /// - /// # Noise - /// The measurements are not considered to be independent distributed variables. As such, the noises are reduced by a factor of sqrt(2). - /// - /// # Panics - /// + If the epochs of the two states differ. - /// + If the frames of the two states differ. - /// + If both epochs are identical. - pub fn two_way( - aer_t0: AzElRange, - aer_t1: AzElRange, - timestamp_noise_s: f64, - range_noise_km: f64, - doppler_noise_km_s: f64, - ) -> Self { - if aer_t0.epoch == aer_t1.epoch { - return Self::one_way( - aer_t1, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ); - } - - let range_km = (aer_t1.range_km + aer_t0.range_km) * 0.5; - let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; - - // Time tagged at the realization of this measurement, i.e. at the end of the integration time. - let epoch = aer_t1.epoch + timestamp_noise_s * Unit::Second; - - let obs = Vector2::new( - range_km + range_noise_km / 2.0_f64.sqrt(), - doppler_km_s + doppler_noise_km_s / 2.0_f64.sqrt(), - ); - - debug!( - "two way msr @ {epoch}:\naer_t0 = {}\naer_t1 = {}{obs}", - aer_t0, aer_t1 - ); - - Self { epoch, obs } - } -} - -impl TimeTagged for RangeDoppler { - fn epoch(&self) -> Epoch { - self.epoch - } - - fn set_epoch(&mut self, epoch: Epoch) { - self.epoch = epoch - } -} - -impl Measurement for RangeDoppler { - type MeasurementSize = U2; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km, km/s - fn observation(&self) -> Vector2 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km/s".to_string()); - - vec![ - RangeMsr::fields()[0].clone(), - Field::new("Doppler (km/s)", DataType::Float64, false).with_metadata(meta), - ] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { epoch, obs } - } -} - -impl EstimateFrom for Spacecraft { - fn extract(from: Spacecraft) -> Self { - from - } - - fn sensitivity( - msr: &RangeDoppler, - receiver: Self, - transmitter: Orbit, - ) -> OMatrix::MeasurementSize, Self::Size> - where - DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, - { - let delta_r = receiver.orbit.radius_km - transmitter.radius_km; - let delta_v = receiver.orbit.velocity_km_s - transmitter.velocity_km_s; - let ρ = msr.observation()[0]; - let ρ_dot = msr.observation()[1]; - let m11 = delta_r.x / ρ; - let m12 = delta_r.y / ρ; - let m13 = delta_r.z / ρ; - let m21 = delta_v.x / ρ - ρ_dot * delta_r.x / ρ.powi(2); - let m22 = delta_v.y / ρ - ρ_dot * delta_r.y / ρ.powi(2); - let m23 = delta_v.z / ρ - ρ_dot * delta_r.z / ρ.powi(2); - - let items = &[ - m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, m21, m22, m23, m11, m12, m13, 0.0, 0.0, - 0.0, - ]; - - OMatrix::::MeasurementSize, Self::Size>::from_row_slice( - items, - ) - } -} diff --git a/src/od/msr/rangerate.rs b/src/od/msr/rangerate.rs deleted file mode 100644 index e2fbaaa4..00000000 --- a/src/od/msr/rangerate.rs +++ /dev/null @@ -1,132 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::{DimName, Matrix1x6, OVector, Vector1, U1, U6, U7}; -use crate::od::Measurement; -use crate::time::Epoch; -use crate::TimeTagged; -use arrow::datatypes::{DataType, Field}; -use hyperdual::linalg::norm; -use hyperdual::{hyperspace_from_vector, OHyperdual}; -use serde::ser::SerializeSeq; -use serde::{Serialize, Serializer}; -use std::collections::HashMap; - -/// Stores a standard measurement of range (km) -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeRate { - pub dt: Epoch, - pub obs: Vector1, - visible: bool, - h_tilde: Matrix1x6, -} - -impl RangeRate { - pub fn range_rate(&self) -> f64 { - self.obs[(0, 0)] - } - - fn compute_sensitivity( - state: &OVector, U6>, - ) -> (Vector1, Matrix1x6) { - // Extract data from hyperspace - let range_vec = state.fixed_rows::<3>(0).into_owned(); - let velocity_vec = state.fixed_rows::<3>(3).into_owned(); - - // Code up math as usual - let delta_v_vec = velocity_vec / norm(&range_vec); - let range_rate = range_vec.dot(&delta_v_vec); - - // Extract result into Vector2 and Matrix2x6 - let fx = Vector1::new(range_rate.real()); - let mut pmat = Matrix1x6::zeros(); - - for j in 1..U7::dim() { - pmat[j - 1] = range_rate[j]; - } - (fx, pmat) - } - - pub fn new(tx: Orbit, rx: Orbit, visible: bool) -> RangeRate { - assert_eq!(tx.frame, rx.frame, "tx and rx in different frames"); - assert_eq!(tx.epoch, rx.epoch, "tx and rx states have different times"); - - let dt = tx.epoch; - let hyperstate = - hyperspace_from_vector(&(rx.to_cartesian_pos_vel() - tx.to_cartesian_pos_vel())); - let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate); - - RangeRate { - dt, - obs, - visible, - h_tilde, - } - } -} - -impl Measurement for RangeRate { - type MeasurementSize = U1; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km/s - fn observation(&self) -> Vector1 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km/s".to_string()); - vec![Field::new("Doppler (km/s)", DataType::Float64, false).with_metadata(meta)] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { - dt: epoch, - obs, - visible: true, - h_tilde: Matrix1x6::zeros(), - } - } -} - -impl TimeTagged for RangeRate { - fn epoch(&self) -> Epoch { - self.dt - } - - fn set_epoch(&mut self, dt: Epoch) { - self.dt = dt - } -} - -impl Serialize for RangeRate { - /// Serializes the observation vector at the given time. - fn serialize(&self, serializer: S) -> Result - where - S: Serializer, - { - let mut seq = serializer.serialize_seq(Some(3))?; - seq.serialize_element(&self.dt.to_mjd_tai_days())?; - let obs = self.observation(); - seq.serialize_element(&obs[(0, 0)])?; - seq.end() - } -} diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index f5ffc3e0..81ea7bfc 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -18,9 +18,11 @@ use crate::linalg::allocator::Allocator; use crate::linalg::DefaultAllocator; -use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDeviceSim}; +use crate::md::prelude::Interpolatable; +use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; +use indexmap::IndexSet; use nalgebra::{DimName, OMatrix, U1}; use snafu::ResultExt; use std::marker::PhantomData; @@ -46,7 +48,7 @@ where } /// Trait required to build a triplet of a solve-for state, a receiver, and a transmitter. -pub trait Sensitivity +pub trait TrackerSensitivity: TrackingDevice where Self: Sized, DefaultAllocator: Allocator @@ -57,9 +59,9 @@ where /// and S is the size of the state being solved for. fn h_tilde( &self, - msr_types: &[MeasurementType], // Consider switching to array + msr: &Measurement, + msr_types: &IndexSet, // Consider switching to array rx: &Rx, - tx: &Tx, almanac: Arc, ) -> Result, ODError> where @@ -78,7 +80,7 @@ where _tx: PhantomData, } -impl Sensitivity for Measurement +impl TrackerSensitivity for GroundStation where DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> @@ -86,9 +88,9 @@ where { fn h_tilde( &self, - msr_types: &[MeasurementType], + msr: &Measurement, + msr_types: &IndexSet, rx: &Spacecraft, - tx: &GroundStation, almanac: Arc, ) -> Result::Size>, ODError> where @@ -97,7 +99,7 @@ where // Rebuild each row of the scalar sensitivities. let mut mat = OMatrix::::Size>::zeros(); for (ith_row, msr_type) in msr_types.iter().enumerate() { - if !self.data.contains_key(msr_type) { + if !msr.data.contains_key(msr_type) { // Skip computation, this row is zero anyway. continue; } @@ -106,11 +108,10 @@ where Spacecraft, Spacecraft, GroundStation, - >>::new(*msr_type, self, rx, tx, almanac.clone())?; + >>::new(*msr_type, msr, rx, self, almanac.clone())?; mat.set_row(ith_row, &scalar_h.sensitivity_row); } - // ScalarSensitivity Ok(mat) } } diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index 3b40212d..f0e6d4f2 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -16,11 +16,14 @@ along with this program. If not, see . */ +use anise::astro::AzElRange; +use arrow::datatypes::{DataType, Field}; +use serde_derive::{Deserialize, Serialize}; use std::collections::HashMap; -use arrow::datatypes::{DataType, Field}; +use crate::od::ODError; -#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] +#[derive(Copy, Clone, Debug, Hash, Serialize, Deserialize, PartialEq, Eq)] pub enum MeasurementType { Range, Doppler, @@ -48,4 +51,31 @@ impl MeasurementType { ) .with_metadata(meta) } + + /// Computes the one way measurement from an AER object and the noise of this measurement type, returned in the units of this measurement type. + pub fn compute_one_way(self, aer: AzElRange, noise: f64) -> Result { + match self { + MeasurementType::Range => Ok(aer.range_km + noise), + MeasurementType::Doppler => Ok(aer.range_rate_km_s + noise), + } + } + + /// Computes the two way measurement from two AER values and the noise of this measurement type, returned in the units of this measurement type. + pub fn compute_two_way( + self, + aer_t0: AzElRange, + aer_t1: AzElRange, + noise: f64, + ) -> Result { + match self { + MeasurementType::Range => { + let range_km = (aer_t1.range_km + aer_t0.range_km) * 0.5; + Ok(range_km + noise / 2.0_f64.sqrt()) + } + MeasurementType::Doppler => { + let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; + Ok(doppler_km_s + noise / 2.0_f64.sqrt()) + } + } + } } diff --git a/src/od/process/export.rs b/src/od/process/export.rs index abff5ab0..979d8691 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -16,6 +16,7 @@ along with this program. If not, see . */ +use crate::dynamics::SpacecraftDynamics; use crate::io::watermark::pq_writer; use crate::io::{ArrowSnafu, ExportCfg, ParquetSnafu, StdIOSnafu}; use crate::linalg::allocator::Allocator; @@ -30,48 +31,43 @@ use arrow::datatypes::{DataType, Field, Schema}; use arrow::record_batch::RecordBatch; use filter::kalman::KF; use hifitime::TimeScale; +use msr::sensitivity::TrackerSensitivity; +use msr::TrackingDataArc; use nalgebra::Const; use parquet::arrow::ArrowWriter; use snafu::prelude::*; use std::collections::HashMap; use std::fs::File; -use std::ops::Add; use std::path::{Path, PathBuf}; use super::ODProcess; -impl<'a, D: Dynamics, Msr: Measurement, A: DimName> - ODProcess<'a, D, Msr, A, Spacecraft, KF> +impl<'a, MeasurementSize: DimName, A: DimName, T: TrackerSensitivity> + ODProcess<'a, SpacecraftDynamics, MeasurementSize, A, KF, T> where - D::StateType: - Interpolatable + Add::Size>, Output = D::StateType>, - ::VecLength>>::Buffer: Send, - DefaultAllocator: Allocator<::Size> - + Allocator - + Allocator::Size> - + Allocator, Msr::MeasurementSize> + DefaultAllocator: Allocator + + Allocator::Size> + + Allocator, MeasurementSize> + Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator - + Allocator::Size> - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, ::Size> - + Allocator<::VecLength> + + Allocator + + Allocator::Size> + + Allocator<::Size, MeasurementSize> + Allocator + Allocator - + Allocator<::Size, A> - + Allocator::Size> + Allocator<::Size> + Allocator<::VecLength> + Allocator<::Size, ::Size> + Allocator<::Size, A> + Allocator::Size>, - Spacecraft: EstimateFrom, { /// Store the estimates and residuals in a parquet file - pub fn to_parquet>(&self, path: P, cfg: ExportCfg) -> Result { + pub fn to_parquet>( + &self, + arc: &TrackingDataArc, + path: P, + cfg: ExportCfg, + ) -> Result { ensure!( !self.estimates.is_empty(), TooFewMeasurementsSnafu { @@ -221,25 +217,25 @@ where // Add the fields of the residuals let mut msr_fields = Vec::new(); - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Prefit residual: {}", f.name())), + .with_name(format!("Prefit residual: {f:?} ({})", f.unit())), ); } - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Postfit residual: {}", f.name())), + .with_name(format!("Postfit residual: {f:?} ({})", f.unit())), ); } - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Measurement noise: {}", f.name())), + .with_name(format!("Measurement noise: {f:?} ({})", f.unit())), ); } @@ -264,7 +260,7 @@ where .end_epoch .unwrap_or_else(|| self.estimates.last().unwrap().state().epoch()); - let mut residuals: Vec>> = + let mut residuals: Vec>> = Vec::with_capacity(self.residuals.len()); let mut estimates = Vec::with_capacity(self.estimates.len()); @@ -358,7 +354,7 @@ where // Finally, add the residuals. // Prefits - for i in 0..Msr::MeasurementSize::dim() { + for i in 0..MeasurementSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { @@ -370,7 +366,7 @@ where record.push(Arc::new(data.finish())); } // Postfit - for i in 0..Msr::MeasurementSize::dim() { + for i in 0..MeasurementSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { @@ -382,7 +378,7 @@ where record.push(Arc::new(data.finish())); } // Measurement noise - for i in 0..Msr::MeasurementSize::dim() { + for i in 0..MeasurementSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 6d75c4a9..a18ccbc0 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -26,58 +26,56 @@ pub use crate::od::*; use crate::propagators::PropInstance; pub use crate::time::{Duration, Unit}; use anise::prelude::Almanac; +use msr::sensitivity::TrackerSensitivity; use snafu::prelude::*; mod conf; pub use conf::{IterationConf, SmoothingArc}; mod trigger; pub use trigger::EkfTrigger; mod rejectcrit; -use self::msr::TrackingArc; +use self::msr::TrackingDataArc; pub use self::rejectcrit::ResidRejectCrit; use std::collections::BTreeMap; use std::marker::PhantomData; use std::ops::Add; mod export; +// Rename to simultaneous measurement or something like that. + /// An orbit determination process. Note that everything passed to this structure is moved. #[allow(clippy::upper_case_acronyms)] pub struct ODProcess< 'a, D: Dynamics, - Msr: Measurement, + MeasurementSize: DimName, A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, + K: Filter, + T: TrackerSensitivity, > where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> - + Allocator<::Size> - + Allocator<::VecLength> + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator<::Size, ::Size> + Allocator + Allocator + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator::Size>, { /// PropInstance used for the estimation pub prop: PropInstance<'a, D>, /// Kalman filter itself pub kf: K, + /// Tracking devices + pub devices: BTreeMap, /// Vector of estimates available after a pass pub estimates: Vec, /// Vector of residuals available after a pass - pub residuals: Vec>>, + pub residuals: Vec>>, pub ekf_trigger: Option, /// Residual rejection criteria allows preventing bad measurements from affecting the estimation. pub resid_crit: Option, @@ -89,39 +87,31 @@ pub struct ODProcess< impl< 'a, D: Dynamics, - Msr: Measurement, + MeasurementSize: DimName, A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, - > ODProcess<'a, D, Msr, A, S, K> + K: Filter, + T: TrackerSensitivity, + > ODProcess<'a, D, MeasurementSize, A, K, T> where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, ::Size> + Allocator<::VecLength> + + Allocator + + Allocator::Size> + + Allocator + + Allocator<::Size, ::Size> + Allocator + Allocator + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator::Size>, { /// Initialize a new orbit determination process with an optional trigger to switch from a CKF to an EKF. pub fn new( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, ekf_trigger: Option, resid_crit: Option, almanac: Arc, @@ -130,6 +120,7 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), ekf_trigger, @@ -144,6 +135,7 @@ where pub fn ekf( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, trigger: EkfTrigger, resid_crit: Option, almanac: Arc, @@ -152,6 +144,7 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), ekf_trigger: Some(trigger), @@ -273,18 +266,11 @@ where } /// Allows iterating on the filter solution. Requires specifying a smoothing condition to know where to stop the smoothing. - pub fn iterate( + pub fn iterate_arc( &mut self, - measurements: &[(String, Msr)], - devices: &mut BTreeMap, - step_size: Duration, + arc: &TrackingDataArc, config: IterationConf, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - // TODO: Add ExportCfg to iterate and to process so the data can be exported as we process it. Consider a thread writing with channel for faster serialization. - + ) -> Result<(), ODError> { let mut best_rms = self.rms_residual_ratios(); let mut previous_rms = best_rms; let mut divergence_cnt = 0; @@ -318,12 +304,12 @@ where // Reset the propagator self.prop.state = self.init_state; // Empty the estimates and add the first smoothed estimate as the initial estimate - self.estimates = Vec::with_capacity(measurements.len().max(self.estimates.len())); - self.residuals = Vec::with_capacity(measurements.len().max(self.estimates.len())); + self.estimates = Vec::with_capacity(arc.measurements.len().max(self.estimates.len())); + self.residuals = Vec::with_capacity(arc.measurements.len().max(self.estimates.len())); self.kf.set_previous_estimate(&smoothed[0]); // And re-run the filter - self.process::(measurements, devices, step_size)?; + self.process_arc(arc)?; // Compute the new RMS let new_rms = self.rms_residual_ratios(); @@ -407,53 +393,6 @@ where Ok(()) } - /// Allows iterating on the filter solution. Requires specifying a smoothing condition to know where to stop the smoothing. - pub fn iterate_arc( - &mut self, - arc: &TrackingArc, - config: IterationConf, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - let mut devices = arc.rebuild_devices::().context(ODConfigSnafu)?; - - let measurements = &arc.measurements; - let step_size = match arc.min_duration_sep() { - Some(step_size) => step_size, - None => { - return Err(ODError::TooFewMeasurements { - action: "determine the minimum step step", - need: 2, - }) - } - }; - - self.iterate(measurements, &mut devices, step_size, config) - } - - /// Process the provided tracking arc for this orbit determination process. - #[allow(clippy::erasing_op)] - pub fn process_arc(&mut self, arc: &TrackingArc) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - let mut devices = arc.rebuild_devices::().context(ODConfigSnafu)?; - - let measurements = &arc.measurements; - let step_size = match arc.min_duration_sep() { - Some(step_size) => step_size, - None => { - return Err(ODError::TooFewMeasurements { - action: "determining the minimum step size", - need: 2, - }) - } - }; - - self.process(measurements, &mut devices, step_size) - } - /// Process the provided measurements for this orbit determination process given the associated devices. /// /// # Argument details @@ -461,15 +400,8 @@ where /// + The name of all measurement devices must be present in the provided devices, i.e. the key set of `devices` must be a superset of the measurement device names present in the list. /// + The maximum step size to ensure we don't skip any measurements. #[allow(clippy::erasing_op)] - pub fn process( - &mut self, - measurements: &[(String, Msr)], - devices: &mut BTreeMap, - max_step: Duration, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { + pub fn process_arc(&mut self, arc: &TrackingDataArc) -> Result<(), ODError> { + let measurements = &arc.measurements; ensure!( measurements.len() >= 2, TooFewMeasurementsSnafu { @@ -478,12 +410,22 @@ where } ); + let max_step = match arc.min_duration_sep() { + Some(step_size) => step_size, + None => { + return Err(ODError::TooFewMeasurements { + action: "determining the minimum step size", + need: 2, + }) + } + }; + ensure!( !max_step.is_negative() && max_step != Duration::ZERO, StepSizeSnafu { step: max_step } ); - // Start by propagating the estimator (on the same thread). + // Start by propagating the estimator. let num_msrs = measurements.len(); // Update the step size of the navigation propagator if it isn't already fixed step @@ -491,7 +433,8 @@ where self.prop.set_step(max_step, false); } - let prop_time = measurements[num_msrs - 1].1.epoch() - self.kf.previous_estimate().epoch(); + // let prop_time = measurements[num_msrs - 1].1.epoch() - self.kf.previous_estimate().epoch(); + let prop_time = arc.end_epoch().unwrap() - self.kf.previous_estimate().epoch(); info!("Navigation propagating for a total of {prop_time} with step size {max_step}"); let mut epoch = self.prop.state.epoch(); @@ -501,23 +444,13 @@ where info!("Processing {num_msrs} measurements with covariance mapping"); // We'll build a trajectory of the estimated states. This will be used to compute the measurements. - let mut traj: Traj = Traj::new(); + let mut traj: Traj = Traj::new(); let mut msr_accepted_cnt: usize = 0; let tick = Epoch::now().unwrap(); - for (msr_cnt, (device_name, msr)) in measurements.iter().enumerate() { - let next_msr_epoch = msr.epoch(); - - for val in msr.observation().iter() { - ensure!( - val.is_finite(), - InvalidMeasurementSnafu { - epoch: next_msr_epoch, - val: *val - } - ); - } + for (msr_cnt, (epoch_ref, msr)) in measurements.iter().enumerate() { + let next_msr_epoch = *epoch_ref; // Advance the propagator loop { @@ -544,28 +477,28 @@ where .context(ODPropSnafu)?; for state in traj_covar.states { - traj.states.push(S::extract(state)); + // traj.states.push(S::extract(state)); + // HERE: At the time being, only spacecraft estimation is possible, and the trajectory will always be the exact state + // that was propagated. Even once ground station biases are estimated, these won't go through the propagator. + traj.states.push(state); } // Now that we've advanced the propagator, let's see whether we're at the time of the next measurement. // Extract the state and update the STM in the filter. - let nominal_state = S::extract(self.prop.state); + let nominal_state = self.prop.state; // Get the datetime and info needed to compute the theoretical measurement according to the model epoch = nominal_state.epoch(); // Perform a measurement update if nominal_state.epoch() == next_msr_epoch { // Get the computed observations - match devices.get_mut(device_name) { + match self.devices.get_mut(&msr.tracker) { Some(device) => { if let Some(computed_meas) = device.measure(epoch, &traj, None, self.almanac.clone())? { - // Grab the device location - let device_loc = device - .location(epoch, nominal_state.frame(), self.almanac.clone()) - .unwrap(); + let msr_types = device.measurement_types(); // Switch back from extended if necessary if let Some(trigger) = &mut self.ekf_trigger { @@ -575,15 +508,37 @@ where } } - let h_tilde = S::sensitivity(msr, nominal_state, device_loc); + // Check that the observation is valid. + for val in msr.observation::(msr_types).iter() { + ensure!( + val.is_finite(), + InvalidMeasurementSnafu { + epoch: next_msr_epoch, + val: *val + } + ); + } + + let h_tilde = device + .h_tilde::( + msr, + msr_types, + &nominal_state, + self.almanac.clone(), + ) + .unwrap(); self.kf.update_h_tilde(h_tilde); match self.kf.measurement_update( nominal_state, - &msr.observation(), - &computed_meas.observation(), - device.measurement_covar(epoch)?, + &msr.observation(msr_types), + &computed_meas.observation(msr_types), + device.measurement_covar_matrix( + device.measurement_types(), + epoch, + )?, + // device.measurement_covar(epoch)?, self.resid_crit, ) { Ok((estimate, mut residual)) => { @@ -625,11 +580,11 @@ where Err(e) => return Err(e), } } else { - warn!("Real observation exists @ {epoch} but simulated {device_name} does not see it -- ignoring measurement"); + warn!("Real observation exists @ {epoch} but simulated {} does not see it -- ignoring measurement", msr.tracker); } } None => { - error!("Tracking arc references {device_name} which is not in the list of configured devices") + error!("Tracking arc references {} which is not in the list of configured devices", msr.tracker) } } @@ -695,8 +650,7 @@ where // Perform time update // Extract the state and update the STM in the filter. - let prop_state = self.prop.state; - let nominal_state = S::extract(prop_state); + let nominal_state = self.prop.state; // Get the datetime and info needed to compute the theoretical measurement according to the model epoch = nominal_state.epoch(); // No measurement can be used here, let's just do a time update @@ -726,10 +680,9 @@ where } /// Builds the navigation trajectory for the estimated state only - pub fn to_traj(&self) -> Result, NyxError> + pub fn to_traj(&self) -> Result, NyxError> where - DefaultAllocator: Allocator<::VecLength>, - S: Interpolatable, + DefaultAllocator: Allocator<::VecLength>, { if self.estimates.is_empty() { Err(NyxError::NoStateData { @@ -751,38 +704,30 @@ where impl< 'a, D: Dynamics, - Msr: Measurement, + MeasurementSize: DimName, A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, - > ODProcess<'a, D, Msr, A, S, K> + K: Filter, + T: TrackerSensitivity, + > ODProcess<'a, D, MeasurementSize, A, K, T> where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator::Size> + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> + Allocator + Allocator + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator::Size>, { pub fn ckf( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, resid_crit: Option, almanac: Arc, ) -> Self { @@ -790,6 +735,7 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), resid_crit, diff --git a/src/od/simulator/arc.rs b/src/od/simulator/arc.rs index 3327c7e4..8097f672 100644 --- a/src/od/simulator/arc.rs +++ b/src/od/simulator/arc.rs @@ -25,13 +25,13 @@ use rand_pcg::Pcg64Mcg; use crate::dynamics::NyxError; use crate::io::ConfigError; use crate::md::trajectory::Interpolatable; -use crate::od::msr::{RangeDoppler, TrackingArc, TrackingDataArc}; +use crate::od::msr::TrackingDataArc; use crate::od::prelude::Strand; use crate::od::simulator::Cadence; -use crate::od::{GroundStation, Measurement}; +use crate::od::GroundStation; use crate::Spacecraft; use crate::State; -use crate::{linalg::allocator::Allocator, od::TrackingDeviceSim}; +use crate::{linalg::allocator::Allocator, od::TrackingDevice}; use crate::{linalg::DefaultAllocator, md::prelude::Traj}; use std::collections::BTreeMap; use std::fmt::Display; @@ -41,18 +41,14 @@ use std::sync::Arc; use super::{Handoff, TrkConfig}; #[derive(Clone)] -pub struct TrackingArcSim +pub struct TrackingArcSim where - D: TrackingDeviceSim, + D: TrackingDevice, MsrIn: State, - Msr: Measurement, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { /// Map of devices from their names. pub devices: BTreeMap, @@ -65,51 +61,41 @@ where /// Greatest common denominator time series that allows this arc to meet all of the conditions. time_series: TimeSeries, _msr_in: PhantomData, - _msr: PhantomData, } -impl TrackingArcSim +impl TrackingArcSim where - D: TrackingDeviceSim, + D: TrackingDevice, MsrIn: State, - Msr: Measurement, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { /// Build a new tracking arc simulator using the provided seeded random number generator. pub fn with_rng( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, rng: Pcg64Mcg, ) -> Result { // Check that each device has an associated configurations. // We don't care if there are more configurations than chosen devices. - let mut devices_map = BTreeMap::new(); let mut sampling_rates_ns = Vec::with_capacity(devices.len()); - for device in devices { - if let Some(cfg) = configs.get(&device.name()) { + for name in devices.keys() { + if let Some(cfg) = configs.get(name) { if let Err(e) = cfg.sanity_check() { - warn!("Ignoring device {}: {e}", device.name()); + warn!("Ignoring device {name}: {e}"); continue; } sampling_rates_ns.push(cfg.sampling.truncated_nanoseconds()); } else { - warn!( - "Ignoring device {}: no associated tracking configuration", - device.name() - ); + warn!("Ignoring device {name}: no associated tracking configuration",); continue; } - devices_map.insert(device.name(), device); } - if devices_map.is_empty() { + if devices.is_empty() { return Err(ConfigError::InvalidConfig { msg: "None of the devices are properly configured".to_string(), }); @@ -128,13 +114,12 @@ where ); let me = Self { - devices: devices_map, + devices, trajectory, configs, rng, time_series, _msr_in: PhantomData, - _msr: PhantomData, }; info!("{me}"); @@ -144,7 +129,7 @@ where /// Build a new tracking arc simulator using the provided seed to initialize the random number generator. pub fn with_seed( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, seed: u64, @@ -156,7 +141,7 @@ where /// Build a new tracking arc simulator using the system entropy to seed the random number generator. pub fn new( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, ) -> Result { @@ -182,98 +167,6 @@ where pub fn generate_measurements( &mut self, almanac: Arc, - ) -> Result, NyxError> { - let mut measurements = Vec::new(); - - for (name, device) in self.devices.iter_mut() { - let cfg = &self.configs[name]; - if cfg.scheduler.is_some() { - if cfg.strands.is_none() { - return Err(NyxError::CustomError { - msg: format!( - "schedule for {name} must be built before generating measurements" - ), - }); - } else { - warn!("scheduler for {name} is ignored, using the defined tracking strands instead") - } - } - - let init_msr_count = measurements.len(); - let tick = Epoch::now().unwrap(); - - match cfg.strands.as_ref() { - Some(strands) => { - // Strands are defined at this point - 'strands: for (ii, strand) in strands.iter().enumerate() { - // Build the time series for this strand, sampling at the correct rate - for epoch in TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) { - match device.measure( - epoch, - &self.trajectory, - Some(&mut self.rng), - almanac.clone(), - ) { - Ok(msr_opt) => { - if let Some(msr) = msr_opt { - measurements.push((name.clone(), msr)); - } - } - Err(e) => { - if epoch != strand.end { - warn!( - "Skipping the remaining strand #{ii} ending on {}: {e}", - strand.end - ); - } - continue 'strands; - } - } - } - } - - info!( - "Simulated {} measurements for {name} for {} tracking strands in {}", - measurements.len() - init_msr_count, - strands.len(), - (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) - ); - } - None => { - warn!("No tracking strands defined for {name}, skipping"); - } - } - } - - // Reorder the measurements - measurements.sort_by_key(|(_name, msr)| msr.epoch()); - - // Build the tracking arc. - let trk = TrackingArc { - device_cfg: serde_yaml::to_string(&self.devices).unwrap(), - measurements, - }; - - Ok(trk) - } - - /// Generates measurements for the tracking arc using the defined strands - /// - /// # Warning - /// This function will return an error if any of the devices defines as a scheduler. - /// You must create the schedule first using `build_schedule` first. - /// - /// # Notes - /// Although mutable, this function may be called several times to generate different measurements. - /// - /// # Algorithm - /// For each tracking device, and for each strand within that device, sample the trajectory at the sample - /// rate of the tracking device, adding a measurement whenever the spacecraft is visible. - /// Build the measurements as a vector, ordered chronologically. - /// - pub fn simulate_measurements( - &mut self, - almanac: Arc, ) -> Result { let mut measurements = BTreeMap::new(); @@ -300,7 +193,7 @@ where 'strands: for (ii, strand) in strands.iter().enumerate() { // Build the time series for this strand, sampling at the correct rate for epoch in TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) { - match device.measure_new( + match device.measure( epoch, &self.trajectory, Some(&mut self.rng), @@ -347,17 +240,13 @@ where } } -impl Display for TrackingArcSim +impl Display for TrackingArcSim where - D: TrackingDeviceSim, - Msr: Measurement, + D: TrackingDevice, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { write!( @@ -371,7 +260,7 @@ where } // Literally the same as above, but can't make it generic =( -impl TrackingArcSim { +impl TrackingArcSim { /// Builds the schedule provided the config. Requires the tracker to be a ground station. /// /// # Algorithm diff --git a/src/od/simulator/mod.rs b/src/od/simulator/mod.rs index 2e294fc4..453f2a92 100644 --- a/src/od/simulator/mod.rs +++ b/src/od/simulator/mod.rs @@ -23,6 +23,6 @@ pub use arc::TrackingArcSim; mod scheduler; pub use scheduler::{Cadence, Handoff, Scheduler}; mod trackdata; -pub use trackdata::TrackingDeviceSim; +pub use trackdata::TrackingDevice; mod trkconfig; pub use trkconfig::{Strand, TrkConfig}; diff --git a/src/od/simulator/trackdata.rs b/src/od/simulator/trackdata.rs index 24d6857b..d7699adf 100644 --- a/src/od/simulator/trackdata.rs +++ b/src/od/simulator/trackdata.rs @@ -21,32 +21,33 @@ use std::sync::Arc; use anise::almanac::Almanac; use anise::errors::AlmanacResult; use hifitime::Epoch; +use indexmap::IndexSet; +use nalgebra::{DimName, OMatrix}; use rand_pcg::Pcg64Mcg; use crate::io::ConfigRepr; use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, OMatrix}; +use crate::linalg::DefaultAllocator; use crate::md::prelude::{Frame, Traj}; use crate::md::trajectory::Interpolatable; use crate::od::msr::measurement::Measurement as NewMeasurement; use crate::od::msr::MeasurementType; -use crate::od::{Measurement, ODError}; +use crate::od::ODError; use crate::Orbit; /// Tracking device simulator. -pub trait TrackingDeviceSim: ConfigRepr +pub trait TrackingDevice: ConfigRepr where MsrIn: Interpolatable, - Msr: Measurement, - DefaultAllocator: Allocator - + Allocator - + Allocator - + Allocator - + Allocator, + DefaultAllocator: + Allocator + Allocator + Allocator, { /// Returns the name of this tracking data simulator fn name(&self) -> String; + /// Returns the _enabled_ measurement types for thie device. + fn measurement_types(&self) -> &IndexSet; + /// Performs a measurement of the input trajectory at the provided epoch (with integration times if relevant), and returns a measurement from itself to the input state. Returns None of the object is not visible. /// This trait function takes in a trajectory and epoch so it can properly simulate integration times for the measurements. /// If the random number generator is provided, it shall be used to add noise to the measurement. @@ -63,7 +64,7 @@ where traj: &Traj, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError>; + ) -> Result, ODError>; /// Returns the device location at the given epoch and in the given frame. fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult; @@ -74,26 +75,28 @@ where rx: MsrIn, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError>; - - // Return the noise statistics of this tracking device at the requested epoch. - fn measurement_covar( - &mut self, - epoch: Epoch, - ) -> Result, ODError>; - - fn measure_new( - &mut self, - epoch: Epoch, - traj: &Traj, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, ) -> Result, ODError>; // Return the noise statistics of this tracking device for the provided measurement type at the requested epoch. - fn measurement_covar_new( + fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result; + + fn measurement_covar_matrix( &self, - msr_type: MeasurementType, + msr_types: &IndexSet, epoch: Epoch, - ) -> Result; + ) -> Result, ODError> + where + DefaultAllocator: Allocator, + { + // Rebuild the R matrix of the measurement noise. + let mut r_mat = OMatrix::::zeros(); + + for (i, msr_type) in msr_types.iter().enumerate() { + if self.measurement_types().contains(msr_type) { + r_mat[(i, i)] = self.measurement_covar(*msr_type, epoch)?; + } + } + + Ok(r_mat) + } } diff --git a/tests/mission_design/multishoot/mod.rs b/tests/mission_design/multishoot/mod.rs index a6d70b9b..6f25831d 100644 --- a/tests/mission_design/multishoot/mod.rs +++ b/tests/mission_design/multishoot/mod.rs @@ -408,7 +408,7 @@ fn vmag_orbit_raising(almanac: Arc) { .enumerate() { traj.to_parquet_with_step( - &format!("multishoot_to_node_{}.parquet", i), + format!("multishoot_to_node_{}.parquet", i), 2 * Unit::Second, almanac.clone(), ) diff --git a/tests/mission_design/targeter/b_plane.rs b/tests/mission_design/targeter/b_plane.rs index 51435b57..c1358895 100644 --- a/tests/mission_design/targeter/b_plane.rs +++ b/tests/mission_design/targeter/b_plane.rs @@ -1,11 +1,8 @@ extern crate nyx_space as nyx; -use anise::constants::celestial_objects::JUPITER_BARYCENTER; -use anise::constants::celestial_objects::MOON; -use anise::constants::celestial_objects::SUN; +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}; use anise::constants::frames::MOON_J2000; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; diff --git a/tests/mission_design/targeter/finite_burns.rs b/tests/mission_design/targeter/finite_burns.rs index 8ef6f91f..58f29652 100644 --- a/tests/mission_design/targeter/finite_burns.rs +++ b/tests/mission_design/targeter/finite_burns.rs @@ -5,7 +5,6 @@ use hifitime::TimeUnits; use nyx::dynamics::guidance::{LocalFrame, Mnvr, Thruster}; use nyx::linalg::Vector3; use nyx::md::prelude::*; -use nyx::md::targeter::*; use crate::propagation::GMAT_EARTH_GM; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; diff --git a/tests/mission_design/targeter/mod.rs b/tests/mission_design/targeter/mod.rs index 2f88163d..06936ac9 100644 --- a/tests/mission_design/targeter/mod.rs +++ b/tests/mission_design/targeter/mod.rs @@ -4,6 +4,4 @@ mod b_plane; mod finite_burns; mod multi_oe; mod multi_oe_vnc; -#[cfg(feature = "broken-donotuse")] -mod opti_levenberg; mod single_oe; diff --git a/tests/mission_design/targeter/multi_oe.rs b/tests/mission_design/targeter/multi_oe.rs index ac5b11fd..cff458f4 100644 --- a/tests/mission_design/targeter/multi_oe.rs +++ b/tests/mission_design/targeter/multi_oe.rs @@ -1,9 +1,7 @@ extern crate nyx_space as nyx; -// use nyx::dynamics::guidance::Mnvr; use nyx::dynamics::guidance::Thruster; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; diff --git a/tests/mission_design/targeter/multi_oe_vnc.rs b/tests/mission_design/targeter/multi_oe_vnc.rs index 7397b602..ea170589 100644 --- a/tests/mission_design/targeter/multi_oe_vnc.rs +++ b/tests/mission_design/targeter/multi_oe_vnc.rs @@ -1,7 +1,6 @@ extern crate nyx_space as nyx; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; diff --git a/tests/mission_design/targeter/single_oe.rs b/tests/mission_design/targeter/single_oe.rs index 33603a63..b356a441 100644 --- a/tests/mission_design/targeter/single_oe.rs +++ b/tests/mission_design/targeter/single_oe.rs @@ -1,10 +1,7 @@ extern crate nyx_space as nyx; -use anise::constants::celestial_objects::JUPITER_BARYCENTER; -use anise::constants::celestial_objects::MOON; -use anise::constants::celestial_objects::SUN; +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; diff --git a/tests/monte_carlo/framework.rs b/tests/monte_carlo/framework.rs index 6455829e..27089646 100644 --- a/tests/monte_carlo/framework.rs +++ b/tests/monte_carlo/framework.rs @@ -33,7 +33,7 @@ fn test_monte_carlo_epoch(almanac: Arc) { // 5% error on SMA and 5% on Eccentricity let nominal_state = Spacecraft::from(state); - let random_state = MultivariateNormal::new( + let random_state = MvnSpacecraft::new( nominal_state, vec![ StateDispersion::zero_mean(StateParameter::SMA, 0.05), diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index e2df05ec..7a71d58e 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -3,10 +3,10 @@ extern crate nyx_space as nyx; use anise::constants::celestial_objects::{EARTH, MOON, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; +use indexmap::IndexSet; use nalgebra::U2; use nyx::cosmic::Orbit; use nyx::dynamics::SpacecraftDynamics; -use nyx::od::msr::measurement::Measurement as NewMeasurement; use nyx::od::prelude::*; use nyx::time::Epoch; use nyx::{dynamics::OrbitalDynamics, propagators::Propagator}; @@ -16,8 +16,6 @@ use rand_pcg::Pcg64Mcg; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; -use sensitivity::Sensitivity; -use std::collections::HashMap; use std::sync::Arc; #[fixture] @@ -50,6 +48,7 @@ fn nil_measurement(almanac: Arc) { doppler_noise_km_s: Some(StochasticNoise::MIN), integration_time: None, light_time_correction: false, + ..Default::default() }; let at_station = Orbit::try_latlongalt( @@ -85,6 +84,10 @@ fn val_measurements_topo(almanac: Arc) { use self::nyx::od::prelude::*; use std::str::FromStr; + let mut msr_types = IndexSet::new(); + msr_types.insert(MeasurementType::Range); + msr_types.insert(MeasurementType::Doppler); + let cislunar1 = Orbit::cartesian( -6252.59501113, 1728.23921802, @@ -198,7 +201,7 @@ fn val_measurements_topo(almanac: Arc) { .unwrap() .unwrap(); - let obs = meas.observation(); + let obs = meas.observation::(&msr_types); println!( "range difference {:e}\t range rate difference: {:e}", (obs[0] - truth.range).abs(), @@ -247,38 +250,7 @@ fn val_measurements_topo(almanac: Arc) { .unwrap() { traj2_msr_cnt += 1; - let exp_h = Spacecraft::sensitivity( - &msr, - state, - dss65_madrid - .location(state.epoch(), state.orbit.frame, almanac.clone()) - .unwrap(), - ); - - // Rebuild the measurement and recompute the sensitivity. - let mut data = HashMap::new(); - data.insert(MeasurementType::Range, msr.observation()[0]); - data.insert(MeasurementType::Doppler, msr.observation()[1]); - let n_msr = NewMeasurement { - epoch: state.epoch(), - tracker: "DSS 65".to_string(), - data, - }; - let got_h = n_msr - .h_tilde::( - &[MeasurementType::Range, MeasurementType::Doppler], - &state, - &dss65_madrid, - almanac.clone(), - ) - .unwrap(); - let err = exp_h - got_h; - if err.norm() > 0.0 { - println!("ERR = {:.3e}", exp_h - got_h); - println!("EXP = {exp_h:.6}"); - println!("GOT = {got_h:.6}"); - panic!(); - } + println!("{msr}"); } } @@ -296,7 +268,7 @@ fn val_measurements_topo(almanac: Arc) { .measure(state.epoch(), &traj2, Some(&mut rng), almanac.clone()) .unwrap() .unwrap(); - let obs = meas.observation(); + let obs = meas.observation::(&msr_types); println!( "range difference {:e}\t range rate difference: {:e}", (obs[0] - truth.range).abs(), diff --git a/tests/orbit_determination/multi_body.rs b/tests/orbit_determination/multi_body.rs index 0da5273c..6d0904c1 100644 --- a/tests/orbit_determination/multi_body.rs +++ b/tests/orbit_determination/multi_body.rs @@ -4,6 +4,7 @@ use anise::constants::celestial_objects::JUPITER_BARYCENTER; use anise::constants::celestial_objects::MOON; use anise::constants::celestial_objects::SUN; use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::U2; use nyx::od::simulator::TrackingArcSim; use nyx::od::simulator::TrkConfig; use nyx_space::propagators::IntegratorMethod; @@ -26,7 +27,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -48,12 +49,17 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -75,29 +81,29 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices } #[allow(clippy::identity_op)] #[rstest] fn od_val_multi_body_ckf_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); // Define the tracking configurations let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert( - device.name.clone(), - TrkConfig::from_sample_rate(10.seconds()), - ); + for name in sim_devices.keys() { + configs.insert(name.clone(), TrkConfig::from_sample_rate(10.seconds())); } - let all_stations = sim_devices; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -120,19 +126,12 @@ fn od_val_multi_body_ckf_perfect_stations( let (final_truth, traj) = prop.for_duration_with_traj(prop_time).unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.to_parquet_simple("multi_body.parquet").unwrap(); - let new_arc = arc_sim.simulate_measurements(almanac.clone()).unwrap(); - new_arc.to_parquet_simple("multi_body_new.parquet").unwrap(); - let new_arc_reloaded = TrackingDataArc::from_parquet("multi_body_new.parquet").unwrap(); - - assert_eq!(new_arc_reloaded, new_arc); - // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on // the measurements, and the same time step. @@ -156,9 +155,9 @@ fn od_val_multi_body_ckf_perfect_stations( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::<_, RangeDoppler, _, _, _>::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let mut last_est = None; for (no, est) in odp.estimates.iter().enumerate() { @@ -211,13 +210,13 @@ fn od_val_multi_body_ckf_perfect_stations( #[rstest] fn multi_body_ckf_covar_map( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + mut sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // For this test, we're only enabling one station so we can check that the covariance inflates between visibility passes. let _ = pretty_env_logger::try_init(); - let dss13_goldstone = sim_devices[2].clone(); + let dss13_goldstone = sim_devices.get("Goldstone").unwrap(); // Define the tracking configurations let mut configs = BTreeMap::new(); @@ -229,7 +228,9 @@ fn multi_body_ckf_covar_map( .build(), ); - let all_stations = vec![dss13_goldstone]; + // Remove all but Goldstone + sim_devices.remove("Canberra").unwrap(); + sim_devices.remove("Madrid").unwrap(); // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -254,12 +255,10 @@ fn multi_body_ckf_covar_map( let (_, traj) = prop.for_duration_with_traj(prop_time).unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(vec![proc_devices[2].clone()], configs) - .unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -284,9 +283,10 @@ fn multi_body_ckf_covar_map( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac.clone()); + let mut odp = + ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac.clone()); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let mut num_pred = 0_u32; for est in odp.estimates.iter() { diff --git a/tests/orbit_determination/resid_reject.rs b/tests/orbit_determination/resid_reject.rs index b6a4b076..9589da10 100644 --- a/tests/orbit_determination/resid_reject.rs +++ b/tests/orbit_determination/resid_reject.rs @@ -1,5 +1,6 @@ use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; use anise::constants::frames::{EARTH_J2000, IAU_EARTH_FRAME}; +use nalgebra::U2; use nyx_space::dynamics::guidance::LocalFrame; use pretty_env_logger::try_init; @@ -64,7 +65,7 @@ fn traj(epoch: Epoch, almanac: Arc) -> Traj { fn devices_n_configs( epoch: Epoch, almanac: Arc, -) -> (Vec, BTreeMap) { +) -> (BTreeMap, BTreeMap) { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; @@ -107,15 +108,19 @@ fn devices_n_configs( .build(), ); - (vec![dss65_madrid, dss34_canberra], configs) + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + + (devices, configs) } #[fixture] fn tracking_arc( traj: Traj, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, -) -> TrackingArc { +) -> TrackingDataArc { let (devices, configs) = devices_n_configs; // Simulate tracking data @@ -177,9 +182,9 @@ fn initial_estimate(traj: Traj) -> KfEstimate { #[rstest] fn od_resid_reject_inflated_snc_ckf_two_way( traj: Traj, - tracking_arc: TrackingArc, + tracking_arc: TrackingDataArc, initial_estimate: KfEstimate, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, ) { let (devices, _configs) = devices_n_configs; @@ -207,29 +212,15 @@ fn od_resid_reject_inflated_snc_ckf_two_way( // the measurements are accepted. // So we end up with an excellent estimate but an unusably high covariance. - let mut odp = ODProcess::ckf( + let mut odp = ODProcess::<_, U2, _, _, _>::ckf( prop_est, kf, + devices, Some(ResidRejectCrit { num_sigmas: 3.0 }), almanac, ); - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we're stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the hashmap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); - - odp.process( - &tracking_arc.measurements, - &mut devices_map, - tracking_arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&tracking_arc).unwrap(); let path: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -238,7 +229,9 @@ fn od_resid_reject_inflated_snc_ckf_two_way( ] .iter() .collect(); - odp.to_parquet(path, ExportCfg::timestamped()).unwrap(); + + odp.to_parquet(&tracking_arc, path, ExportCfg::timestamped()) + .unwrap(); let num_rejections = odp .residuals @@ -293,9 +286,9 @@ fn od_resid_reject_inflated_snc_ckf_two_way( #[rstest] fn od_resid_reject_default_ckf_two_way( traj: Traj, - tracking_arc: TrackingArc, + tracking_arc: TrackingDataArc, initial_estimate: KfEstimate, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, ) { let (devices, _configs) = devices_n_configs; @@ -319,24 +312,15 @@ fn od_resid_reject_default_ckf_two_way( let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, kf, Some(ResidRejectCrit::default()), almanac); - - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we're stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the hashmap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf( + prop_est, + kf, + devices, + Some(ResidRejectCrit::default()), + almanac, + ); - odp.process( - &tracking_arc.measurements, - &mut devices_map, - tracking_arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&tracking_arc).unwrap(); // Save this result before the asserts for analysis let path: PathBuf = [ @@ -346,7 +330,8 @@ fn od_resid_reject_default_ckf_two_way( ] .iter() .collect(); - odp.to_parquet(path, ExportCfg::timestamped()).unwrap(); + odp.to_parquet(&tracking_arc, path, ExportCfg::timestamped()) + .unwrap(); let num_rejections = odp .residuals diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 119ff775..d532e5b3 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -75,7 +75,9 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { ]); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. - let all_stations = vec![dss65_madrid, dss34_canberra]; + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -131,7 +133,7 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -162,15 +164,14 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - // Let"s filter and iterate on the initial subset of the arc to refine the initial estimate - let subset = arc.filter_by_offset(..3.hours()); + // Let's filter and iterate on the initial subset of the arc to refine the initial estimate + let subset = arc.clone().filter_by_offset(..3.hours()); let remaining = arc.filter_by_offset(3.hours()..); - odp.process_arc::(&subset).unwrap(); - odp.iterate_arc::(&subset, IterationConf::once()) - .unwrap(); + odp.process_arc(&subset).unwrap(); + odp.iterate_arc(&subset, IterationConf::once()).unwrap(); let (sm_rss_pos_km, sm_rss_vel_km_s) = rss_orbit_errors(&initial_state.orbit, &odp.estimates[0].state().orbit); @@ -182,9 +183,10 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { (initial_state.orbit - odp.estimates[0].state().orbit).unwrap() ); - odp.process_arc::(&remaining).unwrap(); + odp.process_arc(&remaining).unwrap(); odp.to_parquet( + &remaining, path.with_file_name("robustness_test_one_way.parquet"), ExportCfg::timestamped(), ) @@ -280,7 +282,10 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { ]); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. - let devices = vec![dss65_madrid, dss34_canberra]; + // Note that we do not have Goldstone so we can test enabling and disabling the EKF. + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); // Disperse the initial state based on some orbital elements errors given from ULA Atlas 5 user guide, table 2.3.3-1 // This assumes that the errors are ONE TENTH of the values given in the table. It assumes that the launch provider has provided an initial state vector, whose error is lower than the injection errors. @@ -353,32 +358,18 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); - - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we"re stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the BTreeMap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); // Check that exporting an empty set returns an error. assert!(odp .to_parquet( + &arc, path.with_file_name("robustness_test_two_way.parquet"), ExportCfg::timestamped(), ) .is_err()); - odp.process( - &arc.measurements, - &mut devices_map, - arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&arc).unwrap(); let mut num_residual_none = 0; let mut num_residual_some = 0; @@ -392,6 +383,7 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { // Export as Parquet let timestamped_path = odp .to_parquet( + &arc, path.with_file_name("robustness_test_two_way.parquet"), ExportCfg::timestamped(), ) diff --git a/tests/orbit_determination/simulator.rs b/tests/orbit_determination/simulator.rs index 0abe7346..0d1c9bf9 100644 --- a/tests/orbit_determination/simulator.rs +++ b/tests/orbit_determination/simulator.rs @@ -1,8 +1,6 @@ -use nyx_space::io::tracking_data::DynamicTrackingArc; use nyx_space::io::ConfigRepr; use nyx_space::md::prelude::*; use nyx_space::md::trajectory::ExportCfg; -use nyx_space::od::msr::RangeDoppler; use nyx_space::od::prelude::*; use nyx_space::od::simulator::TrackingArcSim; use nyx_space::od::simulator::TrkConfig; @@ -78,7 +76,7 @@ fn continuous_tracking(almanac: Arc) { .iter() .collect(); - let devices = GroundStation::load_many(ground_station_file).unwrap(); + let devices = GroundStation::load_named(ground_station_file).unwrap(); // dbg!(&devices); @@ -98,10 +96,9 @@ fn continuous_tracking(almanac: Arc) { dbg!(&configs); // Build the tracking arc simulation to generate a "standard measurement". - let mut trk = TrackingArcSim::::with_seed( - devices, trajectory, configs, 12345, - ) - .unwrap(); + let mut trk = + TrackingArcSim::::with_seed(devices, trajectory, configs, 12345) + .unwrap(); trk.build_schedule(almanac.clone()).unwrap(); let arc = trk.generate_measurements(almanac).unwrap(); @@ -119,17 +116,12 @@ fn continuous_tracking(almanac: Arc) { println!("[{}] {arc}", output_fn.to_string_lossy()); // Now read this file back in. - let dyn_arc = DynamicTrackingArc::from_parquet(output_fn).unwrap(); - // And convert to the same tracking arc as earlier - let arc_concrete = dyn_arc.to_tracking_arc::().unwrap(); + let arc_concrete = TrackingDataArc::from_parquet(output_fn).unwrap(); println!("{arc_concrete}"); assert_eq!(arc.measurements.len(), 116); // Check that we've loaded all of the measurements assert_eq!(arc_concrete.measurements.len(), arc.measurements.len()); - // Check that we find the same device names too - assert_eq!(arc_concrete.device_names(), arc.device_names()); - // Check that we've copied over the device configurations as well - assert_eq!(arc_concrete.device_cfg, arc.device_cfg); + assert_eq!(arc_concrete.unique(), arc.unique()); } diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index 778b9162..4584059d 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -29,7 +29,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -51,12 +51,16 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -78,15 +82,19 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + devices } #[allow(clippy::identity_op)] #[rstest] fn od_val_sc_mb_srp_reals_duals_models( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { /* * This tests that the state transition matrix computation is correct when multiple celestial gravities and solar radiation pressure @@ -116,12 +124,10 @@ fn od_val_sc_mb_srp_reals_duals_models( }]) .build(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } - let all_stations = sim_devices; - // Define the propagator information. let step_size = 10.0 * Unit::Second; let opts = IntegratorOptions::with_fixed_step(step_size); @@ -178,11 +184,10 @@ fn od_val_sc_mb_srp_reals_duals_models( .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.to_parquet_simple(path.with_file_name("sc_msr_arc.parquet")) .unwrap(); @@ -214,11 +219,12 @@ fn od_val_sc_mb_srp_reals_duals_models( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); odp.to_parquet( + &arc, path.with_file_name("spacecraft_od_results.parquet"), ExportCfg::timestamped(), ) @@ -283,8 +289,8 @@ fn od_val_sc_mb_srp_reals_duals_models( #[rstest] fn od_val_sc_srp_estimation( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { /* * This tests that we can correctly estimate the solar radiation pressure. @@ -357,8 +363,8 @@ fn od_val_sc_srp_estimation( }]) .build(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -368,8 +374,7 @@ fn od_val_sc_srp_estimation( TrackingArcSim::with_seed(all_stations, traj.clone(), configs.clone(), 120).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.to_parquet_simple(path.with_file_name("sc_srp_msr_arc.parquet")) .unwrap(); @@ -407,17 +412,19 @@ fn od_val_sc_srp_estimation( SNC3::from_diagonal(2 * Unit::Minute, &[1e-15, 1e-15, 1e-15]), ); - let mut odp = ODProcess::ekf( + let mut odp = SpacecraftODProcess::ekf( prop_est, ckf, + proc_devices, EkfTrigger::new(30, Unit::Minute * 2), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); odp.to_parquet( + &arc, path.with_file_name("sc_od_with_srp.parquet"), ExportCfg::default(), ) diff --git a/tests/orbit_determination/trackingarc.rs b/tests/orbit_determination/trackingarc.rs index ef0d6860..fd189f29 100644 --- a/tests/orbit_determination/trackingarc.rs +++ b/tests/orbit_determination/trackingarc.rs @@ -1,8 +1,6 @@ use anise::constants::frames::{EARTH_J2000, IAU_EARTH_FRAME}; -use nyx_space::io::tracking_data::DynamicTrackingArc; use nyx_space::io::ConfigRepr; use nyx_space::md::prelude::*; -use nyx_space::od::msr::RangeDoppler; use nyx_space::od::prelude::*; use nyx_space::od::simulator::TrackingArcSim; use nyx_space::od::simulator::{Cadence, Strand, TrkConfig}; @@ -47,7 +45,7 @@ fn traj(almanac: Arc) -> Traj { } #[fixture] -fn devices() -> Vec { +fn devices() -> BTreeMap { // Load the ground stations from the test data. let ground_station_file: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -59,11 +57,15 @@ fn devices() -> Vec { .iter() .collect(); - GroundStation::load_many(ground_station_file).unwrap() + GroundStation::load_named(ground_station_file).unwrap() } #[rstest] -fn trk_simple(traj: Traj, devices: Vec, almanac: Arc) { +fn trk_simple( + traj: Traj, + devices: BTreeMap, + almanac: Arc, +) { // Path to output data let path: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -104,7 +106,7 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< // Build the tracking arc simulation to generate a "standard measurement". let mut trk = - TrackingArcSim::::with_seed(devices, traj, configs, 12345) + TrackingArcSim::::with_seed(devices, traj, configs, 12345) .unwrap(); // Test that building the schedule is deterministic @@ -121,47 +123,6 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< let arc = trk.generate_measurements(almanac).unwrap(); - // Test filtering by epoch - let start_epoch = arc.measurements[0].1.epoch() + 1.minutes(); - for (_, msr) in arc.filter_by_epoch(start_epoch..).measurements { - assert!(msr.epoch() >= start_epoch); - } - - for (_, msr) in arc.filter_by_epoch(..=start_epoch).measurements { - assert!(msr.epoch() <= start_epoch); - } - - for (_, msr) in arc.filter_by_epoch(..start_epoch).measurements { - assert!(msr.epoch() < start_epoch); - } - - assert_eq!( - arc.filter_by_epoch(start_epoch..start_epoch) - .measurements - .len(), - 0 - ); - - // Test filtering by duration offset - for (_, msr) in arc.filter_by_offset(1.minutes()..).measurements { - assert!(msr.epoch() >= start_epoch); - } - - for (_, msr) in arc.filter_by_offset(..=1.minutes()).measurements { - assert!(msr.epoch() <= start_epoch); - } - - for (_, msr) in arc.filter_by_offset(..1.minutes()).measurements { - assert!(msr.epoch() < start_epoch); - } - - assert_eq!( - arc.filter_by_offset(1.minutes()..1.minutes()) - .measurements - .len(), - 0 - ); - // Regression assert_eq!(arc.measurements.len(), 197); @@ -178,25 +139,20 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< println!("[{}] {arc}", output_fn.to_string_lossy()); // Now read this file back in. - let dyn_arc = DynamicTrackingArc::from_parquet(output_fn).unwrap(); - // And convert to the same tracking arc as earlier - let arc_concrete = dyn_arc.to_tracking_arc::().unwrap(); + let arc_concrete = TrackingDataArc::from_parquet(output_fn).unwrap(); println!("{arc_concrete}"); // Check that we've loaded all of the measurements assert_eq!(arc_concrete.measurements.len(), arc.measurements.len()); - // Check that we find the same device names too - assert_eq!(arc_concrete.device_names(), arc.device_names()); - // Check that we've copied over the device configurations as well - assert_eq!(arc_concrete.device_cfg, arc.device_cfg); + assert_eq!(arc_concrete.unique(), arc.unique()); } /// Tests that inclusion epochs work #[rstest] fn trkconfig_zero_inclusion( traj: Traj, - devices: Vec, + devices: BTreeMap, almanac: Arc, ) { // Build a tracking config that should always see this vehicle. @@ -209,10 +165,9 @@ fn trkconfig_zero_inclusion( // Build the configs map, where we only have one of the two stations configured let mut configs = BTreeMap::new(); - configs.insert(devices[1].name.clone(), trkcfg_always); + configs.insert("Canberra".to_string(), trkcfg_always); - let mut trk = - TrackingArcSim::::new(devices, traj, configs).unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); @@ -222,7 +177,7 @@ fn trkconfig_zero_inclusion( assert_eq!(arc.measurements.len(), 113); assert_eq!( - arc.device_names().len(), + arc.unique_aliases().len(), 1, "only one device should have measurements" ); @@ -230,7 +185,7 @@ fn trkconfig_zero_inclusion( /// Test invalid tracking configurations #[rstest] -fn trkconfig_invalid(traj: Traj, devices: Vec) { +fn trkconfig_invalid(traj: Traj, devices: BTreeMap) { // Build a tracking config where the exclusion range is less than the sampling rate let trkcfg = TrkConfig::builder() .strands(vec![Strand { @@ -241,18 +196,18 @@ fn trkconfig_invalid(traj: Traj, devices: Vec) { // Build the configs map let mut configs = BTreeMap::new(); - for device in &devices { - configs.insert(device.name.clone(), trkcfg.clone()); + for name in devices.keys() { + configs.insert(name.clone(), trkcfg.clone()); } - assert!(TrackingArcSim::::new(devices, traj, configs).is_err()); + assert!(TrackingArcSim::::new(devices, traj, configs).is_err()); } /// Test a delayed start of the configuration #[rstest] fn trkconfig_delayed_start( traj: Traj, - devices: Vec, + mut devices: BTreeMap, almanac: Arc, ) { let trkcfg = TrkConfig::builder() @@ -263,14 +218,13 @@ fn trkconfig_delayed_start( .sampling(1.26.minutes()) .build(); + devices.remove("Canberra").unwrap(); + // Build the configs map with a single ground station let mut configs = BTreeMap::new(); + configs.insert("Demo Ground Station".to_string(), trkcfg); - configs.insert(devices[0].name.clone(), trkcfg); - - let mut trk = - TrackingArcSim::::new(vec![devices[0].clone()], traj, configs) - .unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); @@ -289,12 +243,16 @@ fn trkconfig_delayed_start( /// Test different cadences and availabilities #[rstest] -fn trkconfig_cadence(traj: Traj, devices: Vec, almanac: Arc) { +fn trkconfig_cadence( + traj: Traj, + devices: BTreeMap, + almanac: Arc, +) { // Build the configs map with a single ground station let mut configs = BTreeMap::new(); configs.insert( - devices[0].name.clone(), + "Demo Ground Station".to_string(), TrkConfig::builder() .scheduler( Scheduler::builder() @@ -308,15 +266,14 @@ fn trkconfig_cadence(traj: Traj, devices: Vec, almana ); configs.insert( - devices[1].name.clone(), + "Canberra".to_string(), TrkConfig::builder() .sampling(26.1.seconds()) .scheduler(Scheduler::default()) .build(), ); - let mut trk = - TrackingArcSim::::new(devices, traj, configs).unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index fed2091c..75148874 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -2,6 +2,7 @@ extern crate nyx_space as nyx; extern crate pretty_env_logger; use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::U2; use nyx::cosmic::Orbit; use nyx::dynamics::orbital::OrbitalDynamics; use nyx::dynamics::sph_harmonics::Harmonics; @@ -28,7 +29,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -50,12 +51,17 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -77,15 +83,20 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + + devices } #[allow(clippy::identity_op)] #[rstest] fn od_tb_val_ekf_fixed_step_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -108,8 +119,8 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let cfg = TrkConfig::load(trkconfig_yaml).unwrap(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -136,8 +147,7 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -163,15 +173,16 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ekf( + let mut odp = ODProcess::<_, U2, _, _, _>::ekf( prop_est, kf, + proc_devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates[odp.estimates.len() - 1]; @@ -221,8 +232,8 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( #[rstest] fn od_tb_val_with_arc( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -283,8 +294,7 @@ fn od_tb_val_with_arc( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 1).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // And serialize to disk let path: PathBuf = [ @@ -321,15 +331,16 @@ fn od_tb_val_with_arc( let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ekf( + let mut odp = ODProcess::<_, U2, _, _, _>::ekf( prop_est, kf, + proc_devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), Some(ResidRejectCrit::default()), almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates[odp.estimates.len() - 1]; @@ -379,8 +390,8 @@ fn od_tb_val_with_arc( #[rstest] fn od_tb_val_ckf_fixed_step_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { /* * This tests that the state transition matrix computation is correct with two body dynamics. @@ -404,8 +415,8 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( .build(); let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -432,8 +443,7 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // And serialize to disk let path: PathBuf = [ @@ -471,15 +481,15 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "tb_ckf.parquet"] .iter() .collect(); - odp.to_parquet(path, ExportCfg::default()).unwrap(); + odp.to_parquet(&arc, path, ExportCfg::default()).unwrap(); // Check that there are no duplicates of epochs. let mut prev_epoch = odp.estimates[0].epoch(); @@ -550,7 +560,7 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); // Iterate - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), @@ -602,8 +612,8 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( #[rstest] fn od_tb_ckf_fixed_step_iteration_test( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -615,8 +625,8 @@ fn od_tb_ckf_fixed_step_iteration_test( let cfg = TrkConfig::from_sample_rate(10.seconds()); let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -641,8 +651,7 @@ fn od_tb_ckf_fixed_step_iteration_test( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -670,9 +679,9 @@ fn od_tb_ckf_fixed_step_iteration_test( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check the final estimate prior to iteration let delta = (odp.estimates.last().unwrap().state().orbit - final_truth.orbit).unwrap(); @@ -692,7 +701,7 @@ fn od_tb_ckf_fixed_step_iteration_test( ); // Iterate, and check that the initial state difference is lower - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), @@ -743,8 +752,8 @@ fn od_tb_ckf_fixed_step_iteration_test( #[rstest] fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -752,8 +761,8 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -778,8 +787,7 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -810,9 +818,9 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( let ckf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { @@ -902,7 +910,8 @@ fn od_tb_ckf_map_covar(almanac: Arc) { let ckf = KF::no_snc(initial_estimate); - let mut odp: SpacecraftODProcess = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp: SpacecraftODProcess = + ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, BTreeMap::new(), None, almanac); odp.predict_for(30.seconds(), duration).unwrap(); @@ -936,8 +945,8 @@ fn od_tb_ckf_map_covar(almanac: Arc) { #[rstest] fn od_tb_val_harmonics_ckf_fixed_step_perfect( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -945,8 +954,8 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -974,8 +983,7 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -1002,9 +1010,9 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { @@ -1044,8 +1052,8 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( #[rstest] fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -1053,8 +1061,8 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -1078,8 +1086,7 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -1119,9 +1126,9 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( let ckf = KF::with_sncs(initial_estimate, vec![process_noise1, process_noise2]); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { diff --git a/tests/orbit_determination/xhat_dev.rs b/tests/orbit_determination/xhat_dev.rs index cb8ad9a9..1490d3a6 100644 --- a/tests/orbit_determination/xhat_dev.rs +++ b/tests/orbit_determination/xhat_dev.rs @@ -33,41 +33,46 @@ fn almanac() -> Arc { * The latter would require iteration and smoothing before playing with an EKF. This will be handled in a subsequent version. **/ -#[allow(clippy::identity_op)] -#[rstest] -fn xhat_dev_test_ekf_two_body(almanac: Arc) { - let _ = pretty_env_logger::try_init(); - +#[fixture] +fn devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ); let dss34_canberra = GroundStation::dss34_canberra( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ); + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices +} + +#[allow(clippy::identity_op)] +#[rstest] +fn xhat_dev_test_ekf_two_body(almanac: Arc, devices: BTreeMap) { + let _ = pretty_env_logger::try_init(); + // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 0.01 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -99,7 +104,7 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { // Simulate tracking data println!("{traj}"); - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -133,12 +138,12 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let pre_smooth_first_est = odp.estimates[0]; let pre_smooth_num_est = odp.estimates.len(); - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::All, @@ -256,46 +261,29 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_multi_body(almanac: Arc) { +fn xhat_dev_test_ekf_multi_body(almanac: Arc, devices: BTreeMap) { // We seed both propagators with the same initial state, but we let a large state deviation in the filter. // This does _not_ impact the prefits, but it impacts the state deviation and therefore the state estimate. // As such, it checks that the filter can return to a nominal state. let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 500; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 10.0 * Unit::Second; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -328,7 +316,7 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -362,10 +350,10 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); - odp.iterate_arc::(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) + odp.process_arc(&arc).unwrap(); + odp.iterate_arc(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) .unwrap(); // Check that the covariance deflated @@ -429,43 +417,26 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_harmonics(almanac: Arc) { +fn xhat_dev_test_ekf_harmonics(almanac: Arc, devices: BTreeMap) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 5000; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 1 * Unit::Minute; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -508,7 +479,7 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -542,9 +513,9 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates.last().unwrap(); @@ -592,43 +563,26 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_realistic(almanac: Arc) { +fn xhat_dev_test_ekf_realistic(almanac: Arc, devices: BTreeMap) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 500; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 10.0 * Unit::Second; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -658,7 +612,7 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -692,9 +646,9 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates.last().unwrap(); @@ -749,37 +703,23 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { +fn xhat_dev_test_ckf_smoother_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -812,7 +752,7 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -841,9 +781,9 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Smoother let smoothed_estimates = odp.smooth(SmoothingArc::All).unwrap(); @@ -1019,37 +959,23 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { +fn xhat_dev_test_ekf_snc_smoother_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 10.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -1082,7 +1008,7 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -1118,15 +1044,16 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ekf( + let mut odp = SpacecraftODProcess::ekf( prop_est, kf, + devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Smoother let smoothed_estimates = odp.smooth(SmoothingArc::All).unwrap(); @@ -1290,37 +1217,23 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { +fn xhat_dev_test_ckf_iteration_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -1353,7 +1266,7 @@ fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -1382,15 +1295,15 @@ fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Clone the initial estimates let pre_iteration_estimates = odp.estimates.clone(); // Iterate - odp.iterate_arc::(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) + odp.iterate_arc(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) .unwrap(); let mut rss_pos_avr = 0.0; From 3aa4fb8e93f0835bc40ccb588c8023e6c8b0ae27 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 17:36:34 -0700 Subject: [PATCH 09/38] Add specific tests to the coverage report --- tests/mission_design/multishoot/mod.rs | 2 +- tests/mission_design/targeter/finite_burns.rs | 2 +- tests/mission_design/targeter/multi_oe.rs | 2 +- tests/mission_design/targeter/multi_oe_vnc.rs | 4 ++-- tests/mission_design/targeter/single_oe.rs | 2 +- tests/orbit_determination/multi_body.rs | 2 +- tests/orbit_determination/resid_reject.rs | 2 +- tests/orbit_determination/robust.rs | 2 +- tests/orbit_determination/spacecraft.rs | 2 +- tests/orbit_determination/trackingarc.rs | 4 ++-- tests/propagation/stopcond.rs | 2 +- tests/propagation/trajectory.rs | 2 +- tests/propulsion/closedloop_multi_oe_ruggiero.rs | 4 ++-- tests/propulsion/schedule.rs | 4 ++-- 14 files changed, 18 insertions(+), 18 deletions(-) diff --git a/tests/mission_design/multishoot/mod.rs b/tests/mission_design/multishoot/mod.rs index 6f25831d..c7511a56 100644 --- a/tests/mission_design/multishoot/mod.rs +++ b/tests/mission_design/multishoot/mod.rs @@ -23,7 +23,7 @@ fn almanac() -> Arc { } #[rstest] -fn alt_orbit_raising(almanac: Arc) { +fn alt_orbit_raising_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); diff --git a/tests/mission_design/targeter/finite_burns.rs b/tests/mission_design/targeter/finite_burns.rs index 58f29652..e330e3d0 100644 --- a/tests/mission_design/targeter/finite_burns.rs +++ b/tests/mission_design/targeter/finite_burns.rs @@ -114,7 +114,7 @@ fn thrust_dir_rate_tgt_sma_aop_raan(almanac: Arc) { #[ignore] #[rstest] -fn thrust_profile_tgt_sma_aop_raan(almanac: Arc) { +fn thrust_profile_tgt_sma_aop_raan_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/multi_oe.rs b/tests/mission_design/targeter/multi_oe.rs index cff458f4..c9229fcc 100644 --- a/tests/mission_design/targeter/multi_oe.rs +++ b/tests/mission_design/targeter/multi_oe.rs @@ -185,7 +185,7 @@ fn conv_tgt_sma_ecc(almanac: Arc) { } #[rstest] -fn tgt_hd_sma_ecc(almanac: Arc) { +fn tgt_hd_sma_ecc_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/multi_oe_vnc.rs b/tests/mission_design/targeter/multi_oe_vnc.rs index ea170589..3632b3f3 100644 --- a/tests/mission_design/targeter/multi_oe_vnc.rs +++ b/tests/mission_design/targeter/multi_oe_vnc.rs @@ -13,7 +13,7 @@ fn almanac() -> Arc { } #[rstest] -fn tgt_vnc_c3_decl(almanac: Arc) { +fn tgt_vnc_c3_decl_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); @@ -61,7 +61,7 @@ fn tgt_vnc_c3_decl(almanac: Arc) { } #[rstest] -fn tgt_vnc_sma_ecc(almanac: Arc) { +fn tgt_vnc_sma_ecc_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/single_oe.rs b/tests/mission_design/targeter/single_oe.rs index b356a441..8c73c590 100644 --- a/tests/mission_design/targeter/single_oe.rs +++ b/tests/mission_design/targeter/single_oe.rs @@ -570,7 +570,7 @@ fn tgt_aop_from_apo(almanac: Arc) { } #[rstest] -fn tgt_aop_from_peri(almanac: Arc) { +fn tgt_aop_from_peri_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/orbit_determination/multi_body.rs b/tests/orbit_determination/multi_body.rs index 6d0904c1..a9b8e1c9 100644 --- a/tests/orbit_determination/multi_body.rs +++ b/tests/orbit_determination/multi_body.rs @@ -208,7 +208,7 @@ fn od_val_multi_body_ckf_perfect_stations( #[allow(clippy::identity_op)] #[rstest] -fn multi_body_ckf_covar_map( +fn multi_body_ckf_covar_map_cov_test( almanac: Arc, mut sim_devices: BTreeMap, proc_devices: BTreeMap, diff --git a/tests/orbit_determination/resid_reject.rs b/tests/orbit_determination/resid_reject.rs index 9589da10..ed7ed806 100644 --- a/tests/orbit_determination/resid_reject.rs +++ b/tests/orbit_determination/resid_reject.rs @@ -284,7 +284,7 @@ fn od_resid_reject_inflated_snc_ckf_two_way( } #[rstest] -fn od_resid_reject_default_ckf_two_way( +fn od_resid_reject_default_ckf_two_way_cov_test( traj: Traj, tracking_arc: TrackingDataArc, initial_estimate: KfEstimate, diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index d532e5b3..665bcbc6 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -239,7 +239,7 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { +fn od_robust_test_ekf_realistic_two_way_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index 4584059d..2e368b4d 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -287,7 +287,7 @@ fn od_val_sc_mb_srp_reals_duals_models( #[allow(clippy::identity_op)] #[rstest] -fn od_val_sc_srp_estimation( +fn od_val_sc_srp_estimation_cov_test( almanac: Arc, sim_devices: BTreeMap, proc_devices: BTreeMap, diff --git a/tests/orbit_determination/trackingarc.rs b/tests/orbit_determination/trackingarc.rs index fd189f29..d47bab5f 100644 --- a/tests/orbit_determination/trackingarc.rs +++ b/tests/orbit_determination/trackingarc.rs @@ -205,7 +205,7 @@ fn trkconfig_invalid(traj: Traj, devices: BTreeMap, mut devices: BTreeMap, almanac: Arc, @@ -243,7 +243,7 @@ fn trkconfig_delayed_start( /// Test different cadences and availabilities #[rstest] -fn trkconfig_cadence( +fn trkconfig_cadence_cov_test( traj: Traj, devices: BTreeMap, almanac: Arc, diff --git a/tests/propagation/stopcond.rs b/tests/propagation/stopcond.rs index c11a7364..a832cdfd 100644 --- a/tests/propagation/stopcond.rs +++ b/tests/propagation/stopcond.rs @@ -28,7 +28,7 @@ fn almanac() -> Arc { } #[rstest] -fn stop_cond_3rd_apo(almanac: Arc) { +fn stop_cond_3rd_apo_cov_test(almanac: Arc) { let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); let start_dt = Epoch::from_mjd_tai(JD_J2000); diff --git a/tests/propagation/trajectory.rs b/tests/propagation/trajectory.rs index 1e3f01de..b5a95b03 100644 --- a/tests/propagation/trajectory.rs +++ b/tests/propagation/trajectory.rs @@ -428,7 +428,7 @@ fn traj_spacecraft(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn traj_ephem_backward(almanac: Arc) { +fn traj_ephem_backward_cov_test(almanac: Arc) { // Test that we can correctly interpolate a spacecraft orbit let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/propulsion/closedloop_multi_oe_ruggiero.rs b/tests/propulsion/closedloop_multi_oe_ruggiero.rs index 042dc8df..63374d9d 100644 --- a/tests/propulsion/closedloop_multi_oe_ruggiero.rs +++ b/tests/propulsion/closedloop_multi_oe_ruggiero.rs @@ -95,7 +95,7 @@ fn qlaw_as_ruggiero_case_a(almanac: Arc) { } #[rstest] -fn qlaw_as_ruggiero_case_b(almanac: Arc) { +fn qlaw_as_ruggiero_case_b_cov_test(almanac: Arc) { // Source: AAS-2004-5089 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); @@ -400,7 +400,7 @@ fn qlaw_as_ruggiero_case_f(almanac: Arc) { } #[rstest] -fn ruggiero_iepc_2011_102(almanac: Arc) { +fn ruggiero_iepc_2011_102_cov_test(almanac: Arc) { // Source: IEPC 2011 102 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/propulsion/schedule.rs b/tests/propulsion/schedule.rs index 5e2aa1d8..b276917c 100644 --- a/tests/propulsion/schedule.rs +++ b/tests/propulsion/schedule.rs @@ -116,7 +116,7 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { } #[rstest] -fn val_transfer_schedule_depl(almanac: Arc) { +fn val_transfer_schedule_depl_cov_test(almanac: Arc) { let eme2k = almanac .frame_from_uid(EARTH_J2000) .unwrap() @@ -242,7 +242,7 @@ fn val_transfer_schedule_depl(almanac: Arc) { } #[rstest] -fn val_transfer_single_maneuver_depl(almanac: Arc) { +fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { /* This is the same test as val_transfer_schedule_depl but uses the maneuver directly as the guidance law. It should work in the same way. */ let eme2k = almanac From 52a416306c3c1c4c2ef8917bee411aad8cd61263 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 17:59:27 -0700 Subject: [PATCH 10/38] Move ground station to its own module for maintainability --- data/tests/config/one_ground_station.yaml | 3 +- src/od/ground_station/builtin.rs | 96 ++++++++ src/od/ground_station/event.rs | 75 ++++++ src/od/ground_station/mod.rs | 278 +--------------------- src/od/ground_station/trk_device.rs | 164 +++++++++++++ 5 files changed, 343 insertions(+), 273 deletions(-) create mode 100644 src/od/ground_station/builtin.rs create mode 100644 src/od/ground_station/event.rs create mode 100644 src/od/ground_station/trk_device.rs diff --git a/data/tests/config/one_ground_station.yaml b/data/tests/config/one_ground_station.yaml index 8522ee2e..28f544fb 100644 --- a/data/tests/config/one_ground_station.yaml +++ b/data/tests/config/one_ground_station.yaml @@ -19,4 +19,5 @@ doppler_noise_km_s: light_time_correction: false measurement_types: - Range - - Doppler \ No newline at end of file + - Doppler +integration_time: 1 min \ No newline at end of file diff --git a/src/od/ground_station/builtin.rs b/src/od/ground_station/builtin.rs new file mode 100644 index 00000000..0fb08abc --- /dev/null +++ b/src/od/ground_station/builtin.rs @@ -0,0 +1,96 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::*; + +impl GroundStation { + pub fn dss65_madrid( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + Self { + name: "Madrid".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: 40.427_222, + longitude_deg: 4.250_556, + height_km: 0.834_939, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + range_noise_km: Some(range_noise_km), + doppler_noise_km_s: Some(doppler_noise_km_s), + } + } + + pub fn dss34_canberra( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + Self { + name: "Canberra".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: -35.398_333, + longitude_deg: 148.981_944, + height_km: 0.691_750, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + range_noise_km: Some(range_noise_km), + doppler_noise_km_s: Some(doppler_noise_km_s), + } + } + + pub fn dss13_goldstone( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + Self { + name: "Goldstone".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: 35.247_164, + longitude_deg: 243.205, + height_km: 1.071_149_04, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + range_noise_km: Some(range_noise_km), + doppler_noise_km_s: Some(doppler_noise_km_s), + } + } +} diff --git a/src/od/ground_station/event.rs b/src/od/ground_station/event.rs new file mode 100644 index 00000000..9a790708 --- /dev/null +++ b/src/od/ground_station/event.rs @@ -0,0 +1,75 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::GroundStation; +use crate::md::EventEvaluator; +use crate::{errors::EventError, md::prelude::Interpolatable}; +use anise::prelude::Almanac; +use hifitime::{Duration, Unit}; +use nalgebra::{allocator::Allocator, DefaultAllocator}; +use std::sync::Arc; + +impl EventEvaluator for &GroundStation +where + DefaultAllocator: Allocator + Allocator + Allocator, +{ + /// Compute the elevation in the SEZ frame. This call will panic if the frame of the input state does not match that of the ground station. + fn eval(&self, rx_gs_frame: &S, almanac: Arc) -> Result { + let dt = rx_gs_frame.epoch(); + // Then, compute the rotation matrix from the body fixed frame of the ground station to its topocentric frame SEZ. + let tx_gs_frame = self.to_orbit(dt, &almanac).unwrap(); + + let from = tx_gs_frame.frame.orientation_id * 1_000 + 1; + let dcm_topo2fixed = tx_gs_frame + .dcm_from_topocentric_to_body_fixed(from) + .unwrap() + .transpose(); + + // Now, rotate the spacecraft in the SEZ frame to compute its elevation as seen from the ground station. + // We transpose the DCM so that it's the fixed to topocentric rotation. + let rx_sez = (dcm_topo2fixed * rx_gs_frame.orbit()).unwrap(); + let tx_sez = (dcm_topo2fixed * tx_gs_frame).unwrap(); + // Now, let's compute the range ρ. + let rho_sez = (rx_sez - tx_sez).unwrap(); + + // Finally, compute the elevation (math is the same as declination) + // Source: Vallado, section 4.4.3 + // Only the sine is needed as per Vallado, and the formula is the same as the declination + // because we're in the SEZ frame. + Ok(rho_sez.declination_deg() - self.elevation_mask_deg) + } + + fn eval_string(&self, state: &S, almanac: Arc) -> Result { + Ok(format!( + "Elevation from {} is {:.6} deg on {}", + self.name, + self.eval(state, almanac)? + self.elevation_mask_deg, + state.epoch() + )) + } + + /// Epoch precision of the election evaluator is 1 ms + fn epoch_precision(&self) -> Duration { + 1 * Unit::Second + } + + /// Angle precision of the elevation evaluator is 1 millidegree. + fn value_precision(&self) -> f64 { + 1e-3 + } +} diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index 1b585cd0..cd6dedfe 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -22,23 +22,19 @@ use anise::errors::AlmanacResult; use anise::prelude::{Almanac, Frame, Orbit}; use indexmap::IndexSet; -use super::msr::measurement::Measurement; use super::msr::MeasurementType; use super::noise::StochasticNoise; use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; -use crate::errors::EventError; use crate::io::ConfigRepr; -use crate::md::prelude::{Interpolatable, Traj}; -use crate::md::EventEvaluator; use crate::time::Epoch; -use crate::Spacecraft; -use hifitime::{Duration, TimeUnits, Unit}; -use nalgebra::{allocator::Allocator, DefaultAllocator}; +use hifitime::Duration; use rand_pcg::Pcg64Mcg; use serde_derive::{Deserialize, Serialize}; -use snafu::ResultExt; use std::fmt; -use std::sync::Arc; + +pub mod builtin; +pub mod event; +pub mod trk_device; #[cfg(feature = "python")] use pyo3::prelude::*; @@ -60,9 +56,6 @@ pub struct GroundStation { pub frame: Frame, pub measurement_types: IndexSet, /// Duration needed to generate a measurement (if unset, it is assumed to be instantaneous) - /// TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - /// Without it, we're stuck having to rebuild them from scratch. - /// https://github.com/nyx-space/nyx/issues/140 pub integration_time: Option, /// Whether to correct for light travel time pub light_time_correction: bool, @@ -100,81 +93,6 @@ impl GroundStation { } } - pub fn dss65_madrid( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - let mut measurement_types = IndexSet::new(); - measurement_types.insert(MeasurementType::Range); - measurement_types.insert(MeasurementType::Doppler); - Self { - name: "Madrid".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: 40.427_222, - longitude_deg: 4.250_556, - height_km: 0.834_939, - frame: iau_earth, - measurement_types, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - - pub fn dss34_canberra( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - let mut measurement_types = IndexSet::new(); - measurement_types.insert(MeasurementType::Range); - measurement_types.insert(MeasurementType::Doppler); - Self { - name: "Canberra".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: -35.398_333, - longitude_deg: 148.981_944, - height_km: 0.691_750, - frame: iau_earth, - measurement_types, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - - pub fn dss13_goldstone( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - let mut measurement_types = IndexSet::new(); - measurement_types.insert(MeasurementType::Range); - measurement_types.insert(MeasurementType::Doppler); - Self { - name: "Goldstone".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: 35.247_164, - longitude_deg: 243.205, - height_km: 1.071_149_04, - frame: iau_earth, - measurement_types, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. /// This is a shortcut to almanac.azimuth_elevation_range_sez. pub fn azimuth_elevation_of( @@ -262,140 +180,6 @@ impl Default for GroundStation { impl ConfigRepr for GroundStation {} -impl TrackingDevice for GroundStation { - fn measurement_types(&self) -> &IndexSet { - &self.measurement_types - } - - /// Perform a measurement from the ground station to the receiver (rx). - fn measure( - &mut self, - epoch: Epoch, - traj: &Traj, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, - ) -> Result, ODError> { - match self.integration_time { - Some(integration_time) => { - // If out of traj bounds, return None. - let rx_0 = traj.at(epoch - integration_time).context(ODTrajSnafu)?; - let rx_1 = traj.at(epoch).context(ODTrajSnafu)?; - - let obstructing_body = if !self.frame.ephem_origin_match(rx_0.frame()) { - Some(rx_0.frame()) - } else { - None - }; - - let aer_t0 = self - .azimuth_elevation_of(rx_0.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - let aer_t1 = self - .azimuth_elevation_of(rx_1.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - - if aer_t0.elevation_deg < self.elevation_mask_deg - || aer_t1.elevation_deg < self.elevation_mask_deg - { - debug!( - "{} (el. mask {:.3} deg) but object moves from {:.3} to {:.3} deg -- no measurement", - self.name, self.elevation_mask_deg, aer_t0.elevation_deg, aer_t1.elevation_deg - ); - return Ok(None); - } - - // Noises are computed at the midpoint of the integration time. - let noises = self.noises(epoch - integration_time * 0.5, rng)?; - - let mut msr = Measurement::new(self.name.clone(), epoch + noises[0].seconds()); - - for (ii, msr_type) in self.measurement_types.iter().enumerate() { - let msr_value = msr_type.compute_two_way(aer_t0, aer_t1, noises[ii + 1])?; - msr.push(*msr_type, msr_value); - } - - Ok(Some(msr)) - } - None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac), - } - } - - fn name(&self) -> String { - self.name.clone() - } - - fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult { - almanac.transform_to(self.to_orbit(epoch, &almanac).unwrap(), frame, None) - } - - fn measure_instantaneous( - &mut self, - rx: Spacecraft, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, - ) -> Result, ODError> { - let obstructing_body = if !self.frame.ephem_origin_match(rx.frame()) { - Some(rx.frame()) - } else { - None - }; - - let aer = self - .azimuth_elevation_of(rx.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - - if aer.elevation_deg >= self.elevation_mask_deg { - // Only update the noises if the measurement is valid. - let noises = self.noises(rx.orbit.epoch, rng)?; - - let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch + noises[0].seconds()); - - for (ii, msr_type) in self.measurement_types.iter().enumerate() { - let msr_value = msr_type.compute_one_way(aer, noises[ii + 1])?; - msr.push(*msr_type, msr_value); - } - - Ok(Some(msr)) - } else { - debug!( - "{} {} (el. mask {:.3} deg), object at {:.3} deg -- no measurement", - self.name, rx.orbit.epoch, self.elevation_mask_deg, aer.elevation_deg - ); - Ok(None) - } - } - - /// Returns the measurement noise of this ground station. - /// - /// # Methodology - /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise. - /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is - /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The - /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. - fn measurement_covar( - &self, - msr_type: super::prelude::MeasurementType, - epoch: Epoch, - ) -> Result { - Ok(match msr_type { - super::msr::MeasurementType::Range => self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .covariance(epoch), - super::msr::MeasurementType::Doppler => self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .covariance(epoch), - }) - } -} - impl fmt::Display for GroundStation { // Prints the Keplerian orbital elements with units fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { @@ -411,56 +195,6 @@ impl fmt::Display for GroundStation { } } -impl EventEvaluator for &GroundStation -where - DefaultAllocator: Allocator + Allocator + Allocator, -{ - /// Compute the elevation in the SEZ frame. This call will panic if the frame of the input state does not match that of the ground station. - fn eval(&self, rx_gs_frame: &S, almanac: Arc) -> Result { - let dt = rx_gs_frame.epoch(); - // Then, compute the rotation matrix from the body fixed frame of the ground station to its topocentric frame SEZ. - let tx_gs_frame = self.to_orbit(dt, &almanac).unwrap(); - - let from = tx_gs_frame.frame.orientation_id * 1_000 + 1; - let dcm_topo2fixed = tx_gs_frame - .dcm_from_topocentric_to_body_fixed(from) - .unwrap() - .transpose(); - - // Now, rotate the spacecraft in the SEZ frame to compute its elevation as seen from the ground station. - // We transpose the DCM so that it's the fixed to topocentric rotation. - let rx_sez = (dcm_topo2fixed * rx_gs_frame.orbit()).unwrap(); - let tx_sez = (dcm_topo2fixed * tx_gs_frame).unwrap(); - // Now, let's compute the range ρ. - let rho_sez = (rx_sez - tx_sez).unwrap(); - - // Finally, compute the elevation (math is the same as declination) - // Source: Vallado, section 4.4.3 - // Only the sine is needed as per Vallado, and the formula is the same as the declination - // because we're in the SEZ frame. - Ok(rho_sez.declination_deg() - self.elevation_mask_deg) - } - - fn eval_string(&self, state: &S, almanac: Arc) -> Result { - Ok(format!( - "Elevation from {} is {:.6} deg on {}", - self.name, - self.eval(state, almanac)? + self.elevation_mask_deg, - state.epoch() - )) - } - - /// Epoch precision of the election evaluator is 1 ms - fn epoch_precision(&self) -> Duration { - 1 * Unit::Second - } - - /// Angle precision of the elevation evaluator is 1 millidegree. - fn value_precision(&self) -> f64 { - 1e-3 - } -} - #[cfg(test)] mod gs_ut { @@ -516,7 +250,7 @@ mod gs_ut { height_km: 0.4, light_time_correction: false, timestamp_noise_s: None, - integration_time: None, + integration_time: Some(60 * Unit::Second), }; assert_eq!(expected_gs, gs); diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs new file mode 100644 index 00000000..9e09e852 --- /dev/null +++ b/src/od/ground_station/trk_device.rs @@ -0,0 +1,164 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; +use crate::md::prelude::{Interpolatable, Traj}; +use crate::od::msr::measurement::Measurement; +use crate::od::msr::MeasurementType; +use crate::time::Epoch; +use crate::Spacecraft; +use anise::errors::AlmanacResult; +use anise::frames::Frame; +use anise::prelude::{Almanac, Orbit}; +use hifitime::TimeUnits; +use indexmap::IndexSet; +use rand_pcg::Pcg64Mcg; +use snafu::ResultExt; +use std::sync::Arc; + +use super::GroundStation; + +impl TrackingDevice for GroundStation { + fn measurement_types(&self) -> &IndexSet { + &self.measurement_types + } + + /// Perform a measurement from the ground station to the receiver (rx). + fn measure( + &mut self, + epoch: Epoch, + traj: &Traj, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError> { + match self.integration_time { + Some(integration_time) => { + // If out of traj bounds, return None. + let rx_0 = traj.at(epoch - integration_time).context(ODTrajSnafu)?; + let rx_1 = traj.at(epoch).context(ODTrajSnafu)?; + + let obstructing_body = if !self.frame.ephem_origin_match(rx_0.frame()) { + Some(rx_0.frame()) + } else { + None + }; + + let aer_t0 = self + .azimuth_elevation_of(rx_0.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + let aer_t1 = self + .azimuth_elevation_of(rx_1.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + + if aer_t0.elevation_deg < self.elevation_mask_deg + || aer_t1.elevation_deg < self.elevation_mask_deg + { + debug!( + "{} (el. mask {:.3} deg) but object moves from {:.3} to {:.3} deg -- no measurement", + self.name, self.elevation_mask_deg, aer_t0.elevation_deg, aer_t1.elevation_deg + ); + return Ok(None); + } + + // Noises are computed at the midpoint of the integration time. + let noises = self.noises(epoch - integration_time * 0.5, rng)?; + + let mut msr = Measurement::new(self.name.clone(), epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_two_way(aer_t0, aer_t1, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) + } + None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac), + } + } + + fn name(&self) -> String { + self.name.clone() + } + + fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult { + almanac.transform_to(self.to_orbit(epoch, &almanac).unwrap(), frame, None) + } + + fn measure_instantaneous( + &mut self, + rx: Spacecraft, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError> { + let obstructing_body = if !self.frame.ephem_origin_match(rx.frame()) { + Some(rx.frame()) + } else { + None + }; + + let aer = self + .azimuth_elevation_of(rx.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + + if aer.elevation_deg >= self.elevation_mask_deg { + // Only update the noises if the measurement is valid. + let noises = self.noises(rx.orbit.epoch, rng)?; + + let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_one_way(aer, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) + } else { + debug!( + "{} {} (el. mask {:.3} deg), object at {:.3} deg -- no measurement", + self.name, rx.orbit.epoch, self.elevation_mask_deg, aer.elevation_deg + ); + Ok(None) + } + } + + /// Returns the measurement noise of this ground station. + /// + /// # Methodology + /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise. + /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is + /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The + /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. + fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result { + Ok(match msr_type { + MeasurementType::Range => self + .range_noise_km + .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? + .covariance(epoch), + MeasurementType::Doppler => self + .doppler_noise_km_s + .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? + .covariance(epoch), + }) + } +} From baf92ae8e42693cbfbfdb76e32039052489ff9e7 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 18:14:34 -0700 Subject: [PATCH 11/38] Switch to serde_yml --- Cargo.toml | 5 ++--- src/cosmic/spacecraft.rs | 12 +++++------ src/io/matrices.rs | 20 +++++++++---------- src/io/mod.rs | 14 ++++++------- src/od/ground_station/mod.rs | 2 +- src/od/msr/data_arc.rs | 13 ------------ src/od/noise/gauss_markov.rs | 10 +++++----- src/od/simulator/scheduler.rs | 20 +++++++++---------- src/od/simulator/trkconfig.rs | 18 ++++++++--------- .../orbit_determination/ground_station.rs | 6 +++--- src/python/orbit_determination/trkconfig.rs | 2 +- src/python/pyo3utils/mod.rs | 2 +- 12 files changed, 55 insertions(+), 69 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 6b108063..1063e804 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -38,8 +38,7 @@ gitlab = { repository = "nyx-space/nyx", branch = "master" } nalgebra = "0.33" log = "0.4" hifitime = "4.0.0" -# anise = "0.5.0" # UNRELEASED -anise = { git = "https://github.com/nyx-space/anise.git", branch = "master" } +anise = "0.5.0" # UNRELEASED flate2 = { version = "1.0", features = [ "rust_backend", ], default-features = false } @@ -64,7 +63,7 @@ parquet = { version = "53.0.0", default-features = false, features = [ ] } arrow = "53.0.0" shadow-rs = { version = "0.36.0", default-features = false } -serde_yaml = "0.9.21" +serde_yml = "0.0.12" whoami = "1.3.0" either = { version = "1.8.1", features = ["serde"] } num = "0.4.0" diff --git a/src/cosmic/spacecraft.rs b/src/cosmic/spacecraft.rs index fcc4d16e..9f427ebf 100644 --- a/src/cosmic/spacecraft.rs +++ b/src/cosmic/spacecraft.rs @@ -808,7 +808,7 @@ impl ConfigRepr for Spacecraft {} #[test] fn test_serde() { - use serde_yaml; + use serde_yml; use std::str::FromStr; use anise::constants::frames::EARTH_J2000; @@ -826,10 +826,10 @@ fn test_serde() { let sc = Spacecraft::new(orbit, 500.0, 159.0, 2.0, 2.0, 1.8, 2.2); - let serialized_sc = serde_yaml::to_string(&sc).unwrap(); + let serialized_sc = serde_yml::to_string(&sc).unwrap(); println!("{}", serialized_sc); - let deser_sc: Spacecraft = serde_yaml::from_str(&serialized_sc).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(&serialized_sc).unwrap(); assert_eq!(sc, deser_sc); @@ -860,7 +860,7 @@ drag: cd: 2.2 "#; - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); assert_eq!(sc, deser_sc); // Check that we can specify a thruster info entirely. @@ -898,7 +898,7 @@ thruster: isp_s: 300.5, thrust_N: 1e-5, }); - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); assert_eq!(sc_thruster, deser_sc); // Tests the minimum definition which will set all of the defaults too @@ -922,7 +922,7 @@ dry_mass_kg: 500.0 fuel_mass_kg: 159.0 "#; - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); let sc = Spacecraft::new(orbit, 500.0, 159.0, 0.0, 0.0, 1.8, 2.2); assert_eq!(sc, deser_sc); diff --git a/src/io/matrices.rs b/src/io/matrices.rs index 99018615..619e2543 100644 --- a/src/io/matrices.rs +++ b/src/io/matrices.rs @@ -84,16 +84,16 @@ pub struct Mat6([[f64; 6]; 6]); #[test] fn test_serde2() { - use serde_yaml; + use serde_yml; let m_diag = Matrix2Serde { inner: Either::Left(Diag2([1.0, 2.0])), }; - println!("Diag -- \n{}", serde_yaml::to_string(&m_diag).unwrap()); + println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); // Load from one line list let diag_s = "[1.0, 2.0]"; - let diag_loaded: Matrix2Serde = serde_yaml::from_str(diag_s).unwrap(); + let diag_loaded: Matrix2Serde = serde_yml::from_str(diag_s).unwrap(); assert_eq!(diag_loaded, m_diag); let m_full = Matrix2Serde { @@ -101,30 +101,30 @@ fn test_serde2() { }; // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yaml::to_string(&m_full).unwrap()); + println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); // Load from list let full_mat = r#" - [1.0, 2.0] # Row 1 - [1.0, 2.0] # Row 2 "#; - let full_loaded: Matrix2Serde = serde_yaml::from_str(full_mat).unwrap(); + let full_loaded: Matrix2Serde = serde_yml::from_str(full_mat).unwrap(); assert_eq!(full_loaded, m_full); } #[test] fn test_serde6() { - use serde_yaml; + use serde_yml; let m_diag = Matrix6Serde { inner: Either::Left(Diag6([1.0, 2.0, 3.0, 4.0, 5.0, 6.0])), }; - println!("Diag -- \n{}", serde_yaml::to_string(&m_diag).unwrap()); + println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); // Load from one line list let diag_s = "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]"; - let diag_loaded: Matrix6Serde = serde_yaml::from_str(diag_s).unwrap(); + let diag_loaded: Matrix6Serde = serde_yml::from_str(diag_s).unwrap(); assert_eq!(diag_loaded, m_diag); let m_full = Matrix6Serde { @@ -132,7 +132,7 @@ fn test_serde6() { }; // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yaml::to_string(&m_full).unwrap()); + println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); // Load from list let full_mat = r#" - [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 1 @@ -143,7 +143,7 @@ fn test_serde6() { - [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 6 "#; - let full_loaded: Matrix6Serde = serde_yaml::from_str(full_mat).unwrap(); + let full_loaded: Matrix6Serde = serde_yml::from_str(full_mat).unwrap(); assert_eq!(full_loaded, m_full); } diff --git a/src/io/mod.rs b/src/io/mod.rs index 55cdcb55..ca2d321f 100644 --- a/src/io/mod.rs +++ b/src/io/mod.rs @@ -29,7 +29,7 @@ use hifitime::Duration; use serde::de::DeserializeOwned; use serde::{Deserialize, Deserializer}; use serde::{Serialize, Serializer}; -use serde_yaml::Error as YamlError; +use serde_yml::Error as YamlError; use std::collections::{BTreeMap, HashMap}; use std::convert::From; use std::fmt::Debug; @@ -163,7 +163,7 @@ pub enum ConfigError { ReadError { source: io::Error }, #[snafu(display("failed to parse YAML configuration file: {source}"))] - ParseError { source: serde_yaml::Error }, + ParseError { source: serde_yml::Error }, #[snafu(display("of invalid configuration: {msg}"))] InvalidConfig { msg: String }, @@ -223,7 +223,7 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided path to a yaml @@ -234,7 +234,7 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a map of names to "selves" from the provided path to a yaml @@ -245,19 +245,19 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided string of a yaml fn loads_many(data: &str) -> Result, ConfigError> { debug!("Loading YAML:\n{data}"); - serde_yaml::from_str(data).context(ParseSnafu) + serde_yml::from_str(data).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided string of a yaml fn loads_named(data: &str) -> Result, ConfigError> { debug!("Loading YAML:\n{data}"); - serde_yaml::from_str(data).context(ParseSnafu) + serde_yml::from_str(data).context(ParseSnafu) } } diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index cd6dedfe..d90d5fbc 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -328,7 +328,7 @@ mod gs_ut { assert_eq!(expected, stations); // Serialize back - let reser = serde_yaml::to_string(&expected).unwrap(); + let reser = serde_yml::to_string(&expected).unwrap(); dbg!(reser); } } diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index 5477ed0f..23b49703 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -427,16 +427,3 @@ impl PartialEq for TrackingDataArc { self.measurements == other.measurements } } - -#[cfg(test)] -mod ut_tracker { - use super::TrackingDataArc; - - #[test] - fn test_lro_data() { - let trk = TrackingDataArc::from_parquet("04_lro_simulated_tracking.parquet").unwrap(); - println!("{trk}"); - println!("min step = {}", trk.min_duration_sep().unwrap()); - // TODO: Add test nulls in specific sets, and missing columns. - } -} diff --git a/src/od/noise/gauss_markov.rs b/src/od/noise/gauss_markov.rs index 1957e126..a004253a 100644 --- a/src/od/noise/gauss_markov.rs +++ b/src/od/noise/gauss_markov.rs @@ -291,7 +291,7 @@ impl GaussMarkov { for item in as_list.iter() { // Check that the item is a dictionary let next: Self = - serde_yaml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; + serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; selves.push(next); } Ok(selves) @@ -307,7 +307,7 @@ impl GaussMarkov { // Try to convert the underlying data match pyany_to_value(v_any) { Ok(value) => { - match serde_yaml::from_value(value) { + match serde_yml::from_value(value) { Ok(next) => selves.push(next), Err(_) => { // Maybe this was to be parsed in full as a single item @@ -412,15 +412,15 @@ mod ut_gm { #[test] fn serde_test() { - use serde_yaml; + use serde_yml; use std::env; use std::path::PathBuf; // Note that we set the initial bias to zero because it is not serialized. let gm = GaussMarkov::new(Duration::MAX, 0.1).unwrap(); - let serialized = serde_yaml::to_string(&gm).unwrap(); + let serialized = serde_yml::to_string(&gm).unwrap(); println!("{serialized}"); - let gm_deser: GaussMarkov = serde_yaml::from_str(&serialized).unwrap(); + let gm_deser: GaussMarkov = serde_yml::from_str(&serialized).unwrap(); assert_eq!(gm_deser, gm); let test_data: PathBuf = [ diff --git a/src/od/simulator/scheduler.rs b/src/od/simulator/scheduler.rs index 117ff745..8f7267b0 100644 --- a/src/od/simulator/scheduler.rs +++ b/src/od/simulator/scheduler.rs @@ -122,13 +122,13 @@ mod scheduler_ut { #[test] fn serde_cadence() { use hifitime::TimeUnits; - use serde_yaml; + use serde_yml; - let cont: Cadence = serde_yaml::from_str("!Continuous").unwrap(); + let cont: Cadence = serde_yml::from_str("!Continuous").unwrap(); assert_eq!(cont, Cadence::Continuous); let int: Cadence = - serde_yaml::from_str("!Intermittent {on: 1 h 35 min, off: 15 h 02 min 3 s}").unwrap(); + serde_yml::from_str("!Intermittent {on: 1 h 35 min, off: 15 h 02 min 3 s}").unwrap(); assert_eq!( int, Cadence::Intermittent { @@ -141,23 +141,23 @@ mod scheduler_ut { r#"Intermittent { on: "1 h 35 min", off: "15 h 2 min 3 s" }"# ); - let serialized = serde_yaml::to_string(&int).unwrap(); - let deserd: Cadence = serde_yaml::from_str(&serialized).unwrap(); + let serialized = serde_yml::to_string(&int).unwrap(); + let deserd: Cadence = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, int); } #[test] fn api_and_serde_scheduler() { use hifitime::TimeUnits; - use serde_yaml; + use serde_yml; let scheduler = Scheduler::default(); - let serialized = serde_yaml::to_string(&scheduler).unwrap(); + let serialized = serde_yml::to_string(&scheduler).unwrap(); assert_eq!( serialized, "handoff: Eager\ncadence: Continuous\nmin_samples: 0\nsample_alignment: null\n" ); - let deserd: Scheduler = serde_yaml::from_str(&serialized).unwrap(); + let deserd: Scheduler = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, scheduler); let scheduler = Scheduler::builder() @@ -168,12 +168,12 @@ mod scheduler_ut { }) .build(); - let serialized = serde_yaml::to_string(&scheduler).unwrap(); + let serialized = serde_yml::to_string(&scheduler).unwrap(); assert_eq!( serialized, "handoff: Eager\ncadence: !Intermittent\n on: 12 min\n off: 17 h 5 min\nmin_samples: 10\nsample_alignment: 1 s\n" ); - let deserd: Scheduler = serde_yaml::from_str(&serialized).unwrap(); + let deserd: Scheduler = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, scheduler); } diff --git a/src/od/simulator/trkconfig.rs b/src/od/simulator/trkconfig.rs index e6570243..9a0934b0 100644 --- a/src/od/simulator/trkconfig.rs +++ b/src/od/simulator/trkconfig.rs @@ -60,7 +60,7 @@ impl FromStr for TrkConfig { type Err = ConfigError; fn from_str(s: &str) -> Result { - serde_yaml::from_str(s).map_err(|source| ConfigError::ParseError { source }) + serde_yml::from_str(s).map_err(|source| ConfigError::ParseError { source }) } } @@ -206,13 +206,13 @@ mod trkconfig_ut { #[test] fn serde_trkconfig() { - use serde_yaml; + use serde_yml; // Test the default config let cfg = TrkConfig::default(); - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); assert_eq!( cfg.scheduler.unwrap(), @@ -234,9 +234,9 @@ mod trkconfig_ut { sampling: 45.2.seconds(), ..Default::default() }; - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); } @@ -263,16 +263,16 @@ mod trkconfig_ut { #[test] fn api_trk_config() { - use serde_yaml; + use serde_yml; let cfg = TrkConfig::builder() .sampling(15.seconds()) .scheduler(Scheduler::builder().handoff(Handoff::Overlap).build()) .build(); - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); let cfg = TrkConfig::builder() diff --git a/src/python/orbit_determination/ground_station.rs b/src/python/orbit_determination/ground_station.rs index 03f99c35..c85d2610 100644 --- a/src/python/orbit_determination/ground_station.rs +++ b/src/python/orbit_determination/ground_station.rs @@ -126,7 +126,7 @@ impl GroundStation { for item in as_list.iter() { // Check that the item is a dictionary let next: Self = - serde_yaml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; + serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; as_map.insert(next.name.clone(), next); } Ok(as_map) @@ -148,12 +148,12 @@ impl GroundStation { // Try to convert the underlying data match pyany_to_value(v_any) { Ok(value) => { - let next: Self = serde_yaml::from_value(value).context(ParseSnafu)?; + let next: Self = serde_yml::from_value(value).context(ParseSnafu)?; as_map.insert(as_str, next); } Err(_) => { // Maybe this was to be parsed in full as a single item - let me: Self = serde_yaml::from_value(pyany_to_value(as_dict)?) + let me: Self = serde_yml::from_value(pyany_to_value(as_dict)?) .context(ParseSnafu)?; as_map.clear(); as_map.insert(me.name.clone(), me); diff --git a/src/python/orbit_determination/trkconfig.rs b/src/python/orbit_determination/trkconfig.rs index b28a146a..b0fee72e 100644 --- a/src/python/orbit_determination/trkconfig.rs +++ b/src/python/orbit_determination/trkconfig.rs @@ -71,7 +71,7 @@ impl TrkConfig { } fn __repr__(&self) -> String { - serde_yaml::to_string(&self).unwrap() + serde_yml::to_string(&self).unwrap() } fn __str__(&self) -> String { diff --git a/src/python/pyo3utils/mod.rs b/src/python/pyo3utils/mod.rs index 6aede635..146abeb1 100644 --- a/src/python/pyo3utils/mod.rs +++ b/src/python/pyo3utils/mod.rs @@ -18,7 +18,7 @@ use crate::io::ConfigError; use pyo3::types::{PyAny, PyDict, PyList}; -use serde_yaml::{Mapping, Value}; +use serde_yml::{Mapping, Value}; /// Try to convert the provided PyAny into a SerDe YAML Value pub fn pyany_to_value(any: &PyAny) -> Result { From b1eb2d71f4c5f3e880b3ca1c7f814d4650f8cd7b Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 18:19:08 -0700 Subject: [PATCH 12/38] Remove unused rust-embed --- Cargo.toml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 1063e804..7eb0b6aa 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -38,7 +38,7 @@ gitlab = { repository = "nyx-space/nyx", branch = "master" } nalgebra = "0.33" log = "0.4" hifitime = "4.0.0" -anise = "0.5.0" # UNRELEASED +anise = "0.5.0" flate2 = { version = "1.0", features = [ "rust_backend", ], default-features = false } @@ -49,7 +49,6 @@ hyperdual = "1.3.0" bytes = "1.0" rand = "0.8" rand_distr = "0.4" -rust-embed = "8" regex = "1.5" rayon = "1.6" lazy_static = "1.4.0" From bbf019f150180cbe204f77a82417b7d8dc8505e7 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 20:47:02 -0700 Subject: [PATCH 13/38] WIP az/el --- src/od/ground_station/mod.rs | 4 +--- src/od/ground_station/trk_device.rs | 1 + src/od/msr/sensitivity.rs | 1 + src/od/msr/types.rs | 20 ++++++++++++++------ src/od/simulator/scheduler.rs | 2 +- 5 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index d90d5fbc..44e596c5 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -148,6 +148,7 @@ impl GroundStation { .doppler_noise_km_s .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? .sample(epoch, rng), + _ => todo!("az/el"), }; } } @@ -211,7 +212,6 @@ mod gs_ut { use hifitime::TimeUnits; - // Get the path to the root directory of the current Cargo project let test_data: PathBuf = [ env::var("CARGO_MANIFEST_DIR").unwrap(), "data".to_string(), @@ -262,8 +262,6 @@ mod gs_ut { use std::env; use std::path::PathBuf; - // Get the path to the root directory of the current Cargo project - let test_file: PathBuf = [ env::var("CARGO_MANIFEST_DIR").unwrap(), "data".to_string(), diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs index 9e09e852..94c55f44 100644 --- a/src/od/ground_station/trk_device.rs +++ b/src/od/ground_station/trk_device.rs @@ -159,6 +159,7 @@ impl TrackingDevice for GroundStation { .doppler_noise_km_s .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? .covariance(epoch), + _ => todo!("az/el"), }) } } diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 81ea7bfc..4730742e 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -187,6 +187,7 @@ impl ScalarSensitivityT _tx: PhantomData::<_>, }) } + _ => todo!("az/el"), } } } diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index f0e6d4f2..b59c6900 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -27,14 +27,17 @@ use crate::od::ODError; pub enum MeasurementType { Range, Doppler, + Azimuth, + Elevation, } impl MeasurementType { /// Returns the expected unit of this measurement type pub fn unit(self) -> &'static str { match self { - MeasurementType::Range => "km", - MeasurementType::Doppler => "km/s", + Self::Range => "km", + Self::Doppler => "km/s", + Self::Azimuth | Self::Elevation => "deg", } } @@ -55,8 +58,10 @@ impl MeasurementType { /// Computes the one way measurement from an AER object and the noise of this measurement type, returned in the units of this measurement type. pub fn compute_one_way(self, aer: AzElRange, noise: f64) -> Result { match self { - MeasurementType::Range => Ok(aer.range_km + noise), - MeasurementType::Doppler => Ok(aer.range_rate_km_s + noise), + Self::Range => Ok(aer.range_km + noise), + Self::Doppler => Ok(aer.range_rate_km_s + noise), + Self::Azimuth => Ok(aer.azimuth_deg + noise), + Self::Elevation => Ok(aer.elevation_deg + noise), } } @@ -68,14 +73,17 @@ impl MeasurementType { noise: f64, ) -> Result { match self { - MeasurementType::Range => { + Self::Range => { let range_km = (aer_t1.range_km + aer_t0.range_km) * 0.5; Ok(range_km + noise / 2.0_f64.sqrt()) } - MeasurementType::Doppler => { + Self::Doppler => { let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; Ok(doppler_km_s + noise / 2.0_f64.sqrt()) } + Self::Azimuth | Self::Elevation => Err(ODError::MeasurementSimError { + details: format!("{self:?} does not support two way"), + }), } } } diff --git a/src/od/simulator/scheduler.rs b/src/od/simulator/scheduler.rs index 8f7267b0..5444269c 100644 --- a/src/od/simulator/scheduler.rs +++ b/src/od/simulator/scheduler.rs @@ -171,7 +171,7 @@ mod scheduler_ut { let serialized = serde_yml::to_string(&scheduler).unwrap(); assert_eq!( serialized, - "handoff: Eager\ncadence: !Intermittent\n on: 12 min\n off: 17 h 5 min\nmin_samples: 10\nsample_alignment: 1 s\n" + "handoff: Eager\ncadence: !Intermittent\n 'on': '12 min'\n 'off': '17 h 5 min'\nmin_samples: 10\nsample_alignment: '1 s'\n" ); let deserd: Scheduler = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, scheduler); From 62b7e78afca595b142ea6e0df1e9199b455ddcc3 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 21:19:16 -0700 Subject: [PATCH 14/38] Ground station stochastics now indexmap instead of explicit --- data/tests/config/many_ground_stations.yaml | 42 ++++---- data/tests/config/one_ground_station.yaml | 21 ++-- src/od/ground_station/builtin.rs | 24 +++-- src/od/ground_station/mod.rs | 103 +++++++++++--------- src/od/ground_station/trk_device.rs | 19 ++-- src/od/mod.rs | 2 +- src/od/msr/types.rs | 4 + tests/orbit_determination/measurements.rs | 9 +- 8 files changed, 128 insertions(+), 96 deletions(-) diff --git a/data/tests/config/many_ground_stations.yaml b/data/tests/config/many_ground_stations.yaml index b1379047..9d0f7d3e 100644 --- a/data/tests/config/many_ground_stations.yaml +++ b/data/tests/config/many_ground_stations.yaml @@ -5,21 +5,22 @@ mu_km3_s2: 398600.435436096 shape: null elevation_mask_deg: 5.0 - range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m - doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_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 height_km: 0.4 measurement_types: - - Range - - Doppler + - range_km + - doppler_km_s - name: Canberra frame: @@ -31,15 +32,16 @@ longitude_deg: 148.981944 height_km: 0.691750 elevation_mask_deg: 5.0 - range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m - doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_s: + bias: + tau: 24 h + process_noise: 50.0e-6 # 5 cm/s light_time_correction: false measurement_types: - - Range - - Doppler \ No newline at end of file + - range_km + - doppler_km_s \ No newline at end of file diff --git a/data/tests/config/one_ground_station.yaml b/data/tests/config/one_ground_station.yaml index 28f544fb..3a7aff68 100644 --- a/data/tests/config/one_ground_station.yaml +++ b/data/tests/config/one_ground_station.yaml @@ -8,16 +8,17 @@ elevation_mask_deg: 5.0 latitude_deg: 2.3522 longitude_deg: 48.8566 height_km: 0.4 -range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m -doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s +stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_s: + bias: + tau: 24 h + process_noise: 50.0e-6 # 5 cm/s light_time_correction: false measurement_types: - - Range - - Doppler + - range_km + - doppler_km_s integration_time: 1 min \ No newline at end of file diff --git a/src/od/ground_station/builtin.rs b/src/od/ground_station/builtin.rs index 0fb08abc..07e398d0 100644 --- a/src/od/ground_station/builtin.rs +++ b/src/od/ground_station/builtin.rs @@ -28,6 +28,11 @@ impl GroundStation { let mut measurement_types = IndexSet::new(); measurement_types.insert(MeasurementType::Range); measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + Self { name: "Madrid".to_string(), elevation_mask_deg: elevation_mask, @@ -39,8 +44,7 @@ impl GroundStation { integration_time: None, light_time_correction: false, timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), + stochastic_noises: Some(stochastics), } } @@ -53,6 +57,11 @@ impl GroundStation { let mut measurement_types = IndexSet::new(); measurement_types.insert(MeasurementType::Range); measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + Self { name: "Canberra".to_string(), elevation_mask_deg: elevation_mask, @@ -64,8 +73,7 @@ impl GroundStation { integration_time: None, light_time_correction: false, timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), + stochastic_noises: Some(stochastics), } } @@ -78,6 +86,11 @@ impl GroundStation { let mut measurement_types = IndexSet::new(); measurement_types.insert(MeasurementType::Range); measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + Self { name: "Goldstone".to_string(), elevation_mask_deg: elevation_mask, @@ -89,8 +102,7 @@ impl GroundStation { integration_time: None, light_time_correction: false, timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), + stochastic_noises: Some(stochastics), } } } diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index 44e596c5..47e4706b 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -20,12 +20,14 @@ use anise::astro::{Aberration, AzElRange, PhysicsResult}; use anise::constants::frames::EARTH_J2000; use anise::errors::AlmanacResult; use anise::prelude::{Almanac, Frame, Orbit}; -use indexmap::IndexSet; +use indexmap::{IndexMap, IndexSet}; +use snafu::ensure; use super::msr::MeasurementType; use super::noise::StochasticNoise; use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; use crate::io::ConfigRepr; +use crate::od::NoiseNotConfiguredSnafu; use crate::time::Epoch; use hifitime::Duration; use rand_pcg::Pcg64Mcg; @@ -61,10 +63,7 @@ pub struct GroundStation { pub light_time_correction: bool, /// Noise on the timestamp of the measurement pub timestamp_noise_s: Option, - /// Noise on the range data of the measurement - pub range_noise_km: Option, - /// Noise on the Doppler data of the measurement - pub doppler_noise_km_s: Option, + pub stochastic_noises: Option>, } impl GroundStation { @@ -88,8 +87,7 @@ impl GroundStation { integration_time: None, light_time_correction: false, timestamp_noise_s: None, - range_noise_km: None, - doppler_noise_km_s: None, + stochastic_noises: None, } } @@ -132,24 +130,27 @@ impl GroundStation { let mut noises = vec![0.0; self.measurement_types.len() + 1]; if let Some(rng) = rng { + ensure!( + self.stochastic_noises.is_some(), + NoiseNotConfiguredSnafu { + kind: "ground station stochastics".to_string(), + } + ); // Add the timestamp noise first if let Some(mut timestamp_noise) = self.timestamp_noise_s { noises[0] = timestamp_noise.sample(epoch, rng); } + let stochastics = self.stochastic_noises.as_mut().unwrap(); + for (ii, msr_type) in self.measurement_types.iter().enumerate() { - noises[ii + 1] = match msr_type { - MeasurementType::Range => self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .sample(epoch, rng), - MeasurementType::Doppler => self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .sample(epoch, rng), - _ => todo!("az/el"), - }; + noises[ii + 1] = stochastics + .get_mut(msr_type) + .ok_or(ODError::NoiseNotConfigured { + kind: format!("{msr_type:?}"), + })? + .sample(epoch, rng); } } @@ -173,8 +174,7 @@ impl Default for GroundStation { integration_time: None, light_time_correction: false, timestamp_noise_s: None, - range_noise_km: None, - doppler_noise_km_s: None, + stochastic_noises: None, } } } @@ -200,7 +200,7 @@ impl fmt::Display for GroundStation { mod gs_ut { use anise::constants::frames::IAU_EARTH_FRAME; - use indexmap::IndexSet; + use indexmap::{IndexMap, IndexSet}; use crate::io::ConfigRepr; use crate::od::prelude::*; @@ -232,19 +232,28 @@ mod gs_ut { measurement_types.insert(MeasurementType::Range); measurement_types.insert(MeasurementType::Doppler); + let mut stochastics = IndexMap::new(); + stochastics.insert( + MeasurementType::Range, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), + ..Default::default() + }, + ); + stochastics.insert( + MeasurementType::Doppler, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), + ..Default::default() + }, + ); + let expected_gs = GroundStation { name: "Demo ground station".to_string(), frame: IAU_EARTH_FRAME, measurement_types, elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), + stochastic_noises: Some(stochastics), latitude_deg: 2.3522, longitude_deg: 48.8566, height_km: 0.4, @@ -253,6 +262,8 @@ mod gs_ut { integration_time: Some(60 * Unit::Second), }; + println!("{}", serde_yml::to_string(&expected_gs).unwrap()); + assert_eq!(expected_gs, gs); } @@ -280,20 +291,29 @@ mod gs_ut { measurement_types.insert(MeasurementType::Range); measurement_types.insert(MeasurementType::Doppler); + let mut stochastics = IndexMap::new(); + stochastics.insert( + MeasurementType::Range, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), + ..Default::default() + }, + ); + stochastics.insert( + MeasurementType::Doppler, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), + ..Default::default() + }, + ); + let expected = vec![ GroundStation { name: "Demo ground station".to_string(), frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), measurement_types: measurement_types.clone(), elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), + stochastic_noises: Some(stochastics.clone()), latitude_deg: 2.3522, longitude_deg: 48.8566, height_km: 0.4, @@ -306,14 +326,7 @@ mod gs_ut { frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), measurement_types, elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), + stochastic_noises: Some(stochastics), latitude_deg: -35.398333, longitude_deg: 148.981944, height_km: 0.691750, diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs index 94c55f44..44f10dab 100644 --- a/src/od/ground_station/trk_device.rs +++ b/src/od/ground_station/trk_device.rs @@ -150,16 +150,13 @@ impl TrackingDevice for GroundStation { /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result { - Ok(match msr_type { - MeasurementType::Range => self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .covariance(epoch), - MeasurementType::Doppler => self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .covariance(epoch), - _ => todo!("az/el"), - }) + let stochastics = self.stochastic_noises.as_ref().unwrap(); + + Ok(stochastics + .get(&msr_type) + .ok_or(ODError::NoiseNotConfigured { + kind: format!("{msr_type:?}"), + })? + .covariance(epoch)) } } diff --git a/src/od/mod.rs b/src/od/mod.rs index ecac0a61..a28ef042 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -190,7 +190,7 @@ pub enum ODError { #[snafu(display("Noise matrix is singular"))] SingularNoiseRk, #[snafu(display("{kind} noise not configured"))] - NoiseNotConfigured { kind: &'static str }, + NoiseNotConfigured { kind: String }, #[snafu(display("measurement sim error: {details}"))] MeasurementSimError { details: String }, #[snafu(display("during an OD encountered {source}"))] diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index b59c6900..f74bf5e8 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -25,9 +25,13 @@ use crate::od::ODError; #[derive(Copy, Clone, Debug, Hash, Serialize, Deserialize, PartialEq, Eq)] pub enum MeasurementType { + #[serde(rename = "range_km")] Range, + #[serde(rename = "doppler_km_s")] Doppler, + #[serde(rename = "azimuth_deg")] Azimuth, + #[serde(rename = "elevation_deg")] Elevation, } diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index 7a71d58e..496ae7a1 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -3,7 +3,7 @@ extern crate nyx_space as nyx; use anise::constants::celestial_objects::{EARTH, MOON, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; -use indexmap::IndexSet; +use indexmap::{IndexMap, IndexSet}; use nalgebra::U2; use nyx::cosmic::Orbit; use nyx::dynamics::SpacecraftDynamics; @@ -36,6 +36,10 @@ fn nil_measurement(almanac: Arc) { let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, StochasticNoise::MIN); + stochastics.insert(MeasurementType::Doppler, StochasticNoise::MIN); + let mut station = GroundStation { name: "nil".to_string(), latitude_deg: lat, @@ -44,8 +48,7 @@ fn nil_measurement(almanac: Arc) { frame: eme2k, elevation_mask_deg: 0.0, timestamp_noise_s: None, - range_noise_km: Some(StochasticNoise::MIN), - doppler_noise_km_s: Some(StochasticNoise::MIN), + stochastic_noises: Some(stochastics), integration_time: None, light_time_correction: false, ..Default::default() From b16228207f25b013bbba7056677cbecdfffaff91 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 21:26:37 -0700 Subject: [PATCH 15/38] Fix LRO ground network --- examples/04_lro_od/dsn-network.yaml | 60 +++++++++++++++++------------ 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/examples/04_lro_od/dsn-network.yaml b/examples/04_lro_od/dsn-network.yaml index 4e8168ea..320f7ce1 100644 --- a/examples/04_lro_od/dsn-network.yaml +++ b/examples/04_lro_od/dsn-network.yaml @@ -5,18 +5,22 @@ mu_km3_s2: 398600.435436096 shape: null elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false latitude_deg: 40.427222 longitude_deg: 4.250556 height_km: 0.834939 + measurement_types: + - range_km + - doppler_km_s - name: DSS-34 Canberra frame: @@ -28,15 +32,19 @@ longitude_deg: 148.981944 height_km: 0.691750 elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - range_km + - doppler_km_s - name: DSS-13 Goldstone frame: @@ -48,12 +56,16 @@ longitude_deg: 243.205 height_km: 1.071149 elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - range_km + - doppler_km_s From 2bc2fb15146d1e894844defc4b8578545fad74be Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 21:39:12 -0700 Subject: [PATCH 16/38] Trying to debug the gs ut on github actions --- src/od/ground_station/mod.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index 47e4706b..d2f46201 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -224,6 +224,8 @@ mod gs_ut { assert!(test_data.exists(), "Could not find the test data"); + println!("Loading {}", test_data.to_str().unwrap()); + let gs = GroundStation::load(test_data).unwrap(); dbg!(&gs); @@ -283,6 +285,8 @@ mod gs_ut { .iter() .collect(); + println!("Loading {}", test_file.to_str().unwrap()); + let stations = GroundStation::load_many(test_file).unwrap(); dbg!(&stations); From 81de67c4d1290cd5a49919dc52943ce8d0624c50 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 21:46:14 -0700 Subject: [PATCH 17/38] Again --- .github/workflows/rust.yaml | 6 +++++- src/od/ground_station/mod.rs | 4 ---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/rust.yaml b/.github/workflows/rust.yaml index 8434d6ee..17d333c7 100644 --- a/.github/workflows/rust.yaml +++ b/.github/workflows/rust.yaml @@ -73,7 +73,11 @@ jobs: restore-keys: ${{ runner.os }}-cargo- - name: Unit Test (debug) - run: cargo test --lib + run: | + cat /home/runner/work/nyx/nyx/data/tests/config/many_ground_stations.yaml + cat /home/runner/work/nyx/nyx/data/tests/config/one_ground_station.yaml + ls -lh /home/runner/work/nyx/nyx/data/tests/config/ + cargo test --lib - name: All integration tests (release) run: cargo test --release --test "*" diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index d2f46201..47e4706b 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -224,8 +224,6 @@ mod gs_ut { assert!(test_data.exists(), "Could not find the test data"); - println!("Loading {}", test_data.to_str().unwrap()); - let gs = GroundStation::load(test_data).unwrap(); dbg!(&gs); @@ -285,8 +283,6 @@ mod gs_ut { .iter() .collect(); - println!("Loading {}", test_file.to_str().unwrap()); - let stations = GroundStation::load_many(test_file).unwrap(); dbg!(&stations); From 5103c8fa8efa8cade42eb69620cd02e957b432be Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 22:33:02 -0700 Subject: [PATCH 18/38] Fix trkconfig previously with walagen unwraps Coverage now takes 8 min on my desktop, so I'm moving it to the normal workflow --- .github/workflows/coverage.yaml | 43 ---- .github/workflows/rust.yaml | 35 +++ src/od/simulator/arc.rs | 222 +++++++++--------- src/od/simulator/trkconfig.rs | 6 +- tests/orbit_determination/simulator.rs | 7 +- tests/orbit_determination/spacecraft.rs | 4 +- tests/orbit_determination/trackingarc.rs | 11 +- .../closedloop_multi_oe_ruggiero.rs | 6 +- 8 files changed, 172 insertions(+), 162 deletions(-) delete mode 100644 .github/workflows/coverage.yaml diff --git a/.github/workflows/coverage.yaml b/.github/workflows/coverage.yaml deleted file mode 100644 index 8780f0ae..00000000 --- a/.github/workflows/coverage.yaml +++ /dev/null @@ -1,43 +0,0 @@ -name: Daily Workflow - -on: - push: - branches: - - master - workflow_dispatch: - -jobs: - full-coverage: - name: Unit test and integration test coverage analysis - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - - name: Install stable toolchain - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable - components: rustfmt, clippy - - - name: Download data - run: | - wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de440s.bsp - wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de438.bsp # GMAT validation cases - wget -O data/pck08.pca http://public-data.nyxspace.com/anise/v0.4/pck08.pca - wget -O data/earth_latest_high_prec.bpc http://public-data.nyxspace.com/anise/ci/earth_latest_high_prec-2023-09-08.bpc - - - name: Install cargo-llvm-cov - uses: taiki-e/install-action@cargo-llvm-cov - - name: Generate full code coverage - run: | - cargo llvm-cov test --release --lib --no-report - cargo llvm-cov test --release cov_test --no-report - cargo llvm-cov report --lcov --output-path lcov.info - - name: Upload coverage to Codecov - uses: codecov/codecov-action@v3 - with: - token: ${{ secrets.CODECOV_TOKEN }} # not required for public repos - files: lcov.info - fail_ci_if_error: false diff --git a/.github/workflows/rust.yaml b/.github/workflows/rust.yaml index 17d333c7..33a1437c 100644 --- a/.github/workflows/rust.yaml +++ b/.github/workflows/rust.yaml @@ -93,6 +93,41 @@ jobs: cargo build --example 03_geo_raise --release cargo build --example 03_geo_sk --release + coverage: + name: Unit test and integration test coverage analysis + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + + - name: Install stable toolchain + uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + components: rustfmt, clippy + + - name: Download data + run: | + wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de440s.bsp + wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de438.bsp # GMAT validation cases + wget -O data/pck08.pca http://public-data.nyxspace.com/anise/v0.4/pck08.pca + wget -O data/earth_latest_high_prec.bpc http://public-data.nyxspace.com/anise/ci/earth_latest_high_prec-2023-09-08.bpc + + - name: Install cargo-llvm-cov + uses: taiki-e/install-action@cargo-llvm-cov + - name: Generate full code coverage + run: | + cargo llvm-cov test --release --lib --no-report + cargo llvm-cov test --release cov_test --no-report + cargo llvm-cov report --lcov --output-path lcov.info + - name: Upload coverage to Codecov + uses: codecov/codecov-action@v3 + with: + token: ${{ secrets.CODECOV_TOKEN }} # not required for public repos + files: lcov.info + fail_ci_if_error: false + lints: name: Lints runs-on: ubuntu-latest diff --git a/src/od/simulator/arc.rs b/src/od/simulator/arc.rs index 8097f672..caa59a60 100644 --- a/src/od/simulator/arc.rs +++ b/src/od/simulator/arc.rs @@ -95,7 +95,7 @@ where } } - if devices.is_empty() { + if sampling_rates_ns.is_empty() { return Err(ConfigError::InvalidConfig { msg: "None of the devices are properly configured".to_string(), }); @@ -171,61 +171,64 @@ where let mut measurements = BTreeMap::new(); for (name, device) in self.devices.iter_mut() { - let cfg = &self.configs[name]; - if cfg.scheduler.is_some() { - if cfg.strands.is_none() { - return Err(NyxError::CustomError { - msg: format!( - "schedule for {name} must be built before generating measurements" - ), - }); - } else { - warn!("scheduler for {name} is ignored, using the defined tracking strands instead") + if let Some(cfg) = self.configs.get(name) { + if cfg.scheduler.is_some() { + if cfg.strands.is_none() { + return Err(NyxError::CustomError { + msg: format!( + "schedule for {name} must be built before generating measurements" + ), + }); + } else { + warn!("scheduler for {name} is ignored, using the defined tracking strands instead") + } } - } - let init_msr_count = measurements.len(); - let tick = Epoch::now().unwrap(); - - match cfg.strands.as_ref() { - Some(strands) => { - // Strands are defined at this point - 'strands: for (ii, strand) in strands.iter().enumerate() { - // Build the time series for this strand, sampling at the correct rate - for epoch in TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) { - match device.measure( - epoch, - &self.trajectory, - Some(&mut self.rng), - almanac.clone(), - ) { - Ok(msr_opt) => { - if let Some(msr) = msr_opt { - measurements.insert(epoch, msr); + let init_msr_count = measurements.len(); + let tick = Epoch::now().unwrap(); + + match cfg.strands.as_ref() { + Some(strands) => { + // Strands are defined at this point + 'strands: for (ii, strand) in strands.iter().enumerate() { + // Build the time series for this strand, sampling at the correct rate + for epoch in + TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) + { + match device.measure( + epoch, + &self.trajectory, + Some(&mut self.rng), + almanac.clone(), + ) { + Ok(msr_opt) => { + if let Some(msr) = msr_opt { + measurements.insert(epoch, msr); + } } - } - Err(e) => { - if epoch != strand.end { - warn!( + Err(e) => { + if epoch != strand.end { + warn!( "Skipping the remaining strand #{ii} ending on {}: {e}", strand.end ); + } + continue 'strands; } - continue 'strands; } } } - } - info!( - "Simulated {} measurements for {name} for {} tracking strands in {}", - measurements.len() - init_msr_count, - strands.len(), - (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) - ); - } - None => { - warn!("No tracking strands defined for {name}, skipping"); + info!( + "Simulated {} measurements for {name} for {} tracking strands in {}", + measurements.len() - init_msr_count, + strands.len(), + (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) + ); + } + None => { + warn!("No tracking strands defined for {name}, skipping"); + } } } } @@ -280,78 +283,81 @@ impl TrackingArcSim { ) -> Result, NyxError> { // Consider using find_all via the heuristic let mut built_cfg = self.configs.clone(); + for (name, device) in self.devices.iter() { - let cfg = &self.configs[name]; - if let Some(scheduler) = cfg.scheduler { - info!("Building schedule for {name}"); - built_cfg.get_mut(name).unwrap().scheduler = None; - built_cfg.get_mut(name).unwrap().strands = Some(Vec::new()); - // Convert the trajectory into the ground station frame. - let traj = self.trajectory.to_frame(device.frame, almanac.clone())?; - - match traj.find_arcs(&device, almanac.clone()) { - Err(_) => info!("No measurements from {name}"), - Ok(elevation_arcs) => { - for arc in elevation_arcs { - let strand_start = arc.rise.state.epoch(); - let strand_end = arc.fall.state.epoch(); - - if strand_end - strand_start - < cfg.sampling * i64::from(scheduler.min_samples) - { - info!( + if let Some(cfg) = self.configs.get(name) { + if let Some(scheduler) = cfg.scheduler { + info!("Building schedule for {name}"); + built_cfg.get_mut(name).unwrap().scheduler = None; + built_cfg.get_mut(name).unwrap().strands = Some(Vec::new()); + + // Convert the trajectory into the ground station frame. + let traj = self.trajectory.to_frame(device.frame, almanac.clone())?; + + match traj.find_arcs(&device, almanac.clone()) { + Err(_) => info!("No measurements from {name}"), + Ok(elevation_arcs) => { + for arc in elevation_arcs { + let strand_start = arc.rise.state.epoch(); + let strand_end = arc.fall.state.epoch(); + + if strand_end - strand_start + < cfg.sampling * i64::from(scheduler.min_samples) + { + info!( "Too few samples from {name} opportunity from {strand_start} to {strand_end}, discarding strand", ); - continue; - } + continue; + } - let mut strand_range = Strand { - start: strand_start, - end: strand_end, - }; + let mut strand_range = Strand { + start: strand_start, + end: strand_end, + }; - // If there is an alignment, apply it - if let Some(alignment) = scheduler.sample_alignment { - strand_range.start = strand_range.start.round(alignment); - strand_range.end = strand_range.end.round(alignment); - } + // If there is an alignment, apply it + if let Some(alignment) = scheduler.sample_alignment { + strand_range.start = strand_range.start.round(alignment); + strand_range.end = strand_range.end.round(alignment); + } - if let Cadence::Intermittent { on, off } = scheduler.cadence { - // Check that the next start time is within the allocated time - if let Some(prev_strand) = - built_cfg[name].strands.as_ref().unwrap().last() - { - if prev_strand.end + off > strand_range.start { - // We're turning on the tracking sooner than the schedule allows, so let's fix that. - strand_range.start = prev_strand.end + off; - // Check that we didn't eat into the whole tracking opportunity - if strand_range.start > strand_end { - // Lost this whole opportunity. - info!("Discarding {name} opportunity from {strand_start} to {strand_end} due to cadence {:?}", scheduler.cadence); - continue; + if let Cadence::Intermittent { on, off } = scheduler.cadence { + // Check that the next start time is within the allocated time + if let Some(prev_strand) = + built_cfg[name].strands.as_ref().unwrap().last() + { + if prev_strand.end + off > strand_range.start { + // We're turning on the tracking sooner than the schedule allows, so let's fix that. + strand_range.start = prev_strand.end + off; + // Check that we didn't eat into the whole tracking opportunity + if strand_range.start > strand_end { + // Lost this whole opportunity. + info!("Discarding {name} opportunity from {strand_start} to {strand_end} due to cadence {:?}", scheduler.cadence); + continue; + } } } + // Check that we aren't tracking for longer than configured + if strand_range.end - strand_range.start > on { + strand_range.end = strand_range.start + on; + } } - // Check that we aren't tracking for longer than configured - if strand_range.end - strand_range.start > on { - strand_range.end = strand_range.start + on; - } + + // We've found when the spacecraft is below the horizon, so this is a new strand. + built_cfg + .get_mut(name) + .unwrap() + .strands + .as_mut() + .unwrap() + .push(strand_range); } - // We've found when the spacecraft is below the horizon, so this is a new strand. - built_cfg - .get_mut(name) - .unwrap() - .strands - .as_mut() - .unwrap() - .push(strand_range); + info!( + "Built {} tracking strands for {name}", + built_cfg[name].strands.as_ref().unwrap().len() + ); } - - info!( - "Built {} tracking strands for {name}", - built_cfg[name].strands.as_ref().unwrap().len() - ); } } } @@ -359,8 +365,10 @@ impl TrackingArcSim { // Build all of the strands, remembering which tracker they come from. let mut cfg_as_vec = Vec::new(); for (name, cfg) in &built_cfg { - for (ii, strand) in cfg.strands.as_ref().unwrap().iter().enumerate() { - cfg_as_vec.push((name.clone(), ii, *strand)); + if let Some(strands) = &cfg.strands { + for (ii, strand) in strands.iter().enumerate() { + cfg_as_vec.push((name.clone(), ii, *strand)); + } } } // Iterate through the strands by chronological order. Cannot use maps because we change types. diff --git a/src/od/simulator/trkconfig.rs b/src/od/simulator/trkconfig.rs index 9a0934b0..7ee43ed5 100644 --- a/src/od/simulator/trkconfig.rs +++ b/src/od/simulator/trkconfig.rs @@ -93,7 +93,11 @@ impl TrkConfig { for (ii, strand) in strands.iter().enumerate() { if strand.duration() < self.sampling { return Err(ConfigError::InvalidConfig { - msg: format!("Strand #{ii} is shorter than sampling time"), + msg: format!( + "Strand #{ii} lasts {} which is shorter than sampling time of {}", + strand.duration(), + self.sampling + ), }); } if strand.duration().is_negative() { diff --git a/tests/orbit_determination/simulator.rs b/tests/orbit_determination/simulator.rs index 0d1c9bf9..20234444 100644 --- a/tests/orbit_determination/simulator.rs +++ b/tests/orbit_determination/simulator.rs @@ -76,9 +76,10 @@ fn continuous_tracking(almanac: Arc) { .iter() .collect(); - let devices = GroundStation::load_named(ground_station_file).unwrap(); - - // dbg!(&devices); + let mut devices = BTreeMap::new(); + for gs in GroundStation::load_many(ground_station_file).unwrap() { + devices.insert(gs.name.clone(), gs); + } // Load the tracking configuration from the test data. let trkconfg_yaml: PathBuf = [ diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index 2e368b4d..751e3680 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -449,8 +449,8 @@ fn od_val_sc_srp_estimation_cov_test( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 1e-3, "More than 1 meter error"); - assert!(delta.vmag_km_s() < 1e-6, "More than 1 millimeter/s error"); + assert!(delta.rmag_km() < 2e-3, "More than 2 meter error"); + assert!(delta.vmag_km_s() < 2e-6, "More than 2 millimeter/s error"); assert!( (est.state().srp.cr - truth_cr).abs() < (1.5 - truth_cr), "Cr estimation did not improve" diff --git a/tests/orbit_determination/trackingarc.rs b/tests/orbit_determination/trackingarc.rs index d47bab5f..111baabe 100644 --- a/tests/orbit_determination/trackingarc.rs +++ b/tests/orbit_determination/trackingarc.rs @@ -57,7 +57,12 @@ fn devices() -> BTreeMap { .iter() .collect(); - GroundStation::load_named(ground_station_file).unwrap() + let mut devices = BTreeMap::new(); + for gs in GroundStation::load_many(ground_station_file).unwrap() { + devices.insert(gs.name.clone(), gs); + } + + devices } #[rstest] @@ -222,7 +227,7 @@ fn trkconfig_delayed_start_cov_test( // Build the configs map with a single ground station let mut configs = BTreeMap::new(); - configs.insert("Demo Ground Station".to_string(), trkcfg); + configs.insert("Demo ground station".to_string(), trkcfg); let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); @@ -252,7 +257,7 @@ fn trkconfig_cadence_cov_test( let mut configs = BTreeMap::new(); configs.insert( - "Demo Ground Station".to_string(), + "Demo ground station".to_string(), TrkConfig::builder() .scheduler( Scheduler::builder() diff --git a/tests/propulsion/closedloop_multi_oe_ruggiero.rs b/tests/propulsion/closedloop_multi_oe_ruggiero.rs index 63374d9d..88e6c190 100644 --- a/tests/propulsion/closedloop_multi_oe_ruggiero.rs +++ b/tests/propulsion/closedloop_multi_oe_ruggiero.rs @@ -95,7 +95,7 @@ fn qlaw_as_ruggiero_case_a(almanac: Arc) { } #[rstest] -fn qlaw_as_ruggiero_case_b_cov_test(almanac: Arc) { +fn qlaw_as_ruggiero_case_b(almanac: Arc) { // Source: AAS-2004-5089 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); @@ -154,7 +154,7 @@ fn qlaw_as_ruggiero_case_b_cov_test(almanac: Arc) { } #[rstest] -fn qlaw_as_ruggiero_case_c(almanac: Arc) { +fn qlaw_as_ruggiero_case_c_cov_test(almanac: Arc) { // Source: AAS-2004-5089 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); @@ -400,7 +400,7 @@ fn qlaw_as_ruggiero_case_f(almanac: Arc) { } #[rstest] -fn ruggiero_iepc_2011_102_cov_test(almanac: Arc) { +fn ruggiero_iepc_2011_102(almanac: Arc) { // Source: IEPC 2011 102 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); From 79e808dd3db4eb31271d96601a9a92debfbf2654 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 22:48:51 -0700 Subject: [PATCH 19/38] Add two tests to coverage + fix LRO ground station config --- examples/04_lro_od/dsn-network.yaml | 9 ++++++--- tests/monte_carlo/framework.rs | 2 +- tests/orbit_determination/two_body.rs | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/04_lro_od/dsn-network.yaml b/examples/04_lro_od/dsn-network.yaml index 320f7ce1..5f6bc446 100644 --- a/examples/04_lro_od/dsn-network.yaml +++ b/examples/04_lro_od/dsn-network.yaml @@ -1,4 +1,5 @@ -- name: DSS-65 Madrid +DSS-65 Madrid: + name: DSS-65 Madrid frame: ephemeris_id: 399 orientation_id: 399 @@ -22,7 +23,8 @@ - range_km - doppler_km_s -- name: DSS-34 Canberra +DSS-34 Canberra: + name: DSS-34 Canberra frame: ephemeris_id: 399 orientation_id: 399 @@ -46,7 +48,8 @@ - range_km - doppler_km_s -- name: DSS-13 Goldstone +DSS-13 Goldstone: + name: DSS-13 Goldstone frame: ephemeris_id: 399 orientation_id: 399 diff --git a/tests/monte_carlo/framework.rs b/tests/monte_carlo/framework.rs index 27089646..190e3b93 100644 --- a/tests/monte_carlo/framework.rs +++ b/tests/monte_carlo/framework.rs @@ -19,7 +19,7 @@ fn almanac() -> Arc { } #[rstest] -fn test_monte_carlo_epoch(almanac: Arc) { +fn test_monte_carlo_epoch_cov_test(almanac: Arc) { extern crate pretty_env_logger; let _ = pretty_env_logger::try_init(); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 75148874..9b8ab581 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -943,7 +943,7 @@ fn od_tb_ckf_map_covar(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn od_tb_val_harmonics_ckf_fixed_step_perfect( +fn od_tb_val_harmonics_ckf_fixed_step_perfect_cov_test( almanac: Arc, sim_devices: BTreeMap, proc_devices: BTreeMap, From af0cf166efaec2a705af7be21d8445a914b807a4 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 22 Nov 2024 22:59:35 -0700 Subject: [PATCH 20/38] Fix coverage report building --- .github/workflows/rust.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rust.yaml b/.github/workflows/rust.yaml index 33a1437c..f7cceffa 100644 --- a/.github/workflows/rust.yaml +++ b/.github/workflows/rust.yaml @@ -120,7 +120,7 @@ jobs: run: | cargo llvm-cov test --release --lib --no-report cargo llvm-cov test --release cov_test --no-report - cargo llvm-cov report --lcov --output-path lcov.info + cargo llvm-cov report --release --lcov --output-path lcov.info - name: Upload coverage to Codecov uses: codecov/codecov-action@v3 with: From 6b447da00e0a0d7babd848bfe33422299a770645 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sat, 23 Nov 2024 17:59:20 -0700 Subject: [PATCH 21/38] Fix singular noise matrix if only one measurement type is used. --- examples/04_lro_od/plot_od_rslt.py | 2 ++ src/od/msr/sensitivity.rs | 2 +- src/od/process/mod.rs | 4 +--- tests/orbit_determination/robust.rs | 9 +++++++-- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 4d06dffb..df17ea7f 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -86,6 +86,8 @@ connectgaps=True, line=dict(dash="dash", color="black"), ) + unit = msr.split()[-1][1:-1] + fig.update_layout(yaxis_title=unit) fig.show() # Plot the RIC uncertainty diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 4730742e..36c9e19d 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -97,7 +97,7 @@ where DefaultAllocator: Allocator + Allocator::Size>, { // Rebuild each row of the scalar sensitivities. - let mut mat = OMatrix::::Size>::zeros(); + let mut mat = OMatrix::::Size>::identity(); for (ith_row, msr_type) in msr_types.iter().enumerate() { if !msr.data.contains_key(msr_type) { // Skip computation, this row is zero anyway. diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index a18ccbc0..3713b6d1 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -477,8 +477,7 @@ where .context(ODPropSnafu)?; for state in traj_covar.states { - // traj.states.push(S::extract(state)); - // HERE: At the time being, only spacecraft estimation is possible, and the trajectory will always be the exact state + // NOTE: At the time being, only spacecraft estimation is possible, and the trajectory will always be the exact state // that was propagated. Even once ground station biases are estimated, these won't go through the propagator. traj.states.push(state); } @@ -538,7 +537,6 @@ where device.measurement_types(), epoch, )?, - // device.measurement_covar(epoch)?, self.resid_crit, ) { Ok((estimate, mut residual)) => { diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 665bcbc6..743e2646 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -39,7 +39,7 @@ fn almanac() -> Arc { #[allow(clippy::identity_op)] #[rstest] -fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { +fn od_robust_test_ekf_realistic_one_way_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); @@ -149,6 +149,11 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { arc.to_parquet_simple(&path).unwrap(); + println!("{arc}\n{arc:?}"); + // Reload + let reloaded = TrackingDataArc::from_parquet(path.join("ekf_robust_msr.parquet")).unwrap(); + assert_eq!(reloaded, arc); + // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be _nearly_ perfect because we"ve removed SATURN_BARYCENTER from the estimated trajectory let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; @@ -239,7 +244,7 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn od_robust_test_ekf_realistic_two_way_cov_test(almanac: Arc) { +fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { let _ = pretty_env_logger::try_init(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); From e882b4317c1d12d00f84b0109a9d5dfa508a88b7 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sat, 23 Nov 2024 22:27:22 -0700 Subject: [PATCH 22/38] Support az/el in OD (manual test with LRO example) --- src/od/msr/sensitivity.rs | 39 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 37 insertions(+), 2 deletions(-) diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 36c9e19d..f9457c52 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -127,7 +127,9 @@ impl ScalarSensitivityT almanac: Arc, ) -> Result { let receiver = rx.orbit; - // Compute the device location + // Compute the device location in the receiver frame because we compute the sensitivity in that frame. + // This frame is required because the scalar measurements are frame independent, but the sensitivity + // must be in the estimation frame. let transmitter = tx .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) .context(ODAlmanacSnafu { @@ -187,7 +189,40 @@ impl ScalarSensitivityT _tx: PhantomData::<_>, }) } - _ => todo!("az/el"), + MeasurementType::Azimuth => { + let m11 = -delta_r.y / (delta_r.x.powi(2) + delta_r.y.powi(2)); + let m12 = delta_r.x / (delta_r.x.powi(2) + delta_r.y.powi(2)); + let m13 = 0.0; + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + MeasurementType::Elevation => { + let r2 = delta_r.norm().powi(2); + + let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt()); + let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt()); + let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } } } } From 6830def0aa7eb031bd387ecc3331a3cbe1cd3726 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 24 Nov 2024 09:04:56 -0700 Subject: [PATCH 23/38] Fix reloading of parquet in one way ekf test --- tests/orbit_determination/robust.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 743e2646..72e1ceba 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -151,7 +151,7 @@ fn od_robust_test_ekf_realistic_one_way_cov_test(almanac: Arc) { println!("{arc}\n{arc:?}"); // Reload - let reloaded = TrackingDataArc::from_parquet(path.join("ekf_robust_msr.parquet")).unwrap(); + let reloaded = TrackingDataArc::from_parquet(&path).unwrap(); assert_eq!(reloaded, arc); // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. From 9a29b16ddafb2155f2d480fb015cbaf9f6b8b974 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 24 Nov 2024 11:24:23 -0700 Subject: [PATCH 24/38] WIP support for az/el in tracking data reloading --- src/od/ground_station/mod.rs | 33 +++ src/od/msr/data_arc.rs | 54 +++- src/od/msr/types.rs | 12 +- src/od/process/export.rs | 32 +-- src/od/process/mod.rs | 94 ++++--- tests/orbit_determination/mod.rs | 1 + tests/orbit_determination/robust.rs | 8 +- tests/orbit_determination/robust_az_el.rs | 310 ++++++++++++++++++++++ 8 files changed, 467 insertions(+), 77 deletions(-) create mode 100644 tests/orbit_determination/robust_az_el.rs diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index 47e4706b..5ddbc476 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -91,6 +91,39 @@ impl GroundStation { } } + /// Returns a copy of this ground station with the new measurement type added (or replaced) + pub fn with_msr_type(mut self, msr_type: MeasurementType, noise: StochasticNoise) -> Self { + if self.stochastic_noises.is_none() { + self.stochastic_noises = Some(IndexMap::new()); + } + + self.stochastic_noises + .as_mut() + .unwrap() + .insert(msr_type, noise); + + self.measurement_types.insert(msr_type); + + self + } + + /// Returns a copy of this ground station without the provided measurement type (if defined, else no error) + pub fn without_msr_type(mut self, msr_type: MeasurementType) -> Self { + if let Some(noises) = self.stochastic_noises.as_mut() { + noises.swap_remove(&msr_type); + } + + self.measurement_types.swap_remove(&msr_type); + + self + } + + pub fn with_integration_time(mut self, integration_time: Option) -> Self { + self.integration_time = integration_time; + + self + } + /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. /// This is a shortcut to almanac.azimuth_elevation_range_sez. pub fn azimuth_elevation_of( diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs index 23b49703..031aef5c 100644 --- a/src/od/msr/data_arc.rs +++ b/src/od/msr/data_arc.rs @@ -68,13 +68,17 @@ impl TrackingDataArc { let mut has_epoch = false; let mut has_tracking_dev = false; let mut range_avail = false; - let mut rate_avail = false; + let mut doppler_avail = false; + let mut az_avail = false; + let mut el_avail = false; for field in &reader.schema().fields { match field.name().as_str() { "Epoch (UTC)" => has_epoch = true, "Tracking device" => has_tracking_dev = true, "Range (km)" => range_avail = true, - "Doppler (km/s)" => rate_avail = true, + "Doppler (km/s)" => doppler_avail = true, + "Azimuth (deg)" => az_avail = true, + "Elevation (deg)" => el_avail = true, _ => {} } } @@ -94,9 +98,9 @@ impl TrackingDataArc { ); ensure!( - range_avail || rate_avail, + range_avail || doppler_avail || az_avail || el_avail, MissingDataSnafu { - which: "`Range (km)` or `Doppler (km/s)`" + which: "`Range (km)` or `Doppler (km/s)` or `Azimuth (deg)` or `Elevation (deg)`" } ); @@ -135,7 +139,7 @@ impl TrackingDataArc { None }; - let doppler_data: Option<&PrimitiveArray> = if rate_avail { + let doppler_data: Option<&PrimitiveArray> = if doppler_avail { Some( batch .column_by_name("Doppler (km/s)") @@ -148,6 +152,32 @@ impl TrackingDataArc { None }; + let azimuth_data: Option<&PrimitiveArray> = if az_avail { + Some( + batch + .column_by_name("Azimuth (deg)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + let elevation_data: Option<&PrimitiveArray> = if el_avail { + Some( + batch + .column_by_name("Elevation (deg)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + // Set the measurements in the tracking arc for i in 0..batch.num_rows() { let epoch = Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { @@ -168,12 +198,24 @@ impl TrackingDataArc { .insert(MeasurementType::Range, range_data.unwrap().value(i)); } - if rate_avail { + if doppler_avail { measurement .data .insert(MeasurementType::Doppler, doppler_data.unwrap().value(i)); } + if az_avail { + measurement + .data + .insert(MeasurementType::Azimuth, azimuth_data.unwrap().value(i)); + } + + if el_avail { + measurement + .data + .insert(MeasurementType::Elevation, elevation_data.unwrap().value(i)); + } + measurements.insert(epoch, measurement); } } diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index f74bf5e8..79c46059 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -70,6 +70,7 @@ impl MeasurementType { } /// Computes the two way measurement from two AER values and the noise of this measurement type, returned in the units of this measurement type. + /// Two way is modeled by averaging the measurement in between both times, and adding the noise divided by sqrt(2). pub fn compute_two_way( self, aer_t0: AzElRange, @@ -85,9 +86,14 @@ impl MeasurementType { let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; Ok(doppler_km_s + noise / 2.0_f64.sqrt()) } - Self::Azimuth | Self::Elevation => Err(ODError::MeasurementSimError { - details: format!("{self:?} does not support two way"), - }), + Self::Azimuth => { + let az_deg = (aer_t1.azimuth_deg + aer_t0.azimuth_deg) * 0.5; + Ok(az_deg + noise / 2.0_f64.sqrt()) + } + Self::Elevation => { + let el_deg = (aer_t1.elevation_deg + aer_t0.elevation_deg) * 0.5; + Ok(el_deg + noise / 2.0_f64.sqrt()) + } } } } diff --git a/src/od/process/export.rs b/src/od/process/export.rs index 979d8691..8332a410 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -42,24 +42,24 @@ use std::path::{Path, PathBuf}; use super::ODProcess; -impl<'a, MeasurementSize: DimName, A: DimName, T: TrackerSensitivity> - ODProcess<'a, SpacecraftDynamics, MeasurementSize, A, KF, T> +impl<'a, MsrSize: DimName, Accel: DimName, Trk: TrackerSensitivity> + ODProcess<'a, SpacecraftDynamics, MsrSize, Accel, KF, Trk> where - DefaultAllocator: Allocator - + Allocator::Size> - + Allocator, MeasurementSize> + DefaultAllocator: Allocator + + Allocator::Size> + + Allocator, MsrSize> + Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator - + Allocator::Size> - + Allocator<::Size, MeasurementSize> - + Allocator - + Allocator + + Allocator + + Allocator::Size> + + Allocator<::Size, MsrSize> + + Allocator + + Allocator + Allocator<::Size> + Allocator<::VecLength> + Allocator<::Size, ::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator<::Size, Accel> + + Allocator::Size>, { /// Store the estimates and residuals in a parquet file pub fn to_parquet>( @@ -260,7 +260,7 @@ where .end_epoch .unwrap_or_else(|| self.estimates.last().unwrap().state().epoch()); - let mut residuals: Vec>> = + let mut residuals: Vec>> = Vec::with_capacity(self.residuals.len()); let mut estimates = Vec::with_capacity(self.estimates.len()); @@ -354,7 +354,7 @@ where // Finally, add the residuals. // Prefits - for i in 0..MeasurementSize::dim() { + for i in 0..MsrSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { @@ -366,7 +366,7 @@ where record.push(Arc::new(data.finish())); } // Postfit - for i in 0..MeasurementSize::dim() { + for i in 0..MsrSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { @@ -378,7 +378,7 @@ where record.push(Arc::new(data.finish())); } // Measurement noise - for i in 0..MeasurementSize::dim() { + for i in 0..MsrSize::dim() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 3713b6d1..4198f557 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -40,78 +40,76 @@ use std::marker::PhantomData; use std::ops::Add; mod export; -// Rename to simultaneous measurement or something like that. - /// An orbit determination process. Note that everything passed to this structure is moved. #[allow(clippy::upper_case_acronyms)] pub struct ODProcess< 'a, D: Dynamics, - MeasurementSize: DimName, - A: DimName, - K: Filter, - T: TrackerSensitivity, + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, > where D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> - + Allocator - + Allocator::Size> - + Allocator + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { /// PropInstance used for the estimation pub prop: PropInstance<'a, D>, /// Kalman filter itself pub kf: K, /// Tracking devices - pub devices: BTreeMap, + pub devices: BTreeMap, /// Vector of estimates available after a pass pub estimates: Vec, /// Vector of residuals available after a pass - pub residuals: Vec>>, + pub residuals: Vec>>, pub ekf_trigger: Option, /// Residual rejection criteria allows preventing bad measurements from affecting the estimation. pub resid_crit: Option, pub almanac: Arc, init_state: D::StateType, - _marker: PhantomData, + _marker: PhantomData, } impl< 'a, D: Dynamics, - MeasurementSize: DimName, - A: DimName, - K: Filter, - T: TrackerSensitivity, - > ODProcess<'a, D, MeasurementSize, A, K, T> + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, + > ODProcess<'a, D, MsrSize, Accel, K, Trk> where D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> - + Allocator - + Allocator::Size> - + Allocator + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { /// Initialize a new orbit determination process with an optional trigger to switch from a CKF to an EKF. pub fn new( prop: PropInstance<'a, D>, kf: K, - devices: BTreeMap, + devices: BTreeMap, ekf_trigger: Option, resid_crit: Option, almanac: Arc, @@ -127,7 +125,7 @@ where resid_crit, almanac, init_state, - _marker: PhantomData::, + _marker: PhantomData::, } } @@ -135,7 +133,7 @@ where pub fn ekf( prop: PropInstance<'a, D>, kf: K, - devices: BTreeMap, + devices: BTreeMap, trigger: EkfTrigger, resid_crit: Option, almanac: Arc, @@ -151,7 +149,7 @@ where resid_crit, almanac, init_state, - _marker: PhantomData::, + _marker: PhantomData::, } } @@ -508,7 +506,7 @@ where } // Check that the observation is valid. - for val in msr.observation::(msr_types).iter() { + for val in msr.observation::(msr_types).iter() { ensure!( val.is_finite(), InvalidMeasurementSnafu { @@ -519,7 +517,7 @@ where } let h_tilde = device - .h_tilde::( + .h_tilde::( msr, msr_types, &nominal_state, @@ -702,30 +700,30 @@ where impl< 'a, D: Dynamics, - MeasurementSize: DimName, - A: DimName, - K: Filter, - T: TrackerSensitivity, - > ODProcess<'a, D, MeasurementSize, A, K, T> + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, + > ODProcess<'a, D, MsrSize, Accel, K, Trk> where D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> - + Allocator - + Allocator::Size> - + Allocator + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { pub fn ckf( prop: PropInstance<'a, D>, kf: K, - devices: BTreeMap, + devices: BTreeMap, resid_crit: Option, almanac: Arc, ) -> Self { @@ -740,7 +738,7 @@ where ekf_trigger: None, init_state, almanac, - _marker: PhantomData::, + _marker: PhantomData::, } } } diff --git a/tests/orbit_determination/mod.rs b/tests/orbit_determination/mod.rs index d2b6fdb3..e3145bd0 100644 --- a/tests/orbit_determination/mod.rs +++ b/tests/orbit_determination/mod.rs @@ -11,6 +11,7 @@ mod measurements; mod multi_body; mod resid_reject; mod robust; +mod robust_az_el; mod simulator; mod spacecraft; mod trackingarc; diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 72e1ceba..67102a5f 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -154,8 +154,8 @@ fn od_robust_test_ekf_realistic_one_way_cov_test(almanac: Arc) { let reloaded = TrackingDataArc::from_parquet(&path).unwrap(); assert_eq!(reloaded, arc); - // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. - // We expect the estimated orbit to be _nearly_ perfect because we"ve removed SATURN_BARYCENTER from the estimated trajectory + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); let setup = Propagator::new(estimator, IntegratorMethod::RungeKutta4, opts); @@ -348,8 +348,8 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { println!("{arc}"); - // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. - // We expect the estimated orbit to be _nearly_ perfect because we"ve removed SATURN_BARYCENTER from the estimated trajectory + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); let setup = Propagator::default(estimator); diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs new file mode 100644 index 00000000..b27aa58e --- /dev/null +++ b/tests/orbit_determination/robust_az_el.rs @@ -0,0 +1,310 @@ +extern crate nyx_space as nyx; +extern crate pretty_env_logger; + +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; +use anise::constants::frames::IAU_EARTH_FRAME; +use nyx::cosmic::Orbit; +use nyx::dynamics::orbital::OrbitalDynamics; +use nyx::dynamics::SpacecraftDynamics; +use nyx::io::ExportCfg; +use nyx::md::StateParameter; +use nyx::od::prelude::*; +use nyx::propagators::{IntegratorOptions, Propagator}; +use nyx::time::{Epoch, TimeUnits, Unit}; +use nyx::utils::rss_orbit_errors; +use nyx::Spacecraft; +use nyx_space::mc::StateDispersion; +use nyx_space::propagators::IntegratorMethod; +use std::collections::BTreeMap; +use std::env; +use std::path::PathBuf; + +use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; +use rstest::*; +use std::sync::Arc; + +#[fixture] +fn almanac() -> Arc { + use crate::test_almanac_arcd; + test_almanac_arcd() +} + +/// Using identically configured ground stations for all tests. +#[fixture] +fn devices(almanac: Arc) -> BTreeMap { + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + let elevation_mask = 10.0; + let integration_time = Some(60 * Unit::Second); + + let dss65_madrid = GroundStation::dss65_madrid( + elevation_mask, + StochasticNoise::MIN, + StochasticNoise::MIN, + iau_earth, + ) + .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .with_integration_time(integration_time); + + let dss34_canberra = GroundStation::dss34_canberra( + elevation_mask, + StochasticNoise::MIN, + StochasticNoise::MIN, + iau_earth, + ) + .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .with_integration_time(integration_time); + + // let dss13_goldstone = GroundStation::dss13_goldstone( + // elevation_mask, + // StochasticNoise::MIN, + // StochasticNoise::MIN, + // iau_earth, + // ) + // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + // .with_integration_time(integration_time); + + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + // devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices +} + +#[fixture] +fn initial_state(almanac: Arc) -> Spacecraft { + // Define state information. + let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let dt = Epoch::from_gregorian_tai_at_midnight(2020, 1, 1); + Spacecraft::from(Orbit::keplerian( + 22000.0, 0.01, 30.0, 80.0, 40.0, 0.0, dt, eme2k, + )) +} + +#[fixture] +fn initial_estimate(initial_state: Spacecraft) -> KfEstimate { + // Disperse the initial state based on some orbital elements errors given from ULA Atlas 5 user guide, table 2.3.3-1 + // This assumes that the errors are ONE TENTH of the values given in the table. It assumes that the launch provider has provided an initial state vector, whose error is lower than the injection errors. + // The initial covariance is computed based on the realized dispersions. + let initial_estimate = KfEstimate::disperse_from_diag( + initial_state, + vec![ + StateDispersion::zero_mean(StateParameter::Inclination, 0.0025), + StateDispersion::zero_mean(StateParameter::RAAN, 0.022), + StateDispersion::zero_mean(StateParameter::AoP, 0.02), + ], + Some(0), + ) + .unwrap(); + + println!("Initial estimate:\n{}", initial_estimate); + + initial_estimate +} + +#[fixture] +fn truth_setup() -> Propagator { + let step_size = 60.0 * Unit::Second; + let opts = IntegratorOptions::with_max_step(step_size); + + let bodies = vec![MOON, SUN, JUPITER_BARYCENTER, SATURN_BARYCENTER]; + let orbital_dyn = OrbitalDynamics::point_masses(bodies); + Propagator::new( + SpacecraftDynamics::new(orbital_dyn), + IntegratorMethod::DormandPrince78, + opts, + ) +} + +#[fixture] +fn estimator_setup() -> Propagator { + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory + + let step_size = 60.0 * Unit::Second; + let opts = IntegratorOptions::with_max_step(step_size); + + let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; + let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); + + Propagator::new(estimator, IntegratorMethod::RungeKutta89, opts) +} + +/* + * These tests check that if we start with a state deviation in the estimate, the filter will eventually converge back. + * These tests do NOT check that the filter will converge if the initial state in the propagator has that state deviation. + * The latter would require iteration and smoothing before playing with an EKF. This will be handled in a subsequent version. +**/ + +#[allow(clippy::identity_op)] +#[rstest] +fn od_robust_test_ekf_rng_dop_az_el( + almanac: Arc, + devices: BTreeMap, + initial_state: Spacecraft, + initial_estimate: KfEstimate, + truth_setup: Propagator, + estimator_setup: Propagator, +) { + let _ = pretty_env_logger::try_init(); + + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + // Define the ground stations. + let ekf_num_meas = 300; + // Set the disable time to be very low to test enable/disable sequence + let ekf_disable_time = 3 * Unit::Minute; + let elevation_mask = 0.0; + + let dss65_madrid = GroundStation::dss65_madrid( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ); + let dss34_canberra = GroundStation::dss34_canberra( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ); + + // Define the tracking configurations + let configs = BTreeMap::from([ + ( + dss65_madrid.name.clone(), + TrkConfig::from_sample_rate(60.seconds()), + ), + ( + dss34_canberra.name.clone(), + TrkConfig::from_sample_rate(60.seconds()), + ), + ]); + + // Define the propagator information. + let prop_time = 2 * Unit::Day; + + let initial_state_dev = initial_estimate.nominal_state; + let (init_rss_pos_km, init_rss_vel_km_s) = + rss_orbit_errors(&initial_state.orbit, &initial_state_dev.orbit); + + println!("Truth initial state:\n{initial_state}\n{initial_state:x}"); + println!("Filter initial state:\n{initial_state_dev}\n{initial_state_dev:x}"); + println!( + "Initial state dev:\t{:.3} m\t{:.3} m/s\n{}", + init_rss_pos_km * 1e3, + init_rss_vel_km_s * 1e3, + (initial_state.orbit - initial_state_dev.orbit).unwrap() + ); + + let (_, traj) = truth_setup + .with(initial_state, almanac.clone()) + .for_duration_with_traj(prop_time) + .unwrap(); + + // Simulate tracking data + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); + arc_sim.build_schedule(almanac.clone()).unwrap(); + + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); + + // And serialize to disk + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "output_data", + "ekf_rng_dpl_az_el.parquet", + ] + .iter() + .collect(); + + arc.to_parquet_simple(&path).unwrap(); + + println!("{arc}\n{arc:?}"); + // Reload + let reloaded = TrackingDataArc::from_parquet(&path).unwrap(); + assert_eq!(reloaded, arc); + + let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); + + // Define the process noise to assume an unmodeled acceleration on X, Y and Z in the EME2000 frame + let sigma_q = 5e-10_f64.powi(2); + let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); + + let kf = KF::new(initial_estimate, process_noise); + + let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); + + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); + + // Let's filter and iterate on the initial subset of the arc to refine the initial estimate + let subset = arc.clone().filter_by_offset(..3.hours()); + let remaining = arc.filter_by_offset(3.hours()..); + + odp.process_arc(&subset).unwrap(); + odp.iterate_arc(&subset, IterationConf::once()).unwrap(); + + let (sm_rss_pos_km, sm_rss_vel_km_s) = + rss_orbit_errors(&initial_state.orbit, &odp.estimates[0].state().orbit); + + println!( + "Initial state error after smoothing:\t{:.3} m\t{:.3} m/s\n{}", + sm_rss_pos_km * 1e3, + sm_rss_vel_km_s * 1e3, + (initial_state.orbit - odp.estimates[0].state().orbit).unwrap() + ); + + odp.process_arc(&remaining).unwrap(); + + odp.to_parquet( + &remaining, + path.with_file_name("robustness_test_one_way.parquet"), + ExportCfg::timestamped(), + ) + .unwrap(); + + // Check that the covariance deflated + let est = &odp.estimates[odp.estimates.len() - 1]; + let final_truth_state = traj.at(est.epoch()).unwrap(); + + println!("Estimate:\n{}", est); + println!("Truth:\n{}", final_truth_state); + println!( + "Delta state with truth (epoch match: {}):\n{}", + final_truth_state.epoch() == est.epoch(), + (final_truth_state.orbit - est.state().orbit).unwrap() + ); + + for i in 0..6 { + if est.covar[(i, i)] < 0.0 { + println!( + "covar diagonal element negative @ [{}, {}] = {:.3e}-- issue #164", + i, + i, + est.covar[(i, i)], + ); + } + } + + assert_eq!( + final_truth_state.epoch(), + est.epoch(), + "time of final EST and TRUTH epochs differ" + ); + let delta = (est.state().orbit - final_truth_state.orbit).unwrap(); + println!( + "RMAG error = {:.6} m\tVMAG error = {:.6} m/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e3 + ); + + assert!( + delta.rmag_km() < 0.06, + "Position error should be less than 50 meters" + ); + assert!( + delta.vmag_km_s() < 2e-4, + "Velocity error should be on centimeter level" + ); +} From a3dc3e19c16555956b80a45136bcb61d43dcef93 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 24 Nov 2024 12:06:32 -0700 Subject: [PATCH 25/38] WIP variable size simultaneous KF processing --- src/od/process/mod.rs | 141 ++++++++++++---------- tests/orbit_determination/robust_az_el.rs | 20 +-- 2 files changed, 89 insertions(+), 72 deletions(-) diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 4198f557..56a19022 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -26,6 +26,7 @@ pub use crate::od::*; use crate::propagators::PropInstance; pub use crate::time::{Duration, Unit}; use anise::prelude::Almanac; +use indexmap::IndexSet; use msr::sensitivity::TrackerSensitivity; use snafu::prelude::*; mod conf; @@ -505,75 +506,91 @@ where } } - // Check that the observation is valid. - for val in msr.observation::(msr_types).iter() { - ensure!( - val.is_finite(), - InvalidMeasurementSnafu { - epoch: next_msr_epoch, - val: *val - } - ); - } + // Perform several measurement updates to ensure the desired dimensionality. + let windows = msr_types.len() / MsrSize::USIZE; + for wno in 0..=windows { + let mut cur_msr_types = IndexSet::new(); + for msr_type in msr_types + .iter() + .copied() + .skip(wno * MsrSize::USIZE) + .take(MsrSize::USIZE) + { + cur_msr_types.insert(msr_type); + } - let h_tilde = device - .h_tilde::( - msr, - msr_types, - &nominal_state, - self.almanac.clone(), - ) - .unwrap(); - - self.kf.update_h_tilde(h_tilde); - - match self.kf.measurement_update( - nominal_state, - &msr.observation(msr_types), - &computed_meas.observation(msr_types), - device.measurement_covar_matrix( - device.measurement_types(), - epoch, - )?, - self.resid_crit, - ) { - Ok((estimate, mut residual)) => { - debug!("processed msr #{msr_cnt} @ {epoch}"); - - residual.tracker = Some(device.name()); - - if !residual.rejected { - msr_accepted_cnt += 1; - } + if cur_msr_types.is_empty() { + // We've processed all measurements. + break; + } - // 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 - // StdEkfTrigger needs to store the time of the previous measurement). - - if let Some(trigger) = &mut self.ekf_trigger { - if trigger.enable_ekf(&estimate) - && !self.kf.is_extended() - { - self.kf.set_extended(true); - if !estimate.within_3sigma() { - warn!("EKF enabled @ {epoch} but filter DIVERGING"); - } else { - info!("EKF enabled @ {epoch}"); - } + // Check that the observation is valid. + for val in msr.observation::(&cur_msr_types).iter() { + ensure!( + val.is_finite(), + InvalidMeasurementSnafu { + epoch: next_msr_epoch, + val: *val + } + ); + } + + let h_tilde = device + .h_tilde::( + msr, + &cur_msr_types, + &nominal_state, + self.almanac.clone(), + ) + .unwrap(); + + self.kf.update_h_tilde(h_tilde); + + match self.kf.measurement_update( + nominal_state, + &msr.observation(&cur_msr_types), + &computed_meas.observation(&cur_msr_types), + device.measurement_covar_matrix(&cur_msr_types, epoch)?, + self.resid_crit, + ) { + Ok((estimate, mut residual)) => { + debug!("processed msr #{msr_cnt} of type(s) {cur_msr_types:?} @ {epoch}"); + + residual.tracker = Some(device.name()); + + if !residual.rejected { + msr_accepted_cnt += 1; } - if self.kf.is_extended() { - self.prop.state = - self.prop.state + estimate.state_deviation(); + + // 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 + // StdEkfTrigger needs to store the time of the previous measurement). + + if let Some(trigger) = &mut self.ekf_trigger { + if trigger.enable_ekf(&estimate) + && !self.kf.is_extended() + { + self.kf.set_extended(true); + if !estimate.within_3sigma() { + warn!("EKF enabled @ {epoch} but filter DIVERGING"); + } else { + info!("EKF enabled @ {epoch}"); + } + } + if self.kf.is_extended() { + self.prop.state = self.prop.state + + estimate.state_deviation(); + } } - } - self.prop.state.reset_stm(); + self.prop.state.reset_stm(); - self.estimates.push(estimate); - self.residuals.push(Some(residual)); + self.estimates.push(estimate); + self.residuals.push(Some(residual)); + } + Err(e) => return Err(e), } - Err(e) => return Err(e), } } else { warn!("Real observation exists @ {epoch} but simulated {} does not see it -- ignoring measurement", msr.tracker); diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index b27aa58e..839391f1 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -56,20 +56,20 @@ fn devices(almanac: Arc) -> BTreeMap { .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) .with_integration_time(integration_time); - // let dss13_goldstone = GroundStation::dss13_goldstone( - // elevation_mask, - // StochasticNoise::MIN, - // StochasticNoise::MIN, - // iau_earth, - // ) - // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - // .with_integration_time(integration_time); + let dss13_goldstone = GroundStation::dss13_goldstone( + elevation_mask, + StochasticNoise::MIN, + StochasticNoise::MIN, + iau_earth, + ) + .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .with_integration_time(integration_time); let mut devices = BTreeMap::new(); devices.insert(dss65_madrid.name(), dss65_madrid); devices.insert(dss34_canberra.name(), dss34_canberra); - // devices.insert(dss13_goldstone.name(), dss13_goldstone); + devices.insert(dss13_goldstone.name(), dss13_goldstone); devices } From 6927ec3603e16e269ea6dbcbf82c111113c8ebc2 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 24 Nov 2024 17:56:12 -0700 Subject: [PATCH 26/38] Fix eporting residuals with all measurement types --- src/od/estimate/residual.rs | 31 ++++++++++++++++++++++- src/od/ground_station/trk_device.rs | 13 +++++++--- src/od/process/export.rs | 21 ++++++++++----- src/od/process/mod.rs | 11 +++++--- tests/orbit_determination/robust_az_el.rs | 17 ++++++++----- 5 files changed, 74 insertions(+), 19 deletions(-) diff --git a/src/od/estimate/residual.rs b/src/od/estimate/residual.rs index 30d3d0b1..4cdd93c4 100644 --- a/src/od/estimate/residual.rs +++ b/src/od/estimate/residual.rs @@ -18,7 +18,9 @@ use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, OVector}; +use crate::od::msr::MeasurementType; use hifitime::Epoch; +use indexmap::IndexSet; use std::fmt; /// Stores an Estimate, as the result of a `time_update` or `measurement_update`. @@ -44,6 +46,8 @@ where pub rejected: bool, /// Name of the tracker that caused this residual pub tracker: Option, + /// Measurement types used to compute this residual (in order) + pub msr_types: IndexSet, } impl Residual @@ -61,6 +65,7 @@ where ratio: 0.0, rejected: true, tracker: None, + msr_types: IndexSet::new(), } } @@ -79,6 +84,7 @@ where tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: true, tracker: None, + msr_types: IndexSet::new(), } } @@ -97,8 +103,30 @@ where tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: false, tracker: None, + msr_types: IndexSet::new(), } } + + /// Returns the prefit for this measurement type, if available + pub fn prefit(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.prefit[idx]) + } + + /// Returns the postfit for this measurement type, if available + pub fn postfit(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.postfit[idx]) + } + + /// Returns the tracker noise for this measurement type, if available + pub fn trk_noise(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.tracker_msr_noise[idx]) + } } impl fmt::Display for Residual @@ -109,7 +137,8 @@ where fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, - "Residual from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}", + "Residual of {:?} from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}", + self.msr_types, self.tracker.as_ref().unwrap_or(&"Unknown".to_string()), self.epoch, self.ratio, diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs index 44f10dab..b2b31e05 100644 --- a/src/od/ground_station/trk_device.rs +++ b/src/od/ground_station/trk_device.rs @@ -48,9 +48,16 @@ impl TrackingDevice for GroundStation { ) -> Result, ODError> { match self.integration_time { Some(integration_time) => { - // If out of traj bounds, return None. - let rx_0 = traj.at(epoch - integration_time).context(ODTrajSnafu)?; - let rx_1 = traj.at(epoch).context(ODTrajSnafu)?; + // If out of traj bounds, return None, else the whole strand is rejected. + let rx_0 = match traj.at(epoch - integration_time) { + Ok(rx) => rx, + Err(_) => return Ok(None), + }; + + let rx_1 = match traj.at(epoch).context(ODTrajSnafu) { + Ok(rx) => rx, + Err(_) => return Ok(None), + }; let obstructing_body = if !self.frame.ephem_origin_match(rx_0.frame()) { Some(rx_0.frame()) diff --git a/src/od/process/export.rs b/src/od/process/export.rs index 8332a410..92001e9c 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -354,11 +354,14 @@ where // Finally, add the residuals. // Prefits - for i in 0..MsrSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.prefit[i]); + match resid.prefit(msr_type) { + Some(prefit) => data.append_value(prefit), + None => data.append_null(), + }; } else { data.append_null(); } @@ -366,11 +369,14 @@ where record.push(Arc::new(data.finish())); } // Postfit - for i in 0..MsrSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.postfit[i]); + match resid.postfit(msr_type) { + Some(postfit) => data.append_value(postfit), + None => data.append_null(), + }; } else { data.append_null(); } @@ -378,11 +384,14 @@ where record.push(Arc::new(data.finish())); } // Measurement noise - for i in 0..MsrSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.tracker_msr_noise[i]); + match resid.trk_noise(msr_type) { + Some(noise) => data.append_value(noise), + None => data.append_null(), + }; } else { data.append_null(); } diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 56a19022..180f7313 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -508,6 +508,7 @@ where // Perform several measurement updates to ensure the desired dimensionality. let windows = msr_types.len() / MsrSize::USIZE; + let mut msr_rejected = false; for wno in 0..=windows { let mut cur_msr_types = IndexSet::new(); for msr_type in msr_types @@ -554,12 +555,13 @@ where self.resid_crit, ) { Ok((estimate, mut residual)) => { - debug!("processed msr #{msr_cnt} of type(s) {cur_msr_types:?} @ {epoch}"); + debug!("processed measurement #{msr_cnt} for {cur_msr_types:?} @ {epoch} from {}", device.name()); residual.tracker = Some(device.name()); + residual.msr_types = cur_msr_types; - if !residual.rejected { - msr_accepted_cnt += 1; + if residual.rejected { + msr_rejected = true; } // Switch to EKF if necessary, and update the dynamics and such @@ -592,6 +594,9 @@ where Err(e) => return Err(e), } } + if !msr_rejected { + msr_accepted_cnt += 1; + } } else { warn!("Real observation exists @ {epoch} but simulated {} does not see it -- ignoring measurement", msr.tracker); } diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index 839391f1..6385cd47 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -130,7 +130,7 @@ fn estimator_setup() -> Propagator { let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); - Propagator::new(estimator, IntegratorMethod::RungeKutta89, opts) + Propagator::new(estimator, IntegratorMethod::DormandPrince78, opts) } /* @@ -153,7 +153,7 @@ fn od_robust_test_ekf_rng_dop_az_el( let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); // Define the ground stations. - let ekf_num_meas = 300; + let ekf_num_meas = 3000; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 3 * Unit::Minute; let elevation_mask = 0.0; @@ -214,7 +214,7 @@ fn od_robust_test_ekf_rng_dop_az_el( let path: PathBuf = [ env!("CARGO_MANIFEST_DIR"), "output_data", - "ekf_rng_dpl_az_el.parquet", + "ekf_rng_dpl_az_el_arc.parquet", ] .iter() .collect(); @@ -245,21 +245,26 @@ fn od_robust_test_ekf_rng_dop_az_el( odp.process_arc(&subset).unwrap(); odp.iterate_arc(&subset, IterationConf::once()).unwrap(); + // Grab the comparison state, which differs from the initial state because of the integration time of the measurements. + let cmp_state = traj + .at(odp.estimates[0].state().orbit.epoch) + .expect("could not find comparison epoch in trajectory"); + let (sm_rss_pos_km, sm_rss_vel_km_s) = - rss_orbit_errors(&initial_state.orbit, &odp.estimates[0].state().orbit); + rss_orbit_errors(&cmp_state.orbit, &odp.estimates[0].state().orbit); println!( "Initial state error after smoothing:\t{:.3} m\t{:.3} m/s\n{}", sm_rss_pos_km * 1e3, sm_rss_vel_km_s * 1e3, - (initial_state.orbit - odp.estimates[0].state().orbit).unwrap() + (cmp_state.orbit - odp.estimates[0].state().orbit).unwrap() ); odp.process_arc(&remaining).unwrap(); odp.to_parquet( &remaining, - path.with_file_name("robustness_test_one_way.parquet"), + path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), ExportCfg::timestamped(), ) .unwrap(); From 9cc143f7e8563c472a6aa85fcc7f53dbd2c03d07 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 24 Nov 2024 18:13:13 -0700 Subject: [PATCH 27/38] Status: Currently have a bug where there are two elevation residuals at the same time. Not sure what is happening there. --- examples/04_lro_od/plot_od_rslt.py | 54 ++++++++++++++++-------------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index df17ea7f..7185d6cd 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -5,47 +5,48 @@ import plotly.express as px if __name__ == "__main__": - df = pl.read_parquet("./04_lro_od_results.parquet") + df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp-2024-11-25T00-53-33.parquet") df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( "Epoch (UTC)", descending=False ) - # Add the +/- 3 sigmas on measurement noise - df = df.with_columns( - [ - (3.0 * pl.col("Measurement noise: Range (km)")).alias( - "Measurement noise 3-Sigma: Range (km)" - ), - (-3.0 * pl.col("Measurement noise: Range (km)")).alias( - "Measurement noise -3-Sigma: Range (km)" - ), - ] - ) - df = df.with_columns( - [ - (3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( - "Measurement noise 3-Sigma: Doppler (km/s)" - ), - (-3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( - "Measurement noise -3-Sigma: Doppler (km/s)" - ), - ] - ) + + all_msr_types = ["Range (km)", "Doppler (km/s)", "Azimuth (deg)", "Elevation (deg)"] + msr_type_count = 0 + msr_types = [] + + for msr_type in all_msr_types: + if f"Measurement noise: {msr_type}" in df.columns: + msr_type_count += 1 + msr_types += [msr_type] + # Add the +/- 3 sigmas on measurement noise + df = df.with_columns( + [ + (3.0 * pl.col(f"Measurement noise: {msr_type}")).alias( + f"Measurement noise 3-Sigma: {msr_type}" + ), + (-3.0 * pl.col(f"Measurement noise: {msr_type}")).alias( + f"Measurement noise -3-Sigma: {msr_type}" + ), + ] + ) # == Residual plots == # Nyx uses the Mahalanobis distance for the residual ratios, so we test the goodness using the Chi Square distribution. - freedoms = 2 # Two degrees of freedoms for the range and the range rate. + freedoms = msr_type_count # Two degrees of freedoms for the range and the range rate. x_chi = np.linspace(chi2.ppf(0.01, freedoms), chi2.ppf(0.99, freedoms), 100) y_chi = chi2.pdf(x_chi, freedoms) # Compute the scaling factor hist = np.histogram(df["Residual ratio"].fill_null(0.0), bins=50)[0] - max_hist = max(hist[1:]) # Ignore the bin of zeros + max_hist = max(hist[1:]) # Ignore the bin of zeros max_chi2_pdf = max(y_chi) scale_factor = max_hist / max_chi2_pdf fig = go.Figure() - fig.add_trace(go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared")) + fig.add_trace( + go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") + ) fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) fig.show() @@ -63,7 +64,7 @@ df_resid_ok = df.filter(df["Residual Rejected"] == False) # Plot the measurement residuals and their noises. - for msr in ["Range (km)", "Doppler (km/s)"]: + for msr in msr_types: y_cols = [ f"{col}: {msr}" for col in [ @@ -101,6 +102,7 @@ y=["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"], ).show() + raise AssertionError("stop") # Load the RIC diff. for fname, errname in [ ("04_lro_od_truth_error", "OD vs Flown"), From 3881e2125ed7d8f342d592947e6249b840b94d31 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Thu, 28 Nov 2024 10:51:02 -0700 Subject: [PATCH 28/38] Fixed sensitivity of azimuth/elevation + fixed duplicates in export --- examples/04_lro_od/plot_od_rslt.py | 2 +- src/od/msr/sensitivity.rs | 78 +++++++++++++++++++---- src/od/process/mod.rs | 13 ++-- tests/orbit_determination/robust_az_el.rs | 17 +++-- 4 files changed, 90 insertions(+), 20 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 7185d6cd..f2b895e9 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -5,7 +5,7 @@ import plotly.express as px if __name__ == "__main__": - df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp-2024-11-25T00-53-33.parquet") + df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp.parquet") df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( "Epoch (UTC)", descending=False diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index f9457c52..6813de55 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -23,7 +23,7 @@ use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; use indexmap::IndexSet; -use nalgebra::{DimName, OMatrix, U1}; +use nalgebra::{Const, DimName, OMatrix, U1}; use snafu::ResultExt; use std::marker::PhantomData; use std::sync::Arc; @@ -130,14 +130,57 @@ impl ScalarSensitivityT // Compute the device location in the receiver frame because we compute the sensitivity in that frame. // This frame is required because the scalar measurements are frame independent, but the sensitivity // must be in the estimation frame. + // let transmitter = tx + // .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) + // .context(ODAlmanacSnafu { + // action: "computing transmitter location when computing sensitivity matrix", + // })?; + + // let delta_r = receiver.radius_km - transmitter.radius_km; + // let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + + let receiver = rx.orbit; + // Compute the device location ensuring that the receiver and transmitter are in the same frame. let transmitter = tx .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) .context(ODAlmanacSnafu { action: "computing transmitter location when computing sensitivity matrix", })?; - let delta_r = receiver.radius_km - transmitter.radius_km; - let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + // Should this be in the SEZ frame? + // let delta_r = receiver.radius_km - transmitter.radius_km; + // let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + + // SEZ DCM is topo to fixed + let sez_dcm = transmitter + .dcm_from_topocentric_to_body_fixed(100000) + .unwrap(); + // .context(EphemerisPhysicsSnafu { action: "" }) + // .context(EphemerisSnafu { + // action: "computing SEZ DCM for AER", + // })?; + + let tx_sez = (sez_dcm.transpose() * transmitter).unwrap(); + // .context(EphemerisPhysicsSnafu { action: "" }) + // .context(EphemerisSnafu { + // action: "transforming transmitter to SEZ", + // })?; + + // Convert the receiver into the transmitter frame. + let rx_in_tx_frame = almanac + .transform_to(rx.orbit, transmitter.frame, None) + .unwrap(); + // Convert into SEZ frame + let rx_sez = (sez_dcm.transpose() * rx_in_tx_frame).unwrap(); + // .context(EphemerisPhysicsSnafu { action: "" }) + // .context(EphemerisSnafu { + // action: "transforming received to SEZ", + // })?; + + // let delta_r = rx_sez.radius_km - tx_sez.radius_km; + // let delta_v = rx_sez.velocity_km_s - tx_sez.velocity_km_s; + let delta_r = tx_sez.radius_km; + let delta_v = tx_sez.velocity_km_s; match msr_type { MeasurementType::Doppler => { @@ -194,10 +237,14 @@ impl ScalarSensitivityT let m12 = delta_r.x / (delta_r.x.powi(2) + delta_r.y.powi(2)); let m13 = 0.0; - let sensitivity_row = - OMatrix::::Size>::from_row_slice(&[ - m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - ]); + let effective_sensitivity_row = + OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) + * sez_dcm.state_dcm(); + + let mut sensitivity_row = OMatrix::::Size>::zeros(); + for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { + sensitivity_row[i] = val; + } Ok(Self { sensitivity_row, @@ -212,10 +259,19 @@ impl ScalarSensitivityT let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt()); let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; - let sensitivity_row = - OMatrix::::Size>::from_row_slice(&[ - m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - ]); + let effective_sensitivity_row = + OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) + * sez_dcm.state_dcm(); + + let mut sensitivity_row = OMatrix::::Size>::zeros(); + for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { + sensitivity_row[i] = val; + } + + // let sensitivity_row = + // OMatrix::::Size>::from_row_slice(&[ + // m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + // ]); Ok(Self { sensitivity_row, diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 180f7313..48feef2a 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -526,12 +526,14 @@ where } // Check that the observation is valid. - for val in msr.observation::(&cur_msr_types).iter() { + for val in + msr.observation::(&cur_msr_types).iter().copied() + { ensure!( val.is_finite(), InvalidMeasurementSnafu { epoch: next_msr_epoch, - val: *val + val } ); } @@ -598,11 +600,14 @@ where msr_accepted_cnt += 1; } } else { - warn!("Real observation exists @ {epoch} but simulated {} does not see it -- ignoring measurement", msr.tracker); + warn!("Ignoring observation @ {epoch} because simulated {} does not expect it", msr.tracker); } } None => { - error!("Tracking arc references {} which is not in the list of configured devices", msr.tracker) + error!( + "Tracker {} is not in the list of configured devices", + msr.tracker + ) } } diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index 6385cd47..a0f2733d 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -44,6 +44,8 @@ fn devices(almanac: Arc) -> BTreeMap { ) .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .without_msr_type(MeasurementType::Range) + .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let dss34_canberra = GroundStation::dss34_canberra( @@ -54,6 +56,8 @@ fn devices(almanac: Arc) -> BTreeMap { ) .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .without_msr_type(MeasurementType::Range) + .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let dss13_goldstone = GroundStation::dss13_goldstone( @@ -64,6 +68,8 @@ fn devices(almanac: Arc) -> BTreeMap { ) .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + .without_msr_type(MeasurementType::Range) + .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let mut devices = BTreeMap::new(); @@ -78,7 +84,7 @@ fn devices(almanac: Arc) -> BTreeMap { fn initial_state(almanac: Arc) -> Spacecraft { // Define state information. let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); - let dt = Epoch::from_gregorian_tai_at_midnight(2020, 1, 1); + let dt = Epoch::from_gregorian_utc_at_midnight(2020, 1, 1); Spacecraft::from(Orbit::keplerian( 22000.0, 0.01, 30.0, 80.0, 40.0, 0.0, dt, eme2k, )) @@ -184,7 +190,7 @@ fn od_robust_test_ekf_rng_dop_az_el( ]); // Define the propagator information. - let prop_time = 2 * Unit::Day; + let prop_time = 0.2 * Unit::Day; let initial_state_dev = initial_estimate.nominal_state; let (init_rss_pos_km, init_rss_vel_km_s) = @@ -236,7 +242,10 @@ fn od_robust_test_ekf_rng_dop_az_el( let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf( + prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), + almanac, + ); // Let's filter and iterate on the initial subset of the arc to refine the initial estimate let subset = arc.clone().filter_by_offset(..3.hours()); @@ -265,7 +274,7 @@ fn od_robust_test_ekf_rng_dop_az_el( odp.to_parquet( &remaining, path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), - ExportCfg::timestamped(), + ExportCfg::default(), ) .unwrap(); From 6c3fd084335a9651464aef80ab2050cf37d7fe30 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Thu, 28 Nov 2024 17:36:48 -0700 Subject: [PATCH 29/38] Debugging in progress --- src/od/filter/kalman.rs | 7 ++- src/od/msr/sensitivity.rs | 56 +++++------------ tests/orbit_determination/robust_az_el.rs | 76 ++++++++++++++--------- 3 files changed, 66 insertions(+), 73 deletions(-) diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 3ebca8ce..0a9510e2 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -299,8 +299,11 @@ where // Compute the prefit ratio for the automatic rejection let r_k_inv = r_k.clone().try_inverse().ok_or(ODError::SingularNoiseRk)?; - let ratio_mat = prefit.transpose() * r_k_inv * &prefit; - let ratio = ratio_mat[0].sqrt(); + + let ratio_mat = prefit.transpose() * &r_k_inv * &prefit; + let ratio = dbg!(ratio_mat[0].powi(2)); + let o_r_k = r_k[(0, 0)]; + dbg!(prefit[0], o_r_k, prefit[0].abs() < o_r_k,); if let Some(resid_reject) = resid_rejection { if ratio > resid_reject.num_sigmas { diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 6813de55..efec9d11 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -127,29 +127,18 @@ impl ScalarSensitivityT almanac: Arc, ) -> Result { let receiver = rx.orbit; + // Compute the device location in the receiver frame because we compute the sensitivity in that frame. // This frame is required because the scalar measurements are frame independent, but the sensitivity // must be in the estimation frame. - // let transmitter = tx - // .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) - // .context(ODAlmanacSnafu { - // action: "computing transmitter location when computing sensitivity matrix", - // })?; - - // let delta_r = receiver.radius_km - transmitter.radius_km; - // let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; - - let receiver = rx.orbit; - // Compute the device location ensuring that the receiver and transmitter are in the same frame. let transmitter = tx .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) .context(ODAlmanacSnafu { action: "computing transmitter location when computing sensitivity matrix", })?; - // Should this be in the SEZ frame? - // let delta_r = receiver.radius_km - transmitter.radius_km; - // let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + let delta_r = receiver.radius_km - transmitter.radius_km; + let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; // SEZ DCM is topo to fixed let sez_dcm = transmitter @@ -166,22 +155,6 @@ impl ScalarSensitivityT // action: "transforming transmitter to SEZ", // })?; - // Convert the receiver into the transmitter frame. - let rx_in_tx_frame = almanac - .transform_to(rx.orbit, transmitter.frame, None) - .unwrap(); - // Convert into SEZ frame - let rx_sez = (sez_dcm.transpose() * rx_in_tx_frame).unwrap(); - // .context(EphemerisPhysicsSnafu { action: "" }) - // .context(EphemerisSnafu { - // action: "transforming received to SEZ", - // })?; - - // let delta_r = rx_sez.radius_km - tx_sez.radius_km; - // let delta_v = rx_sez.velocity_km_s - tx_sez.velocity_km_s; - let delta_r = tx_sez.radius_km; - let delta_v = tx_sez.velocity_km_s; - match msr_type { MeasurementType::Doppler => { // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range. @@ -233,10 +206,13 @@ impl ScalarSensitivityT }) } MeasurementType::Azimuth => { - let m11 = -delta_r.y / (delta_r.x.powi(2) + delta_r.y.powi(2)); - let m12 = delta_r.x / (delta_r.x.powi(2) + delta_r.y.powi(2)); + let denom = tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2); + let m11 = -tx_sez.radius_km.y / denom; + let m12 = tx_sez.radius_km.x / denom; let m13 = 0.0; + // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. + let effective_sensitivity_row = OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) * sez_dcm.state_dcm(); @@ -253,11 +229,14 @@ impl ScalarSensitivityT }) } MeasurementType::Elevation => { - let r2 = delta_r.norm().powi(2); + let r2 = tx_sez.radius_km.norm().powi(2); + let z2 = tx_sez.radius_km.z.powi(2); + + // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. - let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt()); - let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt()); - let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; + let m11 = -(tx_sez.radius_km.x * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt()); + let m12 = -(tx_sez.radius_km.y * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt()); + let m13 = (tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2)).sqrt() / r2; let effective_sensitivity_row = OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) @@ -268,11 +247,6 @@ impl ScalarSensitivityT sensitivity_row[i] = val; } - // let sensitivity_row = - // OMatrix::::Size>::from_row_slice(&[ - // m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - // ]); - Ok(Self { sensitivity_row, _rx: PhantomData::<_>, diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index a0f2733d..10e3dd8d 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -3,6 +3,7 @@ extern crate pretty_env_logger; use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::Const; use nyx::cosmic::Orbit; use nyx::dynamics::orbital::OrbitalDynamics; use nyx::dynamics::SpacecraftDynamics; @@ -34,42 +35,42 @@ fn almanac() -> Arc { fn devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 10.0; - let integration_time = Some(60 * Unit::Second); + let integration_time = None; //Some(60 * Unit::Second); let dss65_madrid = GroundStation::dss65_madrid( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ) - .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - .without_msr_type(MeasurementType::Range) - .without_msr_type(MeasurementType::Doppler) + // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + // .without_msr_type(MeasurementType::Range) + // .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let dss34_canberra = GroundStation::dss34_canberra( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ) - .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - .without_msr_type(MeasurementType::Range) - .without_msr_type(MeasurementType::Doppler) + // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + // .without_msr_type(MeasurementType::Range) + // .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let dss13_goldstone = GroundStation::dss13_goldstone( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ) - .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - .without_msr_type(MeasurementType::Range) - .without_msr_type(MeasurementType::Doppler) + // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) + // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) + // .without_msr_type(MeasurementType::Range) + // .without_msr_type(MeasurementType::Doppler) .with_integration_time(integration_time); let mut devices = BTreeMap::new(); @@ -133,7 +134,7 @@ fn estimator_setup() -> Propagator { let step_size = 60.0 * Unit::Second; let opts = IntegratorOptions::with_max_step(step_size); - let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; + let bodies = vec![MOON, SUN, JUPITER_BARYCENTER, SATURN_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); Propagator::new(estimator, IntegratorMethod::DormandPrince78, opts) @@ -159,7 +160,7 @@ fn od_robust_test_ekf_rng_dop_az_el( let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); // Define the ground stations. - let ekf_num_meas = 3000; + let ekf_num_meas = 10; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 3 * Unit::Minute; let elevation_mask = 0.0; @@ -190,7 +191,7 @@ fn od_robust_test_ekf_rng_dop_az_el( ]); // Define the propagator information. - let prop_time = 0.2 * Unit::Day; + let prop_time = 1.1 * initial_state.orbit.period().unwrap(); let initial_state_dev = initial_estimate.nominal_state; let (init_rss_pos_km, init_rss_vel_km_s) = @@ -242,17 +243,32 @@ fn od_robust_test_ekf_rng_dop_az_el( let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = SpacecraftODProcess::ekf( - prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), + let mut odp = ODProcess::< + SpacecraftDynamics, + Const<1>, + Const<3>, + KF, Const<1>>, + GroundStation, + >::ekf( + prop_est, + kf, + devices, + trig, + Some(ResidRejectCrit::default()), almanac, ); + // let mut odp = SpacecraftODProcess::ekf( + // prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), + // almanac, + // ); + // Let's filter and iterate on the initial subset of the arc to refine the initial estimate - let subset = arc.clone().filter_by_offset(..3.hours()); - let remaining = arc.filter_by_offset(3.hours()..); + // let subset = arc.clone().filter_by_offset(..3.hours()); + // let remaining = arc.filter_by_offset(3.hours()..); - odp.process_arc(&subset).unwrap(); - odp.iterate_arc(&subset, IterationConf::once()).unwrap(); + odp.process_arc(&arc).unwrap(); + odp.iterate_arc(&arc, IterationConf::once()).unwrap(); // Grab the comparison state, which differs from the initial state because of the integration time of the measurements. let cmp_state = traj @@ -269,10 +285,10 @@ fn od_robust_test_ekf_rng_dop_az_el( (cmp_state.orbit - odp.estimates[0].state().orbit).unwrap() ); - odp.process_arc(&remaining).unwrap(); + // odp.process_arc(&remaining).unwrap(); odp.to_parquet( - &remaining, + &arc, path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), ExportCfg::default(), ) From 63006e18a0749fd0660da0ab72ca1e4299df8dac Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sat, 30 Nov 2024 23:41:37 -0700 Subject: [PATCH 30/38] Still debugging ... status follows With the code on master, the prefits are within the measurement noise bounds and running with range+doppler simultaneously or sequentially leads to very similar results. --- examples/04_lro_od/plot_od_rslt.py | 28 +++++++++++------------ src/od/noise/mod.rs | 6 +++-- tests/orbit_determination/robust_az_el.rs | 8 ++----- 3 files changed, 20 insertions(+), 22 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index f2b895e9..930bb2aa 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -43,20 +43,20 @@ max_chi2_pdf = max(y_chi) scale_factor = max_hist / max_chi2_pdf - fig = go.Figure() - fig.add_trace( - go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") - ) - fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) - fig.show() - - px.histogram( - df, - x="Residual ratio", - color="Tracker", - marginal="rug", # can be `box`, `violin` - hover_data=df.columns, - ).show() + # fig = go.Figure() + # fig.add_trace( + # go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") + # ) + # fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) + # fig.show() + + # px.histogram( + # df, + # x="Residual ratio", + # color="Tracker", + # marginal="rug", # can be `box`, `violin` + # hover_data=df.columns, + # ).show() # Plot the residual ratios and whether they were accepted. px.scatter(df, x="Epoch (UTC)", y="Residual ratio", color="Residual Rejected").show() diff --git a/src/od/noise/mod.rs b/src/od/noise/mod.rs index 5d02eed9..74e3e944 100644 --- a/src/od/noise/mod.rs +++ b/src/od/noise/mod.rs @@ -78,7 +78,8 @@ impl StochasticNoise { sigma: 2.0e-3, // 2 m ..Default::default() }), - bias: Some(GaussMarkov::default_range_km()), + // bias: Some(GaussMarkov::default_range_km()), + bias: None, } } @@ -89,7 +90,8 @@ impl StochasticNoise { sigma: 3e-6, // 3 mm/s ..Default::default() }), - bias: Some(GaussMarkov::default_doppler_km_s()), + // bias: Some(GaussMarkov::default_doppler_km_s()), + bias: None, } } diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index 10e3dd8d..ee40ea8d 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -250,12 +250,8 @@ fn od_robust_test_ekf_rng_dop_az_el( KF, Const<1>>, GroundStation, >::ekf( - prop_est, - kf, - devices, - trig, - Some(ResidRejectCrit::default()), - almanac, + prop_est, kf, devices, trig, // Some(ResidRejectCrit::default()), + None, almanac, ); // let mut odp = SpacecraftODProcess::ekf( From 82db8c058c6ee1a23139135daf24dff3e7e20fa6 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 1 Dec 2024 00:24:18 -0700 Subject: [PATCH 31/38] Better prefit ratio computation --- src/od/filter/kalman.rs | 23 ++++++++++++++++------- tests/orbit_determination/robust_az_el.rs | 12 ++++++++---- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 0a9510e2..575e2620 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -297,13 +297,22 @@ where // Compute observation deviation (usually marked as y_i) let prefit = real_obs - computed_obs; - // Compute the prefit ratio for the automatic rejection - let r_k_inv = r_k.clone().try_inverse().ok_or(ODError::SingularNoiseRk)?; - - let ratio_mat = prefit.transpose() * &r_k_inv * &prefit; - let ratio = dbg!(ratio_mat[0].powi(2)); - let o_r_k = r_k[(0, 0)]; - dbg!(prefit[0], o_r_k, prefit[0].abs() < o_r_k,); + // 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. + let r_k_chol_inv = r_k + .clone() + .cholesky() + .ok_or(ODError::SingularNoiseRk)? + .l() + .try_inverse() + .ok_or(ODError::SingularNoiseRk)?; + + // Then we multiply the inverted r_k matrix with prefit, giving us the vector of + // ratios of the prefit over the r_k matrix diagonal. + let ratio_vec = r_k_chol_inv * &prefit; + // Compute the ratio as the dot product. + let ratio = ratio_vec.dot(&ratio_vec); if let Some(resid_reject) = resid_rejection { if ratio > resid_reject.num_sigmas { diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index ee40ea8d..222537bd 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -245,13 +245,17 @@ fn od_robust_test_ekf_rng_dop_az_el( let mut odp = ODProcess::< SpacecraftDynamics, - Const<1>, + Const<2>, Const<3>, - KF, Const<1>>, + KF, Const<2>>, GroundStation, >::ekf( - prop_est, kf, devices, trig, // Some(ResidRejectCrit::default()), - None, almanac, + prop_est, + kf, + devices, + trig, + Some(ResidRejectCrit::default()), + almanac, ); // let mut odp = SpacecraftODProcess::ekf( From ab832acdba99d510023cac8e63217b5991ae5eec Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 1 Dec 2024 18:34:23 -0700 Subject: [PATCH 32/38] Add h tilde verification test, looks decent Azimuth error is higher than elevation, which is higher than range, and which is higher than Doppler. Will try to improve --- tests/orbit_determination/measurements.rs | 83 ++++++++++++++++++++++- tests/orbit_determination/two_body.rs | 8 +++ 2 files changed, 90 insertions(+), 1 deletion(-) diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index 496ae7a1..7319f814 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -4,7 +4,7 @@ use anise::constants::celestial_objects::{EARTH, MOON, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; use indexmap::{IndexMap, IndexSet}; -use nalgebra::U2; +use nalgebra::{Const, U2}; use nyx::cosmic::Orbit; use nyx::dynamics::SpacecraftDynamics; use nyx::od::prelude::*; @@ -16,6 +16,7 @@ use rand_pcg::Pcg64Mcg; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; +use sensitivity::TrackerSensitivity; use std::sync::Arc; #[fixture] @@ -279,3 +280,83 @@ fn val_measurements_topo(almanac: Arc) { ); } } + +/// Verifies that the sensitivity matrix is reasonably well. +#[allow(clippy::identity_op)] +#[rstest] +fn verif_sensitivity_mat(almanac: Arc) { + use self::nyx::cosmic::Orbit; + use self::nyx::md::prelude::*; + use self::nyx::od::prelude::*; + use std::str::FromStr; + + let cislunar1 = Orbit::cartesian( + 58643.769540, + -61696.435624, + -36178.745722, + 2.148654, + -1.202489, + -0.714016, + Epoch::from_str("2022-11-16T13:35:31.0 UTC").unwrap(), + almanac.frame_from_uid(EARTH_J2000).unwrap(), + ); + + let cislunar_sc: Spacecraft = cislunar1.into(); + + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + + let mut dss65_madrid = + GroundStation::dss65_madrid(0.0, StochasticNoise::ZERO, StochasticNoise::ZERO, iau_earth) + .with_msr_type(MeasurementType::Azimuth, StochasticNoise::ZERO) + .with_msr_type(MeasurementType::Elevation, StochasticNoise::ZERO); + + let mut cislunar_sc_pert = cislunar_sc; + cislunar_sc_pert.orbit.radius_km.x += 1.0; + cislunar_sc_pert.orbit.radius_km.y -= 1.0; + cislunar_sc_pert.orbit.radius_km.z += 1.0; + cislunar_sc_pert.orbit.velocity_km_s.x += 1.0e-3; + cislunar_sc_pert.orbit.velocity_km_s.y -= 1.0e-3; + cislunar_sc_pert.orbit.velocity_km_s.z += 1.0e-3; + + let truth_meas = dss65_madrid + .measure_instantaneous(cislunar_sc, None, almanac.clone()) + .expect("successful measurement") + .expect("a measurement"); + + let pert_meas = dss65_madrid + .measure_instantaneous(cislunar_sc_pert, None, almanac.clone()) + .expect("successful measurement") + .expect("a measurement"); + + for msr_type in [ + MeasurementType::Range, + MeasurementType::Doppler, + MeasurementType::Elevation, + MeasurementType::Azimuth, + ] { + let mut msr_types = IndexSet::new(); + msr_types.insert(msr_type); + + let truth_obs = truth_meas.observation::>(&msr_types); + + let pert_obs = pert_meas.observation::>(&msr_types); + + // Given this observation, feed it to the sensitivity matrix, and we should find the original state. + + let h_tilde = dss65_madrid + .h_tilde::>(&truth_meas, &msr_types, &cislunar_sc, almanac.clone()) + .expect("sensitivity should not fail"); + + let delta_state = cislunar_sc.to_vector().fixed_rows::<9>(0) + - cislunar_sc_pert.to_vector().fixed_rows::<9>(0); + + let delta_obs = h_tilde * delta_state; + let computed_obs = truth_obs - delta_obs; + + let sensitivity_error = (pert_obs - computed_obs)[0]; + println!( + "{msr_type:?} error = {sensitivity_error:.3e} {}", + msr_type.unit() + ); + } +} diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 9b8ab581..5565d365 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -525,6 +525,14 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( ); } + for res in odp.residuals.iter().flatten() { + assert!( + res.prefit.norm() < 1e-12, + "prefit should be zero (perfect dynamics) ({:e})", + res + ); + } + for res in odp.residuals.iter().flatten() { assert!( res.postfit.norm() < 1e-12, From dfa3cf5371e9eb4906d0d49cc848eee89edf42d0 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 1 Dec 2024 23:30:08 -0700 Subject: [PATCH 33/38] Az/el measurement types work, but noisy Residual ratios are still too optimistic as a denominator. --- examples/04_lro_od/plot_od_rslt.py | 29 +-- src/od/msr/sensitivity.rs | 48 ++-- src/od/noise/mod.rs | 18 +- tests/orbit_determination/measurements.rs | 2 + tests/orbit_determination/robust_az_el.rs | 46 ++-- tests/orbit_determination/two_body.rs | 271 +++++++++++++++++++++- 6 files changed, 355 insertions(+), 59 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 930bb2aa..2d854014 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -17,6 +17,7 @@ for msr_type in all_msr_types: if f"Measurement noise: {msr_type}" in df.columns: + print(f"Found data for {msr_type}") msr_type_count += 1 msr_types += [msr_type] # Add the +/- 3 sigmas on measurement noise @@ -43,20 +44,20 @@ max_chi2_pdf = max(y_chi) scale_factor = max_hist / max_chi2_pdf - # fig = go.Figure() - # fig.add_trace( - # go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") - # ) - # fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) - # fig.show() - - # px.histogram( - # df, - # x="Residual ratio", - # color="Tracker", - # marginal="rug", # can be `box`, `violin` - # hover_data=df.columns, - # ).show() + fig = go.Figure() + fig.add_trace( + go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") + ) + fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) + fig.show() + + px.histogram( + df, + x="Residual ratio", + color="Tracker", + marginal="rug", # can be `box`, `violin` + hover_data=df.columns, + ).show() # Plot the residual ratios and whether they were accepted. px.scatter(df, x="Epoch (UTC)", y="Residual ratio", color="Residual Rejected").show() diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index efec9d11..71a2b7ac 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -141,15 +141,26 @@ impl ScalarSensitivityT let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; // SEZ DCM is topo to fixed - let sez_dcm = transmitter - .dcm_from_topocentric_to_body_fixed(100000) - .unwrap(); + // let sez_dcm = transmitter + // .dcm_from_topocentric_to_body_fixed(100000) + // .unwrap(); // .context(EphemerisPhysicsSnafu { action: "" }) // .context(EphemerisSnafu { // action: "computing SEZ DCM for AER", // })?; - let tx_sez = (sez_dcm.transpose() * transmitter).unwrap(); + // let tx_sez = (sez_dcm.transpose() * transmitter).unwrap(); + // let rx_sez = (sez_dcm.transpose() * rx.orbit).unwrap(); + + /* + Range error = 1.265e-5 km + Doppler error = 2.800e-8 km/s + Elevation error = 7.491e-4 deg + Azimuth error = -5.718e-4 deg */ + + // let delta_r = rx_sez.radius_km - tx_sez.radius_km; + // let delta_v = rx_sez.velocity_km_s - tx_sez.velocity_km_s; + // .context(EphemerisPhysicsSnafu { action: "" }) // .context(EphemerisSnafu { // action: "transforming transmitter to SEZ", @@ -206,16 +217,19 @@ impl ScalarSensitivityT }) } MeasurementType::Azimuth => { - let denom = tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2); - let m11 = -tx_sez.radius_km.y / denom; - let m12 = tx_sez.radius_km.x / denom; + // let denom = tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2); + // let m11 = -tx_sez.radius_km.y / denom; + // let m12 = tx_sez.radius_km.x / denom; + let denom = delta_r.x.powi(2) + delta_r.y.powi(2); + let m11 = -delta_r.y / denom; + let m12 = delta_r.x / denom; let m13 = 0.0; // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. let effective_sensitivity_row = - OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) - * sez_dcm.state_dcm(); + OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]); + // * sez_dcm.state_dcm(); let mut sensitivity_row = OMatrix::::Size>::zeros(); for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { @@ -229,18 +243,20 @@ impl ScalarSensitivityT }) } MeasurementType::Elevation => { - let r2 = tx_sez.radius_km.norm().powi(2); - let z2 = tx_sez.radius_km.z.powi(2); + // let r2 = tx_sez.radius_km.norm().powi(2); + // let z2 = tx_sez.radius_km.z.powi(2); + let r2 = delta_r.norm().powi(2); + let z2 = delta_r.z.powi(2); // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. - let m11 = -(tx_sez.radius_km.x * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt()); - let m12 = -(tx_sez.radius_km.y * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt()); - let m13 = (tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2)).sqrt() / r2; + let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - z2).sqrt()); + let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - z2).sqrt()); + let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; let effective_sensitivity_row = - OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) - * sez_dcm.state_dcm(); + OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]); + // * sez_dcm.state_dcm(); let mut sensitivity_row = OMatrix::::Size>::zeros(); for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { diff --git a/src/od/noise/mod.rs b/src/od/noise/mod.rs index 74e3e944..b32d7332 100644 --- a/src/od/noise/mod.rs +++ b/src/od/noise/mod.rs @@ -78,8 +78,7 @@ impl StochasticNoise { sigma: 2.0e-3, // 2 m ..Default::default() }), - // bias: Some(GaussMarkov::default_range_km()), - bias: None, + bias: Some(GaussMarkov::default_range_km()), } } @@ -90,8 +89,19 @@ impl StochasticNoise { sigma: 3e-6, // 3 mm/s ..Default::default() }), - // bias: Some(GaussMarkov::default_doppler_km_s()), - bias: None, + bias: Some(GaussMarkov::default_doppler_km_s()), + } + } + + /// Default stochastic process for an angle measurement (azimuth or elevation) + /// Using the instrument bias as the white noise value, zero constant bias. + pub fn default_angle_deg() -> Self { + Self { + white_noise: Some(WhiteNoise { + sigma: 1.0e-2, // 0.01 deg + ..Default::default() + }), + bias: Some(GaussMarkov::default_range_km()), } } diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index 7319f814..dc5033a6 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -358,5 +358,7 @@ fn verif_sensitivity_mat(almanac: Arc) { "{msr_type:?} error = {sensitivity_error:.3e} {}", msr_type.unit() ); + + assert!(sensitivity_error.abs() < 1e-3); } } diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index 222537bd..ae91f9fc 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -43,10 +43,14 @@ fn devices(almanac: Arc) -> BTreeMap { StochasticNoise::default_doppler_km_s(), iau_earth, ) - // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - // .without_msr_type(MeasurementType::Range) - // .without_msr_type(MeasurementType::Doppler) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) .with_integration_time(integration_time); let dss34_canberra = GroundStation::dss34_canberra( @@ -55,10 +59,14 @@ fn devices(almanac: Arc) -> BTreeMap { StochasticNoise::default_doppler_km_s(), iau_earth, ) - // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - // .without_msr_type(MeasurementType::Range) - // .without_msr_type(MeasurementType::Doppler) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) .with_integration_time(integration_time); let dss13_goldstone = GroundStation::dss13_goldstone( @@ -67,10 +75,14 @@ fn devices(almanac: Arc) -> BTreeMap { StochasticNoise::default_doppler_km_s(), iau_earth, ) - // .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN) - // .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN) - // .without_msr_type(MeasurementType::Range) - // .without_msr_type(MeasurementType::Doppler) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) .with_integration_time(integration_time); let mut devices = BTreeMap::new(); @@ -245,16 +257,12 @@ fn od_robust_test_ekf_rng_dop_az_el( let mut odp = ODProcess::< SpacecraftDynamics, - Const<2>, + Const<4>, Const<3>, - KF, Const<2>>, + KF, Const<4>>, GroundStation, >::ekf( - prop_est, - kf, - devices, - trig, - Some(ResidRejectCrit::default()), + prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), almanac, ); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 5565d365..96b6aa9e 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -386,12 +386,22 @@ fn od_tb_val_with_arc( assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); } +#[fixture] +fn cfg() -> TrkConfig { + // Define the tracking configurations + TrkConfig::builder() + .sampling(10.seconds()) + .scheduler(Scheduler::builder().sample_alignment(10.seconds()).build()) + .build() +} + #[allow(clippy::identity_op)] #[rstest] fn od_tb_val_ckf_fixed_step_perfect_stations( almanac: Arc, sim_devices: BTreeMap, proc_devices: BTreeMap, + cfg: TrkConfig, ) { /* * This tests that the state transition matrix computation is correct with two body dynamics. @@ -408,12 +418,6 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( **/ let _ = pretty_env_logger::try_init(); - // Define the tracking configurations - let cfg = TrkConfig::builder() - .sampling(10.seconds()) - .scheduler(Scheduler::builder().sample_alignment(10.seconds()).build()) - .build(); - let mut configs = BTreeMap::new(); for name in sim_devices.keys() { configs.insert(name.clone(), cfg.clone()); @@ -616,6 +620,261 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( assert!(delta.vmag_km_s() < 1e-9, "More than 1 micrometer/s error"); } +#[allow(clippy::identity_op)] +#[rstest] +fn od_tb_val_az_el_ckf_fixed_step_perfect_stations( + almanac: Arc, + mut sim_devices: BTreeMap, + mut proc_devices: BTreeMap, + cfg: TrkConfig, +) { + /* + * This tests that the state transition matrix computation is correct with two body dynamics. + * + * Specifically, the same dynamics are used for both the measurement generation and for the estimation. + * However, only the estimation generation propagates the STM. When STM propagation is enabled, the code will compute + * the dynamics using a hyperdual representation in 7 dimensions: 1 for the reals, 3 for the position partials, + * 3 for the velocity partials. + * + * Hence, if the filter state estimation is any different from the truth data, then it means that the equations of + * motion computed in hyperdual space differ from the ones computes in the reals. + * + * Thereby, this serves as a validation of the orbital dynamics implementation. + **/ + let _ = pretty_env_logger::try_init(); + + for (_, dev) in sim_devices.iter_mut() { + dev.measurement_types.swap_remove(&MeasurementType::Range); + dev.measurement_types.swap_remove(&MeasurementType::Doppler); + dev.measurement_types.insert(MeasurementType::Azimuth); + dev.measurement_types.insert(MeasurementType::Elevation); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Azimuth, StochasticNoise::ZERO); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Elevation, StochasticNoise::ZERO); + } + + for (_, dev) in proc_devices.iter_mut() { + dev.measurement_types.swap_remove(&MeasurementType::Range); + dev.measurement_types.swap_remove(&MeasurementType::Doppler); + dev.measurement_types.insert(MeasurementType::Azimuth); + dev.measurement_types.insert(MeasurementType::Elevation); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Azimuth, StochasticNoise::MIN); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Elevation, StochasticNoise::MIN); + } + + let mut configs = BTreeMap::new(); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); + } + + let all_stations = sim_devices; + + // Define the propagator information. + let prop_time = 1 * Unit::Day; + let step_size = 10.0 * Unit::Second; + let opts = IntegratorOptions::with_fixed_step(step_size); + + // Define state information. + let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let dt = Epoch::from_gregorian_tai_at_midnight(2020, 1, 1); + let initial_state = Orbit::keplerian(22000.0, 0.01, 30.0, 80.0, 40.0, 0.0, dt, eme2k); + + // Generate the truth data on one thread. + let orbital_dyn = SpacecraftDynamics::new(OrbitalDynamics::two_body()); + + let setup = Propagator::new(orbital_dyn, IntegratorMethod::RungeKutta4, opts); + let mut prop = setup.with(initial_state.into(), almanac.clone()); + + let (final_truth, traj) = prop.for_duration_with_traj(prop_time).unwrap(); + + // Simulate tracking data + let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + arc_sim.build_schedule(almanac.clone()).unwrap(); + + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); + + // And serialize to disk + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "output_data", + "od_tb_val_az_el_ckf_fixed_step_perfect_stations.parquet", + ] + .iter() + .collect(); + + arc.to_parquet_simple(path).unwrap(); + + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on + // the measurements, and the same time step. + let initial_state_est = Spacecraft::from(initial_state).with_stm(); + // Use the same setup as earlier + let prop_est = setup.with(initial_state_est, almanac.clone()); + let covar_radius_km = 1.0e-3; + let covar_velocity_km_s = 1.0e-6; + let init_covar = SMatrix::::from_diagonal(&SVector::::from_iterator([ + covar_radius_km, + covar_radius_km, + covar_radius_km, + covar_velocity_km_s, + covar_velocity_km_s, + covar_velocity_km_s, + 0.0, + 0.0, + 0.0, + ])); + + // Define the initial orbit estimate + let initial_estimate = KfEstimate::from_covar(initial_state_est, init_covar); + + let ckf = KF::no_snc(initial_estimate); + + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); + + odp.process_arc(&arc).unwrap(); + + let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "tb_ckf.parquet"] + .iter() + .collect(); + + odp.to_parquet(&arc, path, ExportCfg::default()).unwrap(); + + // Check that there are no duplicates of epochs. + let mut prev_epoch = odp.estimates[0].epoch(); + + for est in odp.estimates.iter().skip(2) { + let this_epoch = est.epoch(); + assert!( + this_epoch > prev_epoch, + "Estimates not continuously going forward: {this_epoch} <= {prev_epoch}" + ); + prev_epoch = this_epoch; + } + + for (no, est) in odp.estimates.iter().enumerate() { + if no == 0 { + // Skip the first estimate which is the initial estimate provided by user + continue; + } + for i in 0..6 { + assert!( + est.covar[(i, i)] >= 0.0, + "covar diagonal element negative @ [{}, {}] = {:e} @ {}", + i, + i, + est.covar[(i, i)], + est.epoch() + ); + } + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + } + + for res in odp.residuals.iter().flatten() { + assert!( + res.prefit.norm() < 1e-12, + "prefit should be zero (perfect dynamics) ({:e})", + res + ); + } + + for res in odp.residuals.iter().flatten() { + assert!( + res.postfit.norm() < 1e-12, + "postfit should be zero (perfect dynamics) ({:e})", + res + ); + } + + let est = odp.estimates.last().unwrap(); + println!("estimate error {:.2e}", est.state_deviation().norm()); + println!("estimate covariance {:.2e}", est.covar.diagonal().norm()); + + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + + assert!( + est.covar.diagonal().norm() < 1e-6, + "estimate covariance norm should be zero (perfect dynamics) ({:e})", + est.covar.diagonal().norm() + ); + + let delta = (est.state().orbit - final_truth.orbit).unwrap(); + println!( + "RMAG error = {:.2e} m\tVMAG error = {:.3e} mm/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e6 + ); + + assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); + assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + + // Iterate + odp.iterate_arc( + &arc, + IterationConf { + smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), + ..Default::default() + }, + ) + .unwrap(); + + println!( + "N-1 one iteration: \n{}", + odp.estimates[odp.estimates.len() - 1] + ); + + println!( + "Initial state after iteration: \n{:x}", + odp.estimates[0].state() + ); + + // Check the final estimate + let est = odp.estimates.last().unwrap(); + println!("estimate error {:.2e}", est.state_deviation().norm()); + println!("estimate covariance {:.2e}", est.covar.diagonal().norm()); + + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + + // Note we accept a larger covariance diagonal here because smoothing will increase the covariance + assert!( + est.covar.diagonal().norm() < 1e-4, + "estimate covariance norm should be zero (perfect dynamics) ({:e})", + est.covar.diagonal().norm() + ); + + let delta = (est.state().orbit - final_truth.orbit).unwrap(); + println!( + "RMAG error = {:.2e} m\tVMAG error = {:.3e} mm/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e6 + ); + + assert!(delta.rmag_km() < 1e-9, "More than 1 micrometer error"); + assert!(delta.vmag_km_s() < 1e-9, "More than 1 micrometer/s error"); +} + #[allow(clippy::identity_op)] #[rstest] fn od_tb_ckf_fixed_step_iteration_test( From 0937fe7b2dce9fd0cf88a791e0545fe9ac22a065 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 1 Dec 2024 23:43:45 -0700 Subject: [PATCH 34/38] Cleanup az/el msr types --- src/od/msr/sensitivity.rs | 58 +++------------ src/od/process/trigger.rs | 1 + tests/orbit_determination/robust_az_el.rs | 91 ++++++++++++----------- 3 files changed, 59 insertions(+), 91 deletions(-) diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 71a2b7ac..d42688da 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -23,7 +23,7 @@ use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice}; use crate::{Spacecraft, State}; use anise::prelude::Almanac; use indexmap::IndexSet; -use nalgebra::{Const, DimName, OMatrix, U1}; +use nalgebra::{DimName, OMatrix, U1}; use snafu::ResultExt; use std::marker::PhantomData; use std::sync::Arc; @@ -140,32 +140,6 @@ impl ScalarSensitivityT let delta_r = receiver.radius_km - transmitter.radius_km; let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; - // SEZ DCM is topo to fixed - // let sez_dcm = transmitter - // .dcm_from_topocentric_to_body_fixed(100000) - // .unwrap(); - // .context(EphemerisPhysicsSnafu { action: "" }) - // .context(EphemerisSnafu { - // action: "computing SEZ DCM for AER", - // })?; - - // let tx_sez = (sez_dcm.transpose() * transmitter).unwrap(); - // let rx_sez = (sez_dcm.transpose() * rx.orbit).unwrap(); - - /* - Range error = 1.265e-5 km - Doppler error = 2.800e-8 km/s - Elevation error = 7.491e-4 deg - Azimuth error = -5.718e-4 deg */ - - // let delta_r = rx_sez.radius_km - tx_sez.radius_km; - // let delta_v = rx_sez.velocity_km_s - tx_sez.velocity_km_s; - - // .context(EphemerisPhysicsSnafu { action: "" }) - // .context(EphemerisSnafu { - // action: "transforming transmitter to SEZ", - // })?; - match msr_type { MeasurementType::Doppler => { // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range. @@ -217,9 +191,6 @@ impl ScalarSensitivityT }) } MeasurementType::Azimuth => { - // let denom = tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2); - // let m11 = -tx_sez.radius_km.y / denom; - // let m12 = tx_sez.radius_km.x / denom; let denom = delta_r.x.powi(2) + delta_r.y.powi(2); let m11 = -delta_r.y / denom; let m12 = delta_r.x / denom; @@ -227,14 +198,10 @@ impl ScalarSensitivityT // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. - let effective_sensitivity_row = - OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]); - // * sez_dcm.state_dcm(); - - let mut sensitivity_row = OMatrix::::Size>::zeros(); - for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { - sensitivity_row[i] = val; - } + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); Ok(Self { sensitivity_row, @@ -243,25 +210,18 @@ impl ScalarSensitivityT }) } MeasurementType::Elevation => { - // let r2 = tx_sez.radius_km.norm().powi(2); - // let z2 = tx_sez.radius_km.z.powi(2); let r2 = delta_r.norm().powi(2); let z2 = delta_r.z.powi(2); // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. - let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - z2).sqrt()); let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - z2).sqrt()); let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; - let effective_sensitivity_row = - OMatrix::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]); - // * sez_dcm.state_dcm(); - - let mut sensitivity_row = OMatrix::::Size>::zeros(); - for (i, val) in effective_sensitivity_row.iter().copied().enumerate() { - sensitivity_row[i] = val; - } + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); Ok(Self { sensitivity_row, diff --git a/src/od/process/trigger.rs b/src/od/process/trigger.rs index e559db5a..f7c21ab8 100644 --- a/src/od/process/trigger.rs +++ b/src/od/process/trigger.rs @@ -24,6 +24,7 @@ use crate::linalg::DefaultAllocator; use crate::time::Duration; use crate::State; +#[derive(Copy, Clone, Debug)] /// An EkfTrigger on the number of measurements processed and a time between measurements. pub struct EkfTrigger { pub num_msrs: usize, diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index ae91f9fc..67c9361b 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -251,59 +251,36 @@ fn od_robust_test_ekf_rng_dop_az_el( let sigma_q = 5e-10_f64.powi(2); let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); - let kf = KF::new(initial_estimate, process_noise); - let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = ODProcess::< + // Run with all data simultaneously + let mut odp_simul = ODProcess::< SpacecraftDynamics, Const<4>, Const<3>, KF, Const<4>>, GroundStation, >::ekf( - prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), - almanac, + prop_est, + KF::new(initial_estimate, process_noise.clone()), + devices.clone(), + trig, + None, + almanac.clone(), ); - // let mut odp = SpacecraftODProcess::ekf( - // prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()), - // almanac, - // ); - - // Let's filter and iterate on the initial subset of the arc to refine the initial estimate - // let subset = arc.clone().filter_by_offset(..3.hours()); - // let remaining = arc.filter_by_offset(3.hours()..); - - odp.process_arc(&arc).unwrap(); - odp.iterate_arc(&arc, IterationConf::once()).unwrap(); + odp_simul.process_arc(&arc).unwrap(); - // Grab the comparison state, which differs from the initial state because of the integration time of the measurements. - let cmp_state = traj - .at(odp.estimates[0].state().orbit.epoch) - .expect("could not find comparison epoch in trajectory"); - - let (sm_rss_pos_km, sm_rss_vel_km_s) = - rss_orbit_errors(&cmp_state.orbit, &odp.estimates[0].state().orbit); - - println!( - "Initial state error after smoothing:\t{:.3} m\t{:.3} m/s\n{}", - sm_rss_pos_km * 1e3, - sm_rss_vel_km_s * 1e3, - (cmp_state.orbit - odp.estimates[0].state().orbit).unwrap() - ); - - // odp.process_arc(&remaining).unwrap(); - - odp.to_parquet( - &arc, - path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), - ExportCfg::default(), - ) - .unwrap(); + odp_simul + .to_parquet( + &arc, + path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), + ExportCfg::default(), + ) + .unwrap(); // Check that the covariance deflated - let est = &odp.estimates[odp.estimates.len() - 1]; + let est = &odp_simul.estimates[odp_simul.estimates.len() - 1]; let final_truth_state = traj.at(est.epoch()).unwrap(); println!("Estimate:\n{}", est); @@ -338,11 +315,41 @@ fn od_robust_test_ekf_rng_dop_az_el( ); assert!( - delta.rmag_km() < 0.06, - "Position error should be less than 50 meters" + delta.rmag_km() < 0.01, + "Position error should be less than 10 meters" ); assert!( delta.vmag_km_s() < 2e-4, "Velocity error should be on centimeter level" ); + + // We get the best results with all data simultaneously, let's rerun with then two-by-two. + let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); + let mut odp_2by2 = SpacecraftODProcess::ekf( + prop_est, + KF::new(initial_estimate, process_noise.clone()), + devices, + trig, + None, + almanac, + ); + + odp_2by2.process_arc(&arc).unwrap(); + let est_2by2 = &odp_2by2.estimates[odp_2by2.estimates.len() - 1]; + + let delta_2by2 = (est.state().orbit - est_2by2.state().orbit).unwrap(); + println!( + "RMAG diff = {:.6} m\tVMAG diff = {:.6} m/s", + delta_2by2.rmag_km() * 1e3, + delta_2by2.vmag_km_s() * 1e3 + ); + + assert!( + delta_2by2.rmag_km() < 0.1, + "Position error should be less than 100 meters" + ); + assert!( + delta_2by2.vmag_km_s() < 1e-1, + "Velocity error should be on decimeter level" + ); } From 23d243ebb6f442dae8176fc0a287395be66a0cb5 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sat, 7 Dec 2024 23:11:14 -0700 Subject: [PATCH 35/38] Compute the Cholesky decomposition of the R_k matrix for the correct tracker noises. Add sequential OD struct. Add error logs for incorrect filter config --- src/od/filter/kalman.rs | 49 ++-- src/od/mod.rs | 95 +------- src/od/process/mod.rs | 12 + src/od/process/rejectcrit.rs | 6 +- tests/orbit_determination/robust_az_el.rs | 258 ++++++++++++++++------ 5 files changed, 247 insertions(+), 173 deletions(-) diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 575e2620..96139470 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -276,7 +276,7 @@ where nominal_state: T, real_obs: &OVector, computed_obs: &OVector, - measurement_covar: OMatrix, + r_k: OMatrix, resid_rejection: Option, ) -> Result<(Self::Estimate, Residual), ODError> { if !self.h_tilde_updated { @@ -291,8 +291,8 @@ where let h_tilde_t = &self.h_tilde.transpose(); let h_p_ht = &self.h_tilde * covar_bar * h_tilde_t; - // Account for state uncertainty in the measurement noise. Equation 4.10 of ODTK MathSpec. - let r_k = &h_p_ht + measurement_covar; + + let s_k = &h_p_ht + &r_k; // Compute observation deviation (usually marked as y_i) let prefit = real_obs - computed_obs; @@ -300,33 +300,36 @@ 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. - let r_k_chol_inv = r_k - .clone() - .cholesky() - .ok_or(ODError::SingularNoiseRk)? - .l() - .try_inverse() - .ok_or(ODError::SingularNoiseRk)?; - - // Then we multiply the inverted r_k matrix with prefit, giving us the vector of - // ratios of the prefit over the r_k matrix diagonal. - let ratio_vec = r_k_chol_inv * &prefit; - // Compute the ratio as the dot product. - let ratio = ratio_vec.dot(&ratio_vec); + let r_k_chol = s_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. + let ratio = s_k + .diagonal() + .iter() + .copied() + .enumerate() + .map(|(idx, r)| prefit[idx] / r.sqrt()) + .sum::() + / (M::USIZE as f64); if let Some(resid_reject) = resid_rejection { - if ratio > resid_reject.num_sigmas { + if ratio.abs() > resid_reject.num_sigmas { // Reject this whole measurement and perform only a time update let pred_est = self.time_update(nominal_state)?; return Ok(( pred_est, - Residual::rejected(epoch, prefit, ratio, r_k.diagonal()), + Residual::rejected(epoch, prefit, ratio, r_k_chol.diagonal()), )); } } - // Compute the Kalman gain but first adding the measurement noise to H⋅P⋅H^T - let mut innovation_covar = h_p_ht + &r_k; + // Compute the innovation matrix (S_k) but using the previously computed s_k. + // This differs from the typical Kalman definition, but it allows constant inflating of the covariance. + // In turn, this allows the filter to not be overly optimistic. In verification tests, using the nominal + // Kalman formulation shows an error roughly 7 times larger with a smaller than expected covariance, despite + // no difference in the truth and sim. + let mut innovation_covar = h_p_ht + &s_k; if !innovation_covar.try_inverse_mut() { return Err(ODError::SingularKalmanGain); } @@ -339,7 +342,7 @@ where let postfit = &prefit - (&self.h_tilde * state_hat); ( state_hat, - Residual::accepted(epoch, prefit, postfit, ratio, r_k.diagonal()), + Residual::accepted(epoch, prefit, postfit, ratio, r_k_chol.diagonal()), ) } else { // Must do a time update first @@ -347,7 +350,7 @@ where let postfit = &prefit - (&self.h_tilde * state_bar); ( state_bar + &gain * &postfit, - Residual::accepted(epoch, prefit, postfit, ratio, r_k.diagonal()), + Residual::accepted(epoch, prefit, postfit, ratio, r_k_chol.diagonal()), ) }; @@ -355,7 +358,7 @@ where let first_term = OMatrix::::Size, ::Size>::identity() - &gain * &self.h_tilde; let covar = - first_term * covar_bar * first_term.transpose() + &gain * &r_k * &gain.transpose(); + first_term * covar_bar * first_term.transpose() + &gain * &s_k * &gain.transpose(); // And wrap up let estimate = KfEstimate { diff --git a/src/od/mod.rs b/src/od/mod.rs index a28ef042..ee1b2a9f 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -67,6 +67,16 @@ pub type SpacecraftODProcess<'a> = self::process::ODProcess< GroundStation, >; +/// A helper type for spacecraft orbit determination sequentially processing measurements +pub type SpacecraftODProcessSeq<'a> = self::process::ODProcess< + 'a, + crate::md::prelude::SpacecraftDynamics, + nalgebra::Const<1>, + nalgebra::Const<3>, + filter::kalman::KF, nalgebra::Const<1>>, + GroundStation, +>; + #[allow(unused_imports)] pub mod prelude { pub use super::estimate::*; @@ -83,89 +93,6 @@ pub mod prelude { pub use crate::time::{Duration, Epoch, TimeUnits, Unit}; } -// /// A trait defining a measurement that can be used in the orbit determination process. -// pub trait Measurement: Copy + TimeTagged { -// /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2). -// type MeasurementSize: DimName; - -// /// Returns the fields for this kind of measurement. -// /// The metadata must include a `unit` field with the unit. -// fn fields() -> Vec; - -// /// Initializes a new measurement from the provided data. -// fn from_observation(epoch: Epoch, obs: OVector) -> Self -// where -// DefaultAllocator: Allocator; - -// /// Returns the measurement/observation as a vector. -// fn observation(&self) -> OVector -// where -// DefaultAllocator: Allocator; -// } - -// /// The Estimate trait defines the interface that is the opposite of a `SolveFor`. -// /// For example, `impl EstimateFrom for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`. -// /// -// /// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and -// /// the ground station bias information. Then, the `impl EstimateFrom for OrbitAndBias` will be added, where `OrbitAndBias` is the -// /// new State that includes the orbit and the bias of one ground station. -// pub trait EstimateFrom -// where -// Self: State, -// DefaultAllocator: Allocator<::Size> -// + Allocator<::VecLength> -// + Allocator<::Size, ::Size> -// + Allocator -// + Allocator -// + Allocator, -// { -// /// From the state extract the state to be estimated -// fn extract(from: O) -> Self; - -// /// Returns the measurement sensitivity (often referred to as H tilde). -// /// -// /// # Limitations -// /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero. -// /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use. -// /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator. -// fn sensitivity( -// msr: &M, -// receiver: Self, -// transmitter: Orbit, -// ) -> OMatrix -// where -// DefaultAllocator: Allocator; -// } - -// /// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state. -// /// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame. -// impl EstimateFrom for O -// where -// O: State + Measurement, -// Self: State, -// DefaultAllocator: Allocator<::Size> -// + Allocator<::VecLength> -// + Allocator<::Size, ::Size> -// + Allocator -// + Allocator -// + Allocator, -// { -// fn extract(from: O) -> Self { -// from -// } - -// fn sensitivity( -// _full_state_msr: &O, -// _receiver: Self, -// _transmitter: Orbit, -// ) -> OMatrix::MeasurementSize, Self::Size> -// where -// DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, -// { -// OMatrix::::identity() -// } -// } - #[derive(Debug, PartialEq, Snafu)] #[snafu(visibility(pub(crate)))] pub enum ODError { @@ -187,7 +114,7 @@ pub enum ODError { SensitivityNotUpdated, #[snafu(display("Kalman gain is singular"))] SingularKalmanGain, - #[snafu(display("Noise matrix is singular"))] + #[snafu(display("noise matrix is singular"))] SingularNoiseRk, #[snafu(display("{kind} noise not configured"))] NoiseNotConfigured { kind: String }, diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 48feef2a..ccc8732e 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -424,6 +424,18 @@ where StepSizeSnafu { step: max_step } ); + // Check proper configuration. + if MsrSize::USIZE > arc.unique_types().len() { + error!("Filter misconfigured: expect high rejection count!"); + error!( + "Arc only contains {} measurement types, but filter configured for {}.", + arc.unique_types().len(), + MsrSize::USIZE + ); + error!("Filter should be configured for these numbers to match."); + error!("Consider running subsequent arcs if ground stations provide different measurements.") + } + // Start by propagating the estimator. let num_msrs = measurements.len(); diff --git a/src/od/process/rejectcrit.rs b/src/od/process/rejectcrit.rs index 9488de56..acd4cf58 100644 --- a/src/od/process/rejectcrit.rs +++ b/src/od/process/rejectcrit.rs @@ -113,10 +113,10 @@ impl ResidRejectCrit { } impl Default for ResidRejectCrit { - /// By default, a measurement is rejected if its prefit residual is greater the 4-sigma value of the measurement noise at that time step. - /// This corresponds to [1 chance in in 15,787](https://en.wikipedia.org/wiki/68%E2%80%9395%E2%80%9399.7_rule). + /// By default, a measurement is rejected if its prefit residual is greater the 3-sigma value of the measurement noise at that time step. + /// This corresponds to [1 chance in in 370](https://en.wikipedia.org/wiki/68%E2%80%9395%E2%80%9399.7_rule). fn default() -> Self { - Self { num_sigmas: 4.0 } + Self { num_sigmas: 3.0 } } } diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index 67c9361b..42e7be28 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -3,6 +3,7 @@ extern crate pretty_env_logger; use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; +use indexmap::IndexSet; use nalgebra::Const; use nyx::cosmic::Orbit; use nyx::dynamics::orbital::OrbitalDynamics; @@ -35,7 +36,7 @@ fn almanac() -> Arc { fn devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 10.0; - let integration_time = None; //Some(60 * Unit::Second); + let integration_time = Some(60 * Unit::Second); let dss65_madrid = GroundStation::dss65_madrid( elevation_mask, @@ -69,26 +70,9 @@ fn devices(almanac: Arc) -> BTreeMap { ) .with_integration_time(integration_time); - let dss13_goldstone = GroundStation::dss13_goldstone( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ) - .with_msr_type( - MeasurementType::Azimuth, - StochasticNoise::default_angle_deg(), - ) - .with_msr_type( - MeasurementType::Elevation, - StochasticNoise::default_angle_deg(), - ) - .with_integration_time(integration_time); - let mut devices = BTreeMap::new(); devices.insert(dss65_madrid.name(), dss65_madrid); devices.insert(dss34_canberra.name(), dss34_canberra); - devices.insert(dss13_goldstone.name(), dss13_goldstone); devices } @@ -152,14 +136,107 @@ fn estimator_setup() -> Propagator { Propagator::new(estimator, IntegratorMethod::DormandPrince78, opts) } +#[rstest] +fn od_robust_all_msr_types( + almanac: Arc, + devices: BTreeMap, + initial_state: Spacecraft, + initial_estimate: KfEstimate, + truth_setup: Propagator, + estimator_setup: Propagator, +) { + od_robust_test_ekf_rng_dop_az_el( + almanac, + devices, + initial_state, + initial_estimate, + truth_setup, + estimator_setup, + ) +} + +#[rstest] +fn od_robust_rng_dpl_only( + almanac: Arc, + mut devices: BTreeMap, + initial_state: Spacecraft, + initial_estimate: KfEstimate, + truth_setup: Propagator, + estimator_setup: Propagator, +) { + // Drop azimuth and elevation. + for (name, gs) in devices.clone() { + devices.insert( + name, + gs.without_msr_type(MeasurementType::Azimuth) + .without_msr_type(MeasurementType::Elevation), + ); + } + + // Increase the noises to avoid rejections + for gs in devices.values_mut() { + let noises = gs.stochastic_noises.as_mut().unwrap(); + for noise in noises.values_mut() { + noise.white_noise.as_mut().unwrap().mean *= 3.0; + } + } + + od_robust_test_ekf_rng_dop_az_el( + almanac, + devices, + initial_state, + initial_estimate, + truth_setup, + estimator_setup, + ) +} + +#[rstest] +fn od_robust_az_rng_then_el_dpl( + almanac: Arc, + mut devices: BTreeMap, + initial_state: Spacecraft, + initial_estimate: KfEstimate, + truth_setup: Propagator, + estimator_setup: Propagator, +) { + let mut processing_order = IndexSet::new(); + + processing_order.insert(MeasurementType::Azimuth); + processing_order.insert(MeasurementType::Range); + processing_order.insert(MeasurementType::Elevation); + processing_order.insert(MeasurementType::Doppler); + + // Drop azimuth and elevation. + for (name, mut gs) in devices.clone() { + gs.measurement_types = processing_order.clone(); + devices.insert(name, gs); + } + + // Increase the noises to avoid rejections + for gs in devices.values_mut() { + let noises = gs.stochastic_noises.as_mut().unwrap(); + for noise in noises.values_mut() { + noise.white_noise.as_mut().unwrap().mean *= 3.0; + } + } + + od_robust_test_ekf_rng_dop_az_el( + almanac, + devices, + initial_state, + initial_estimate, + truth_setup, + estimator_setup, + ) +} + /* * These tests check that if we start with a state deviation in the estimate, the filter will eventually converge back. - * These tests do NOT check that the filter will converge if the initial state in the propagator has that state deviation. - * The latter would require iteration and smoothing before playing with an EKF. This will be handled in a subsequent version. **/ +/// Generic test function used by all of the tests above. #[allow(clippy::identity_op)] -#[rstest] fn od_robust_test_ekf_rng_dop_az_el( almanac: Arc, devices: BTreeMap, @@ -170,37 +247,16 @@ fn od_robust_test_ekf_rng_dop_az_el( ) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); // Define the ground stations. let ekf_num_meas = 10; // Set the disable time to be very low to test enable/disable sequence - let ekf_disable_time = 3 * Unit::Minute; - let elevation_mask = 0.0; - - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); + let ekf_disable_time = 30 * Unit::Minute; // Define the tracking configurations - let configs = BTreeMap::from([ - ( - dss65_madrid.name.clone(), - TrkConfig::from_sample_rate(60.seconds()), - ), - ( - dss34_canberra.name.clone(), - TrkConfig::from_sample_rate(60.seconds()), - ), - ]); + let mut configs = BTreeMap::new(); + for name in devices.keys() { + configs.insert(name.clone(), TrkConfig::from_sample_rate(60.seconds())); + } // Define the propagator information. let prop_time = 1.1 * initial_state.orbit.period().unwrap(); @@ -265,7 +321,7 @@ fn od_robust_test_ekf_rng_dop_az_el( KF::new(initial_estimate, process_noise.clone()), devices.clone(), trig, - None, + Some(ResidRejectCrit::default()), almanac.clone(), ); @@ -309,23 +365,78 @@ fn od_robust_test_ekf_rng_dop_az_el( ); let delta = (est.state().orbit - final_truth_state.orbit).unwrap(); println!( - "RMAG error = {:.6} m\tVMAG error = {:.6} m/s", + "[4x4] RMAG error = {:.6} m\tVMAG error = {:.6} m/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e3 + ); + + // Compute the number of measurements. + if arc.unique_types().len() < 4 { + println!( + "Skipping 4x4 assertions because only {} msr types", + arc.unique_types().len() + ); + } else { + assert!( + delta.rmag_km() < 0.01, + "Position error should be less than 10 meters" + ); + assert!( + delta.vmag_km_s() < 2e-4, + "Velocity error should be on centimeter level" + ); + } + + // We get the best results with all data simultaneously, let's rerun with then two-by-two. + let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); + let mut odp_2by2 = SpacecraftODProcess::ekf( + prop_est, + KF::new(initial_estimate, process_noise.clone()), + devices.clone(), + trig, + None, + almanac.clone(), + ); + + odp_2by2.process_arc(&arc).unwrap(); + let est_2by2 = &odp_2by2.estimates[odp_2by2.estimates.len() - 1]; + + let delta = (est_2by2.state().orbit - final_truth_state.orbit).unwrap(); + println!( + "[2x2] RMAG error = {:.6} m\tVMAG error = {:.6} m/s", delta.rmag_km() * 1e3, delta.vmag_km_s() * 1e3 ); assert!( - delta.rmag_km() < 0.01, - "Position error should be less than 10 meters" + delta.rmag_km() < 0.05, + "Position error should be less than 50 meters" ); assert!( - delta.vmag_km_s() < 2e-4, - "Velocity error should be on centimeter level" + delta.vmag_km_s() < 5e-4, + "Velocity error should be on half decimeter level" ); - // We get the best results with all data simultaneously, let's rerun with then two-by-two. + if arc.unique_types().len() == 4 { + let delta_2by2 = (est.state().orbit - est_2by2.state().orbit).unwrap(); + println!( + "RMAG diff = {:.6} m\tVMAG diff = {:.6} m/s", + delta_2by2.rmag_km() * 1e3, + delta_2by2.vmag_km_s() * 1e3 + ); + + assert!( + delta_2by2.rmag_km() < 0.1, + "Position error should be less than 100 meters" + ); + assert!( + delta_2by2.vmag_km_s() < 1e-1, + "Velocity error should be on decimeter level" + ); + } + // Rerun processing measurements one by one like in ODTK let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); - let mut odp_2by2 = SpacecraftODProcess::ekf( + let mut odp_1by1 = SpacecraftODProcessSeq::ekf( prop_est, KF::new(initial_estimate, process_noise.clone()), devices, @@ -334,22 +445,43 @@ fn od_robust_test_ekf_rng_dop_az_el( almanac, ); - odp_2by2.process_arc(&arc).unwrap(); - let est_2by2 = &odp_2by2.estimates[odp_2by2.estimates.len() - 1]; + odp_1by1.process_arc(&arc).unwrap(); + let est_1by1 = &odp_1by1.estimates[odp_1by1.estimates.len() - 1]; + + let delta = (est_1by1.state().orbit - final_truth_state.orbit).unwrap(); + println!( + "[1x1] RMAG error = {:.6} m\tVMAG error = {:.6} m/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e3 + ); + + assert!( + delta.rmag_km() < 0.15, + "Position error should be less than 150 meters" + ); + assert!( + delta.vmag_km_s() < 1e-3, + "Velocity error should be on decimeter level" + ); + + let delta_1by1 = if arc.unique_types().len() == 4 { + (est.state().orbit - est_1by1.state().orbit).unwrap() + } else { + (est_2by2.state().orbit - est_1by1.state().orbit).unwrap() + }; - let delta_2by2 = (est.state().orbit - est_2by2.state().orbit).unwrap(); println!( "RMAG diff = {:.6} m\tVMAG diff = {:.6} m/s", - delta_2by2.rmag_km() * 1e3, - delta_2by2.vmag_km_s() * 1e3 + delta_1by1.rmag_km() * 1e3, + delta_1by1.vmag_km_s() * 1e3 ); assert!( - delta_2by2.rmag_km() < 0.1, - "Position error should be less than 100 meters" + delta_1by1.rmag_km() < 0.2, + "Position error should be less than 200 meters" ); assert!( - delta_2by2.vmag_km_s() < 1e-1, + delta_1by1.vmag_km_s() < 1e-1, "Velocity error should be on decimeter level" ); } From 37fcbb2a79b413337c081d837b06f0c61a879ffe Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sat, 7 Dec 2024 23:35:48 -0700 Subject: [PATCH 36/38] Add QQ plot --- examples/04_lro_od/plot_od_rslt.py | 98 ++++++++++++++++++----- tests/orbit_determination/resid_reject.rs | 4 +- 2 files changed, 81 insertions(+), 21 deletions(-) diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 2d854014..eeafa63d 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -1,4 +1,5 @@ import polars as pl +from scipy import stats from scipy.stats import chi2 import numpy as np import plotly.graph_objects as go @@ -7,8 +8,10 @@ if __name__ == "__main__": df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp.parquet") - df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( - "Epoch (UTC)", descending=False + df = ( + df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")) + .sort("Epoch (UTC)", descending=False) + .with_columns(df["Residual ratio"].fill_null(0.0).alias("Residual ratio")) ) all_msr_types = ["Range (km)", "Doppler (km/s)", "Azimuth (deg)", "Elevation (deg)"] @@ -32,24 +35,81 @@ ] ) - # == Residual plots == - # Nyx uses the Mahalanobis distance for the residual ratios, so we test the goodness using the Chi Square distribution. - freedoms = msr_type_count # Two degrees of freedoms for the range and the range rate. - x_chi = np.linspace(chi2.ppf(0.01, freedoms), chi2.ppf(0.99, freedoms), 100) - y_chi = chi2.pdf(x_chi, freedoms) - - # Compute the scaling factor - hist = np.histogram(df["Residual ratio"].fill_null(0.0), bins=50)[0] - max_hist = max(hist[1:]) # Ignore the bin of zeros - max_chi2_pdf = max(y_chi) - scale_factor = max_hist / max_chi2_pdf - - fig = go.Figure() - fig.add_trace( - go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") + # Convert the Polars column to a NumPy array for compatibility with scipy and Plotly + residual_ratio = df["Residual ratio"].to_numpy() + + # Create QQ plot + qq = stats.probplot(residual_ratio) + x_qq = np.array([qq[0][0][0], qq[0][0][-1]]) + y_qq = np.array([qq[0][1][0], qq[0][1][-1]]) + + # Create the QQ plot figure + fig_qq = go.Figure() + + # Add scatter points + fig_qq.add_trace( + go.Scatter( + x=qq[0][0], + y=qq[0][1], + mode='markers', + name='Sample Data', + marker=dict(color='blue') + ) ) - fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) - fig.show() + + # Add the theoretical line + fig_qq.add_trace( + go.Scatter( + x=x_qq, + y=y_qq, + mode='lines', + name='Theoretical Normal', + line=dict(color='red') + ) + ) + + # Update layout + fig_qq.update_layout( + title='Normal Q-Q Plot', + xaxis_title='Theoretical Quantiles', + yaxis_title='Sample Quantiles', + ) + + # Show QQ plot + fig_qq.show() + + # Create histogram with normal distribution overlay + hist_fig = px.histogram( + df, + x="Residual ratio", + color="Tracker", + marginal="rug", + hover_data=df.columns, + ) + + # Calculate normal distribution parameters + mean = residual_ratio.mean() + std = residual_ratio.std() + x_range = np.linspace(residual_ratio.min(), residual_ratio.max(), 100) + y_normal = stats.norm.pdf(x_range, mean, std) + + # Scale the normal distribution to match histogram height + max_hist_height = 100 + scaling_factor = max_hist_height / max(y_normal) + y_normal_scaled = y_normal * scaling_factor + + # Add normal distribution curve + hist_fig.add_trace( + go.Scatter( + x=x_range, + y=y_normal_scaled, + name='Normal Distribution', + line=dict(color='red', width=2), + ) + ) + + # Show histogram with normal overlay + hist_fig.show() px.histogram( df, diff --git a/tests/orbit_determination/resid_reject.rs b/tests/orbit_determination/resid_reject.rs index ed7ed806..1cf9937c 100644 --- a/tests/orbit_determination/resid_reject.rs +++ b/tests/orbit_determination/resid_reject.rs @@ -216,7 +216,7 @@ fn od_resid_reject_inflated_snc_ckf_two_way( prop_est, kf, devices, - Some(ResidRejectCrit { num_sigmas: 3.0 }), + Some(ResidRejectCrit { num_sigmas: 2.0 }), // 95% to force rejections almanac, ); @@ -240,7 +240,7 @@ fn od_resid_reject_inflated_snc_ckf_two_way( .filter(|residual| residual.rejected) .count(); - assert!(dbg!(num_rejections) < 20, "oddly high rejections"); + assert!(dbg!(num_rejections) < 30, "oddly high rejections"); // Check that the final post-fit residual isn't too bad, and definitely much better than the prefit. let est = &odp.estimates.last().unwrap(); From 9c22b15dce05b167895d31d281999679f1e56e88 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 8 Dec 2024 00:40:29 -0700 Subject: [PATCH 37/38] Reenable obstruction in tracking data --- examples/04_lro_od/dsn-network.yaml | 6 +-- examples/04_lro_od/main.rs | 2 +- examples/04_lro_od/plot_od_rslt.py | 66 +++++++---------------------- examples/04_lro_od/requirements.txt | 38 +++++++++++++++++ src/od/ground_station/trk_device.rs | 10 ++++- 5 files changed, 66 insertions(+), 56 deletions(-) create mode 100644 examples/04_lro_od/requirements.txt diff --git a/examples/04_lro_od/dsn-network.yaml b/examples/04_lro_od/dsn-network.yaml index 5f6bc446..4484ed62 100644 --- a/examples/04_lro_od/dsn-network.yaml +++ b/examples/04_lro_od/dsn-network.yaml @@ -15,7 +15,7 @@ DSS-65 Madrid: white_noise: mean: 0.0 sigma: 50.0e-6 # 5 cm/s - light_time_correction: false + light_time_correction: true latitude_deg: 40.427222 longitude_deg: 4.250556 height_km: 0.834939 @@ -43,7 +43,7 @@ DSS-34 Canberra: white_noise: mean: 0.0 sigma: 50.0e-6 # 5 cm/s - light_time_correction: false + light_time_correction: true measurement_types: - range_km - doppler_km_s @@ -68,7 +68,7 @@ DSS-13 Goldstone: white_noise: mean: 0.0 sigma: 50.0e-6 # 5 cm/s - light_time_correction: false + light_time_correction: true measurement_types: - range_km - doppler_km_s diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs index c948a0e3..6a92101b 100644 --- a/examples/04_lro_od/main.rs +++ b/examples/04_lro_od/main.rs @@ -253,7 +253,7 @@ fn main() -> Result<(), Box> { // Increase the initial covariance to account for larger deviation. initial_estimate, // Until https://github.com/nyx-space/nyx/issues/351, we need to specify the SNC in the acceleration of the Moon J2000 frame. - SNC3::from_diagonal(10 * Unit::Minute, &[1e-11, 1e-11, 1e-11]), + SNC3::from_diagonal(10 * Unit::Minute, &[1e-12, 1e-12, 1e-12]), ); // We'll set up the OD process to reject measurements whose residuals are mover than 4 sigmas away from what we expect. diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index eeafa63d..176d53f0 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -4,9 +4,13 @@ import numpy as np import plotly.graph_objects as go import plotly.express as px +import click -if __name__ == "__main__": - df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp.parquet") + +@click.command +@click.option("-p", "--path", type=str, default="./04_lro_od_results.parquet") +def main(path: str): + df = pl.read_parquet(path) df = ( df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")) @@ -49,68 +53,25 @@ # Add scatter points fig_qq.add_trace( go.Scatter( - x=qq[0][0], - y=qq[0][1], - mode='markers', - name='Sample Data', - marker=dict(color='blue') + x=qq[0][0], y=qq[0][1], mode="markers", name="Residuals ratios (QQ)", marker=dict(color="blue") ) ) # Add the theoretical line fig_qq.add_trace( - go.Scatter( - x=x_qq, - y=y_qq, - mode='lines', - name='Theoretical Normal', - line=dict(color='red') - ) + go.Scatter(x=x_qq, y=y_qq, mode="lines", name="Theoretical Normal", line=dict(color="red")) ) # Update layout fig_qq.update_layout( - title='Normal Q-Q Plot', - xaxis_title='Theoretical Quantiles', - yaxis_title='Sample Quantiles', + title="Normal Q-Q Plot", + xaxis_title="Theoretical Quantiles", + yaxis_title="Sample Quantiles", ) # Show QQ plot fig_qq.show() - # Create histogram with normal distribution overlay - hist_fig = px.histogram( - df, - x="Residual ratio", - color="Tracker", - marginal="rug", - hover_data=df.columns, - ) - - # Calculate normal distribution parameters - mean = residual_ratio.mean() - std = residual_ratio.std() - x_range = np.linspace(residual_ratio.min(), residual_ratio.max(), 100) - y_normal = stats.norm.pdf(x_range, mean, std) - - # Scale the normal distribution to match histogram height - max_hist_height = 100 - scaling_factor = max_hist_height / max(y_normal) - y_normal_scaled = y_normal * scaling_factor - - # Add normal distribution curve - hist_fig.add_trace( - go.Scatter( - x=x_range, - y=y_normal_scaled, - name='Normal Distribution', - line=dict(color='red', width=2), - ) - ) - - # Show histogram with normal overlay - hist_fig.show() - px.histogram( df, x="Residual ratio", @@ -163,7 +124,6 @@ y=["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"], ).show() - raise AssertionError("stop") # Load the RIC diff. for fname, errname in [ ("04_lro_od_truth_error", "OD vs Flown"), @@ -219,3 +179,7 @@ ], title=f"Velocity error with {errname} ({fname})", ).show() + + +if __name__ == "__main__": + main() diff --git a/examples/04_lro_od/requirements.txt b/examples/04_lro_od/requirements.txt new file mode 100644 index 00000000..c7556494 --- /dev/null +++ b/examples/04_lro_od/requirements.txt @@ -0,0 +1,38 @@ +asttokens==2.4.1 +attrs==23.2.0 +click==8.1.7 +decorator==5.1.1 +executing==2.0.1 +iniconfig==2.0.0 +ipython==8.25.0 +jedi==0.19.1 +matplotlib-inline==0.1.7 +maturin==1.6.0 +numpy==1.26.4 +packaging==24.1 +pandas==2.1.4 +parso==0.8.4 +pexpect==4.9.0 +pip==24.0 +plotly==5.16.1 +pluggy==1.5.0 +polars==0.20.31 +prompt-toolkit==3.0.47 +ptyprocess==0.7.0 +pure-eval==0.2.2 +pyarrow==13.0.0 +pygments==2.18.0 +pytest==7.2.2 +python-dateutil==2.9.0.post0 +python-slugify==8.0.4 +pytz==2024.1 +ruff==0.5.0 +scipy==1.11.4 +six==1.16.0 +stack-data==0.6.3 +tenacity==8.3.0 +text-unidecode==1.3 +traitlets==5.14.3 +typing-extensions==4.12.2 +tzdata==2024.1 +wcwidth==0.2.13 diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs index b2b31e05..9d973614 100644 --- a/src/od/ground_station/trk_device.rs +++ b/src/od/ground_station/trk_device.rs @@ -84,6 +84,14 @@ impl TrackingDevice for GroundStation { self.name, self.elevation_mask_deg, aer_t0.elevation_deg, aer_t1.elevation_deg ); return Ok(None); + } else if aer_t0.is_obstructed() || aer_t1.is_obstructed() { + debug!( + "{} obstruction at t0={}, t1={} -- no measurement", + self.name, + aer_t0.is_obstructed(), + aer_t1.is_obstructed() + ); + return Ok(None); } // Noises are computed at the midpoint of the integration time. @@ -128,7 +136,7 @@ impl TrackingDevice for GroundStation { action: "computing AER", })?; - if aer.elevation_deg >= self.elevation_mask_deg { + if aer.elevation_deg >= self.elevation_mask_deg && !aer.is_obstructed() { // Only update the noises if the measurement is valid. let noises = self.noises(rx.orbit.epoch, rng)?; From 1ec99a998b931fd339ccc3472423e3567097e7dc Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Sun, 8 Dec 2024 08:29:57 -0700 Subject: [PATCH 38/38] Fix OD residual plotting + clippy --- examples/04_lro_od/main.rs | 2 +- examples/04_lro_od/plot_od_rslt.py | 3 +-- src/dynamics/mod.rs | 6 +----- src/lib.rs | 3 --- src/md/opti/multipleshooting/multishoot.rs | 6 +++--- src/md/opti/raphson_finite_diff.rs | 2 +- src/md/opti/raphson_hyperdual.rs | 2 +- src/md/opti/targeter.rs | 2 +- src/od/process/export.rs | 4 ++-- src/propagators/instance.rs | 2 +- 10 files changed, 12 insertions(+), 20 deletions(-) diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs index 6a92101b..69e61b91 100644 --- a/examples/04_lro_od/main.rs +++ b/examples/04_lro_od/main.rs @@ -256,7 +256,7 @@ fn main() -> Result<(), Box> { SNC3::from_diagonal(10 * Unit::Minute, &[1e-12, 1e-12, 1e-12]), ); - // We'll set up the OD process to reject measurements whose residuals are mover than 4 sigmas away from what we expect. + // We'll set up the OD process to reject measurements whose residuals are move than 3 sigmas away from what we expect. let mut odp = SpacecraftODProcess::ckf( setup.with(initial_estimate.state().with_stm(), almanac.clone()), kf, diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 176d53f0..cf8b6e9b 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -15,7 +15,6 @@ def main(path: str): df = ( df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")) .sort("Epoch (UTC)", descending=False) - .with_columns(df["Residual ratio"].fill_null(0.0).alias("Residual ratio")) ) all_msr_types = ["Range (km)", "Doppler (km/s)", "Azimuth (deg)", "Elevation (deg)"] @@ -40,7 +39,7 @@ def main(path: str): ) # Convert the Polars column to a NumPy array for compatibility with scipy and Plotly - residual_ratio = df["Residual ratio"].to_numpy() + residual_ratio = df["Residual ratio"].drop_nulls().to_numpy() # Create QQ plot qq = stats.probplot(residual_ratio) diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index a356a5ee..166bb6d7 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -39,11 +39,6 @@ pub mod orbital; use self::guidance::GuidanceError; pub use self::orbital::*; -/// The gravity module handles spherical harmonics only. It _must_ be combined with a OrbitalDynamics dynamics -/// -/// This module allows loading gravity models from [PDS](http://pds-geosciences.wustl.edu/), [EGM2008](http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm2008/) and GMAT's own COF files. -// pub mod gravity; - /// The spacecraft module allows for simulation of spacecraft dynamics in general, including propulsion/maneuvers. pub mod spacecraft; pub use self::spacecraft::*; @@ -63,6 +58,7 @@ pub mod drag; pub use self::drag::*; /// Define the spherical harmonic models. +/// This module allows loading gravity models from [PDS](http://pds-geosciences.wustl.edu/), [EGM2008](http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm2008/) and GMAT's own COF files. pub mod sph_harmonics; pub use self::sph_harmonics::*; diff --git a/src/lib.rs b/src/lib.rs index 0c7fcdfe..26b0c6f2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -51,9 +51,6 @@ pub mod io; /// All the orbital determination and spacecraft navigation tools and functions. pub mod od; -/// Navigation submodule, relevant to both ground based navigation (orbit determination) and onboard navigation (part of the Guidance, Navigation and Control subsystem) -// pub mod nav; - /// All of the mission design and mission analysis tools and functions pub mod md; diff --git a/src/md/opti/multipleshooting/multishoot.rs b/src/md/opti/multipleshooting/multishoot.rs index c31ac95b..9e4145d0 100644 --- a/src/md/opti/multipleshooting/multishoot.rs +++ b/src/md/opti/multipleshooting/multishoot.rs @@ -59,7 +59,7 @@ pub struct MultipleShooting<'a, T: MultishootNode, const VT: usize, const OT pub all_dvs: Vec>, } -impl<'a, T: MultishootNode, const VT: usize, const OT: usize> MultipleShooting<'a, T, VT, OT> { +impl, const VT: usize, const OT: usize> MultipleShooting<'_, T, VT, OT> { /// Solve the multiple shooting problem by finding the arrangement of nodes to minimize the cost function. pub fn solve( &mut self, @@ -278,8 +278,8 @@ impl<'a, T: MultishootNode, const VT: usize, const OT: usize> MultipleShooti } } -impl<'a, T: MultishootNode, const VT: usize, const OT: usize> fmt::Display - for MultipleShooting<'a, T, VT, OT> +impl, const VT: usize, const OT: usize> fmt::Display + for MultipleShooting<'_, T, VT, OT> { #[allow(clippy::or_fun_call, clippy::clone_on_copy)] fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { diff --git a/src/md/opti/raphson_finite_diff.rs b/src/md/opti/raphson_finite_diff.rs index 260fd045..288991c9 100644 --- a/src/md/opti/raphson_finite_diff.rs +++ b/src/md/opti/raphson_finite_diff.rs @@ -33,7 +33,7 @@ use snafu::{ensure, ResultExt}; #[cfg(not(target_arch = "wasm32"))] use std::time::Instant; -impl<'a, const V: usize, const O: usize> Targeter<'a, V, O> { +impl Targeter<'_, V, O> { /// Differential correction using finite differencing #[allow(clippy::comparison_chain)] pub fn try_achieve_fd( diff --git a/src/md/opti/raphson_hyperdual.rs b/src/md/opti/raphson_hyperdual.rs index eda4ca32..3663713c 100644 --- a/src/md/opti/raphson_hyperdual.rs +++ b/src/md/opti/raphson_hyperdual.rs @@ -30,7 +30,7 @@ use crate::utils::are_eigenvalues_stable; #[cfg(not(target_arch = "wasm32"))] use std::time::Instant; -impl<'a, const V: usize, const O: usize> Targeter<'a, V, O> { +impl Targeter<'_, V, O> { /// Differential correction using hyperdual numbers for the objectives #[allow(clippy::comparison_chain)] pub fn try_achieve_dual( diff --git a/src/md/opti/targeter.rs b/src/md/opti/targeter.rs index 389e4edf..2011e0b7 100644 --- a/src/md/opti/targeter.rs +++ b/src/md/opti/targeter.rs @@ -48,7 +48,7 @@ pub struct Targeter<'a, const V: usize, const O: usize> { pub iterations: usize, } -impl<'a, const V: usize, const O: usize> fmt::Display for Targeter<'a, V, O> { +impl fmt::Display for Targeter<'_, V, O> { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { let mut objmsg = String::from(""); for obj in &self.objectives { diff --git a/src/od/process/export.rs b/src/od/process/export.rs index 92001e9c..e1be1f04 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -42,8 +42,8 @@ use std::path::{Path, PathBuf}; use super::ODProcess; -impl<'a, MsrSize: DimName, Accel: DimName, Trk: TrackerSensitivity> - ODProcess<'a, SpacecraftDynamics, MsrSize, Accel, KF, Trk> +impl> + ODProcess<'_, SpacecraftDynamics, MsrSize, Accel, KF, Trk> where DefaultAllocator: Allocator + Allocator::Size> diff --git a/src/propagators/instance.rs b/src/propagators/instance.rs index c32fd2a0..962e1c32 100644 --- a/src/propagators/instance.rs +++ b/src/propagators/instance.rs @@ -60,7 +60,7 @@ where pub(crate) k: Vec::VecLength>>, } -impl<'a, D: Dynamics> PropInstance<'a, D> +impl PropInstance<'_, D> where DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size>