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
41/// The `ODSolution` structure is designed to manage and analyze the results of an OD process, including
42/// smoothing. It provides various functionalities such as splitting solutions by tracker or measurement type,
43/// joining solutions, and performing statistical analyses.
44///
45/// **Note:** Many methods in this structure assume that the solution has been split into subsets using the `split()` method.
46/// Calling these methods without first splitting will make analysis of operations results less obvious.
47///
48/// # Fields
49/// - `estimates`: A vector of state estimates generated during the OD process.
50/// - `residuals`: A vector of residuals corresponding to the state estimates.
51/// - `gains`: Filter gains used for measurement updates. These are set to `None` after running the smoother.
52/// - `filter_smoother_ratios`: Filter-smoother consistency ratios. These are set to `None` before running the smoother.
53/// - `devices`: A map of tracking devices used in the OD process.
54/// - `measurement_types`: A set of unique measurement types used in the OD process.
55///
56/// Implementation detail: these are not stored in vectors to allow for multiple estimates at the same time, e.g. when
57/// there are simultaneous measurements of angles and the filter processes each as a scalar.
58///
59#[derive(Clone, Debug)]
60#[allow(clippy::upper_case_acronyms)]
61pub struct ODSolution<StateType, EstType, MsrSize, Trk>
62where
63    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
64    EstType: Estimate<StateType>,
65    MsrSize: DimName,
66    Trk: TrackerSensitivity<StateType, StateType>,
67    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
68    DefaultAllocator: Allocator<<StateType as State>::Size>
69        + Allocator<<StateType as State>::VecLength>
70        + Allocator<MsrSize>
71        + Allocator<MsrSize, <StateType as State>::Size>
72        + Allocator<MsrSize, MsrSize>
73        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
74        + Allocator<<StateType as State>::Size, MsrSize>,
75{
76    /// Vector of estimates available after a pass
77    pub estimates: Vec<EstType>,
78    /// Vector of residuals available after a pass
79    pub residuals: Vec<Option<Residual<MsrSize>>>,
80    /// Vector of filter gains used for each measurement update, all None after running the smoother.
81    pub gains: Vec<Option<OMatrix<f64, <StateType as State>::Size, MsrSize>>>,
82    /// Filter-smoother consistency ratios, all None before running the smoother.
83    pub filter_smoother_ratios: Vec<Option<OVector<f64, <StateType as State>::Size>>>,
84    /// Tracking devices
85    pub devices: BTreeMap<String, Trk>,
86    pub measurement_types: IndexSet<MeasurementType>,
87}
88
89impl<StateType, EstType, MsrSize, Trk> ODSolution<StateType, EstType, MsrSize, Trk>
90where
91    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
92    EstType: Estimate<StateType>,
93    MsrSize: DimName,
94    Trk: TrackerSensitivity<StateType, StateType>,
95    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
96    DefaultAllocator: Allocator<<StateType as State>::Size>
97        + Allocator<<StateType as State>::VecLength>
98        + Allocator<MsrSize>
99        + Allocator<MsrSize, <StateType as State>::Size>
100        + Allocator<MsrSize, MsrSize>
101        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
102        + Allocator<<StateType as State>::Size, MsrSize>,
103{
104    pub fn new(
105        devices: BTreeMap<String, Trk>,
106        measurement_types: IndexSet<MeasurementType>,
107    ) -> Self {
108        Self {
109            estimates: Vec::new(),
110            residuals: Vec::new(),
111            gains: Vec::new(),
112            filter_smoother_ratios: Vec::new(),
113            devices,
114            measurement_types,
115        }
116    }
117
118    /// Pushes a new measurement update result, ensuring proper sizes of the arrays.
119    pub(crate) fn push_measurement_update(
120        &mut self,
121        estimate: EstType,
122        residual: Residual<MsrSize>,
123        gain: Option<OMatrix<f64, <StateType as State>::Size, MsrSize>>,
124    ) {
125        self.estimates.push(estimate);
126        self.residuals.push(Some(residual));
127        self.gains.push(gain);
128        self.filter_smoother_ratios.push(None);
129    }
130
131    /// Pushes a new time update result, ensuring proper sizes of the arrays.
132    pub(crate) fn push_time_update(&mut self, estimate: EstType) {
133        self.estimates.push(estimate);
134        self.residuals.push(None);
135        self.gains.push(None);
136        self.filter_smoother_ratios.push(None);
137    }
138
139    /// Returns a zipper iterator on the estimates and the associated residuals.
140    pub fn results(&self) -> Zip<Iter<'_, EstType>, Iter<'_, Option<Residual<MsrSize>>>> {
141        self.estimates.iter().zip(self.residuals.iter())
142    }
143
144    /// Returns True if this is the result of a filter run
145    pub fn is_filter_run(&self) -> bool {
146        self.gains.iter().flatten().count() > 0
147    }
148
149    /// Returns True if this is the result of a smoother run
150    pub fn is_smoother_run(&self) -> bool {
151        self.filter_smoother_ratios.iter().flatten().count() > 0
152    }
153
154    /// Builds the navigation trajectory for the estimated state only
155    pub fn to_traj(&self) -> Result<Traj<StateType>, NyxError>
156    where
157        DefaultAllocator: Allocator<StateType::VecLength>,
158    {
159        if self.estimates.is_empty() {
160            Err(NyxError::NoStateData {
161                msg: "No navigation trajectory to generate: run the OD process first".to_string(),
162            })
163        } else {
164            // Make sure to remove duplicate entries.
165            let mut traj = Traj {
166                states: self.estimates.iter().map(|est| est.state()).collect(),
167                name: None,
168            };
169            traj.finalize();
170            Ok(traj)
171        }
172    }
173
174    /// Returns the accepted residuals.
175    pub fn accepted_residuals(&self) -> Vec<Residual<MsrSize>> {
176        self.residuals
177            .iter()
178            .flatten()
179            .filter(|resid| !resid.rejected)
180            .cloned()
181            .collect::<Vec<Residual<MsrSize>>>()
182    }
183
184    /// Returns the rejected residuals.
185    pub fn rejected_residuals(&self) -> Vec<Residual<MsrSize>> {
186        self.residuals
187            .iter()
188            .flatten()
189            .filter(|resid| resid.rejected)
190            .cloned()
191            .collect::<Vec<Residual<MsrSize>>>()
192    }
193}
194
195impl<StateType, EstType, MsrSize, Trk> PartialEq for ODSolution<StateType, EstType, MsrSize, Trk>
196where
197    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
198    EstType: Estimate<StateType>,
199    MsrSize: DimName,
200    Trk: TrackerSensitivity<StateType, StateType> + PartialEq,
201    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
202    DefaultAllocator: Allocator<<StateType as State>::Size>
203        + Allocator<<StateType as State>::VecLength>
204        + Allocator<MsrSize>
205        + Allocator<MsrSize, <StateType as State>::Size>
206        + Allocator<MsrSize, MsrSize>
207        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
208        + Allocator<<StateType as State>::Size, MsrSize>,
209{
210    /// 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.
211    fn eq(&self, other: &Self) -> bool {
212        self.estimates.len() == other.estimates.len()
213            && self.residuals.len() == other.residuals.len()
214            && self.gains.len() == other.gains.len()
215            && self.filter_smoother_ratios.len() == other.filter_smoother_ratios.len()
216            && self.devices == other.devices
217            && self.measurement_types.iter().all(|msr_type| other.measurement_types.contains(msr_type))
218            && self.estimates.iter().zip(other.estimates.iter()).all(|(mine, theirs)| {
219                (mine.state().to_state_vector() - theirs.state().to_state_vector()).norm() < 1e-6 &&
220                (mine.covar() - theirs.covar()).norm() < 1e-8
221            })
222            && self.residuals.iter().zip(other.residuals.iter()).all(|(mine, theirs)| {
223                if let Some(mine) = mine {
224                    if let Some(theirs) = theirs {
225                        (mine.ratio - theirs.ratio).abs() < 1e-4
226                    } else {
227                        false
228                    }
229                } else {
230                    theirs.is_none()
231                }
232            })
233            // Now check for near equality of gains
234            && self.gains.iter().zip(other.gains.iter()).all(|(my_k, other_k)| {
235                if let Some(my_k) = my_k {
236                    if let Some(other_k) = other_k {
237                        (my_k - other_k).norm() < 1e-8
238                    } else {
239                        false
240                    }
241                } else {
242                    other_k.is_none()
243                }
244            })
245            // Now check for near equality of F-S ratios
246            && self.filter_smoother_ratios.iter().zip(other.filter_smoother_ratios.iter()).all(|(my_fs, other_fs)| {
247                if let Some(my_fs) = my_fs {
248                    if let Some(other_fs) = other_fs {
249                        (my_fs - other_fs).norm() < 1e-8
250                    } else {
251                        false
252                    }
253                } else {
254                    other_fs.is_none()
255                }
256            })
257    }
258}
nyx_space/od/process/solution/mod.rs

nyx_space/od/process/solution/
mod.rs
