nyx_space/od/process/solution/
mod.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use crate::linalg::allocator::Allocator;
20use crate::linalg::{DefaultAllocator, DimName};
21use crate::md::trajectory::{Interpolatable, Traj};
22pub use crate::od::estimate::*;
23pub use crate::od::*;
24use indexmap::IndexSet;
25use msr::sensitivity::TrackerSensitivity;
26use nalgebra::OMatrix;
27use std::collections::BTreeMap;
28use std::iter::Zip;
29use std::ops::Add;
30use std::slice::Iter;
31
32use self::msr::MeasurementType;
33
34mod display;
35mod export;
36mod filter_data;
37mod import;
38mod smooth;
39mod stats;
40
41pub use stats::NormalizedConsistency;
42
43/// The `ODSolution` structure is designed to manage and analyze the results of an OD process, including
44/// smoothing. It provides various functionalities such as splitting solutions by tracker or measurement type,
45/// joining solutions, and performing statistical analyses.
46///
47/// **Note:** Many methods in this structure assume that the solution has been split into subsets using the `split()` method.
48/// Calling these methods without first splitting will make analysis of operations results less obvious.
49///
50/// # Fields
51/// - `estimates`: A vector of state estimates generated during the OD process.
52/// - `residuals`: A vector of residuals corresponding to the state estimates.
53/// - `gains`: Filter gains used for measurement updates. These are set to `None` after running the smoother.
54/// - `filter_smoother_ratios`: Filter-smoother consistency ratios. These are set to `None` before running the smoother.
55/// - `devices`: A map of tracking devices used in the OD process.
56/// - `measurement_types`: A set of unique measurement types used in the OD process.
57///
58/// Implementation detail: these are not stored in vectors to allow for multiple estimates at the same time, e.g. when
59/// there are simultaneous measurements of angles and the filter processes each as a scalar.
60///
61#[derive(Clone, Debug)]
62#[allow(clippy::upper_case_acronyms)]
63pub struct ODSolution<StateType, EstType, MsrSize, Trk>
64where
65    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
66    EstType: Estimate<StateType>,
67    MsrSize: DimName,
68    Trk: TrackerSensitivity<StateType, StateType>,
69    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
70    DefaultAllocator: Allocator<<StateType as State>::Size>
71        + Allocator<<StateType as State>::VecLength>
72        + Allocator<MsrSize>
73        + Allocator<MsrSize, <StateType as State>::Size>
74        + Allocator<MsrSize, MsrSize>
75        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
76        + Allocator<<StateType as State>::Size, MsrSize>,
77{
78    /// Vector of estimates available after a pass
79    pub estimates: Vec<EstType>,
80    /// Vector of residuals available after a pass
81    pub residuals: Vec<Option<Residual<MsrSize>>>,
82    /// Vector of filter gains used for each measurement update, all None after running the smoother.
83    pub gains: Vec<Option<OMatrix<f64, <StateType as State>::Size, MsrSize>>>,
84    /// Filter-smoother consistency ratios, all None before running the smoother.
85    pub filter_smoother_ratios: Vec<Option<OVector<f64, <StateType as State>::Size>>>,
86    /// Tracking devices
87    pub devices: BTreeMap<String, Trk>,
88    pub measurement_types: IndexSet<MeasurementType>,
89}
90
91impl<StateType, EstType, MsrSize, Trk> ODSolution<StateType, EstType, MsrSize, Trk>
92where
93    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
94    EstType: Estimate<StateType>,
95    MsrSize: DimName,
96    Trk: TrackerSensitivity<StateType, StateType>,
97    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
98    DefaultAllocator: Allocator<<StateType as State>::Size>
99        + Allocator<<StateType as State>::VecLength>
100        + Allocator<MsrSize>
101        + Allocator<MsrSize, <StateType as State>::Size>
102        + Allocator<MsrSize, MsrSize>
103        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
104        + Allocator<<StateType as State>::Size, MsrSize>,
105{
106    pub fn new(
107        devices: BTreeMap<String, Trk>,
108        measurement_types: IndexSet<MeasurementType>,
109    ) -> Self {
110        Self {
111            estimates: Vec::new(),
112            residuals: Vec::new(),
113            gains: Vec::new(),
114            filter_smoother_ratios: Vec::new(),
115            devices,
116            measurement_types,
117        }
118    }
119
120    /// Pushes a new measurement update result, ensuring proper sizes of the arrays.
121    pub(crate) fn push_measurement_update(
122        &mut self,
123        estimate: EstType,
124        residual: Residual<MsrSize>,
125        gain: Option<OMatrix<f64, <StateType as State>::Size, MsrSize>>,
126    ) {
127        self.estimates.push(estimate);
128        self.residuals.push(Some(residual));
129        self.gains.push(gain);
130        self.filter_smoother_ratios.push(None);
131    }
132
133    /// Pushes a new time update result, ensuring proper sizes of the arrays.
134    pub(crate) fn push_time_update(&mut self, estimate: EstType) {
135        self.estimates.push(estimate);
136        self.residuals.push(None);
137        self.gains.push(None);
138        self.filter_smoother_ratios.push(None);
139    }
140
141    /// Returns a zipper iterator on the estimates and the associated residuals.
142    pub fn results(&self) -> Zip<Iter<'_, EstType>, Iter<'_, Option<Residual<MsrSize>>>> {
143        self.estimates.iter().zip(self.residuals.iter())
144    }
145
146    /// Returns True if this is the result of a filter run
147    pub fn is_filter_run(&self) -> bool {
148        self.gains.iter().flatten().count() > 0
149    }
150
151    /// Returns True if this is the result of a smoother run
152    pub fn is_smoother_run(&self) -> bool {
153        self.filter_smoother_ratios.iter().flatten().count() > 0
154    }
155
156    /// Builds the navigation trajectory for the estimated state only
157    pub fn to_traj(&self) -> Result<Traj<StateType>, NyxError>
158    where
159        DefaultAllocator: Allocator<StateType::VecLength>,
160    {
161        if self.estimates.is_empty() {
162            Err(NyxError::NoStateData {
163                msg: "No navigation trajectory to generate: run the OD process first".to_string(),
164            })
165        } else {
166            // Make sure to remove duplicate entries.
167            let mut traj = Traj {
168                states: self.estimates.iter().map(|est| est.state()).collect(),
169                name: None,
170            };
171            traj.finalize();
172            Ok(traj)
173        }
174    }
175
176    /// Returns the accepted residuals.
177    pub fn accepted_residuals(&self) -> Vec<Residual<MsrSize>> {
178        self.residuals
179            .iter()
180            .flatten()
181            .filter(|resid| !resid.rejected)
182            .cloned()
183            .collect::<Vec<Residual<MsrSize>>>()
184    }
185
186    /// Returns the rejected residuals.
187    pub fn rejected_residuals(&self) -> Vec<Residual<MsrSize>> {
188        self.residuals
189            .iter()
190            .flatten()
191            .filter(|resid| resid.rejected)
192            .cloned()
193            .collect::<Vec<Residual<MsrSize>>>()
194    }
195}
196
197impl<StateType, EstType, MsrSize, Trk> PartialEq for ODSolution<StateType, EstType, MsrSize, Trk>
198where
199    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
200    EstType: Estimate<StateType>,
201    MsrSize: DimName,
202    Trk: TrackerSensitivity<StateType, StateType> + PartialEq,
203    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
204    DefaultAllocator: Allocator<<StateType as State>::Size>
205        + Allocator<<StateType as State>::VecLength>
206        + Allocator<MsrSize>
207        + Allocator<MsrSize, <StateType as State>::Size>
208        + Allocator<MsrSize, MsrSize>
209        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
210        + Allocator<<StateType as State>::Size, MsrSize>,
211{
212    /// Checks that the covariances are within 1e-8 in norm, the state vectors within 1e-6, the residual ratios within 1e-4, the gains and flight-smoother consistencies within 1e-8.
213    fn eq(&self, other: &Self) -> bool {
214        self.estimates.len() == other.estimates.len()
215            && self.residuals.len() == other.residuals.len()
216            && self.gains.len() == other.gains.len()
217            && self.filter_smoother_ratios.len() == other.filter_smoother_ratios.len()
218            && self.devices == other.devices
219            && self.measurement_types.iter().all(|msr_type| other.measurement_types.contains(msr_type))
220            && self.estimates.iter().zip(other.estimates.iter()).all(|(mine, theirs)| {
221                (mine.state().to_state_vector() - theirs.state().to_state_vector()).norm() < 1e-6 &&
222                (mine.covar() - theirs.covar()).norm() < 1e-8
223            })
224            && self.residuals.iter().zip(other.residuals.iter()).all(|(mine, theirs)| {
225                if let Some(mine) = mine {
226                    if let Some(theirs) = theirs {
227                        (mine.ratio - theirs.ratio).abs() < 1e-4
228                    } else {
229                        false
230                    }
231                } else {
232                    theirs.is_none()
233                }
234            })
235            // Now check for near equality of gains
236            && self.gains.iter().zip(other.gains.iter()).all(|(my_k, other_k)| {
237                if let Some(my_k) = my_k {
238                    if let Some(other_k) = other_k {
239                        (my_k - other_k).norm() < 1e-8
240                    } else {
241                        false
242                    }
243                } else {
244                    other_k.is_none()
245                }
246            })
247            // Now check for near equality of F-S ratios
248            && self.filter_smoother_ratios.iter().zip(other.filter_smoother_ratios.iter()).all(|(my_fs, other_fs)| {
249                if let Some(my_fs) = my_fs {
250                    if let Some(other_fs) = other_fs {
251                        (my_fs - other_fs).norm() < 1e-8
252                    } else {
253                        false
254                    }
255                } else {
256                    other_fs.is_none()
257                }
258            })
259    }
260}
nyx_space/od/process/solution/mod.rs

nyx_space/od/process/solution/
mod.rs
