nyx_space/od/process/solution/
filter_data.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, OMatrix};
21use crate::md::trajectory::Interpolatable;
22pub use crate::od::estimate::*;
23pub use crate::od::*;
24use indexmap::IndexSet;
25use msr::sensitivity::TrackerSensitivity;
26use std::ops::Add;
27
28use self::msr::MeasurementType;
29
30use super::ODSolution;
31
32#[derive(Clone, Debug, PartialEq)]
33#[allow(clippy::upper_case_acronyms)]
34pub struct ODRecord<StateType, EstType, MsrSize>
35where
36    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
37    EstType: Estimate<StateType>,
38    MsrSize: DimName,
39    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
40    DefaultAllocator: Allocator<<StateType as State>::Size>
41        + Allocator<<StateType as State>::VecLength>
42        + Allocator<MsrSize>
43        + Allocator<MsrSize, <StateType as State>::Size>
44        + Allocator<MsrSize, MsrSize>
45        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
46        + Allocator<<StateType as State>::Size, MsrSize>,
47{
48    /// Vector of estimates available after a pass
49    pub estimate: EstType,
50    /// Vector of residuals available after a pass
51    pub residual: Option<Residual<MsrSize>>,
52    /// Vector of filter gains used for each measurement update, all None after running the smoother.
53    pub gain: Option<OMatrix<f64, <StateType as State>::Size, MsrSize>>,
54    /// Filter-smoother consistency ratios, all None before running the smoother.
55    pub filter_smoother_ratio: Option<OVector<f64, <StateType as State>::Size>>,
56}
57
58impl<StateType, EstType, MsrSize, Trk> ODSolution<StateType, EstType, MsrSize, Trk>
59where
60    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
61    EstType: Estimate<StateType>,
62    MsrSize: DimName,
63    Trk: TrackerSensitivity<StateType, StateType>,
64    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
65    DefaultAllocator: Allocator<<StateType as State>::Size>
66        + Allocator<<StateType as State>::VecLength>
67        + Allocator<MsrSize>
68        + Allocator<MsrSize, <StateType as State>::Size>
69        + Allocator<MsrSize, MsrSize>
70        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
71        + Allocator<<StateType as State>::Size, MsrSize>,
72{
73    /// Returns a set of tuples of tracker and measurement types in this OD solution, e.g. `{(Canberra, Range), (Canberra, Doppler)}`.
74    pub fn unique(&self) -> IndexSet<(String, MeasurementType)> {
75        let mut mapping = IndexSet::new();
76        for resid in self.residuals.iter().flatten() {
77            for k in &resid.msr_types {
78                mapping.insert((
79                    resid.tracker.clone().expect("residual tracker not set!"),
80                    *k,
81                ));
82            }
83        }
84        mapping
85    }
86
87    /// Returns this OD solution without any time update
88    pub fn drop_time_updates(mut self) -> Self {
89        let mut estimates = Vec::new();
90        let mut residuals = Vec::new();
91        let mut gains = Vec::new();
92        let mut filter_smoother_ratios = Vec::new();
93
94        for (est, (resid_opt, (gain_opt, fsr_opt))) in self.estimates.iter().zip(
95            self.residuals
96                .iter()
97                .zip(self.gains.iter().zip(self.filter_smoother_ratios.iter())),
98        ) {
99            if resid_opt.is_none() {
100                continue;
101            }
102
103            estimates.push(est.clone());
104            residuals.push(resid_opt.clone());
105            gains.push(gain_opt.clone());
106            filter_smoother_ratios.push(fsr_opt.clone());
107        }
108
109        self.estimates = estimates;
110        self.residuals = residuals;
111        self.gains = gains;
112        self.filter_smoother_ratios = filter_smoother_ratios;
113
114        self
115    }
116
117    /// Returns this OD solution with only data from the desired measurement type, dropping all time updates.
118    pub fn filter_by_msr_type(mut self, msr_type: MeasurementType) -> Self {
119        let mut estimates = Vec::new();
120        let mut residuals = Vec::new();
121        let mut gains = Vec::new();
122        let mut filter_smoother_ratios = Vec::new();
123
124        for (est, (resid_opt, (gain_opt, fsr_opt))) in self.estimates.iter().zip(
125            self.residuals
126                .iter()
127                .zip(self.gains.iter().zip(self.filter_smoother_ratios.iter())),
128        ) {
129            match resid_opt {
130                None => continue, // Drop all time updates
131                Some(resid) => {
132                    if resid.msr_types.contains(&msr_type) {
133                        estimates.push(est.clone());
134                        residuals.push(Some(resid.clone()));
135                        gains.push(gain_opt.clone());
136                        filter_smoother_ratios.push(fsr_opt.clone());
137                    }
138                }
139            }
140        }
141
142        self.estimates = estimates;
143        self.residuals = residuals;
144        self.gains = gains;
145        self.filter_smoother_ratios = filter_smoother_ratios;
146
147        self
148    }
149
150    /// Returns this OD solution with only data from the desired tracker, dropping all time updates.
151    pub fn filter_by_tracker(mut self, tracker: String) -> Self {
152        let mut estimates = Vec::new();
153        let mut residuals = Vec::new();
154        let mut gains = Vec::new();
155        let mut filter_smoother_ratios = Vec::new();
156
157        for (est, (resid_opt, (gain_opt, fsr_opt))) in self.estimates.iter().zip(
158            self.residuals
159                .iter()
160                .zip(self.gains.iter().zip(self.filter_smoother_ratios.iter())),
161        ) {
162            match resid_opt {
163                None => continue, // Drop all time updates
164                Some(resid) => {
165                    if resid.tracker == Some(tracker.clone()) {
166                        estimates.push(est.clone());
167                        residuals.push(Some(resid.clone()));
168                        gains.push(gain_opt.clone());
169                        filter_smoother_ratios.push(fsr_opt.clone());
170                    }
171                }
172            }
173        }
174
175        self.estimates = estimates;
176        self.residuals = residuals;
177        self.gains = gains;
178        self.filter_smoother_ratios = filter_smoother_ratios;
179
180        self
181    }
182
183    /// Returns this OD solution with all data except from the desired tracker, including all time updates
184    pub fn exclude_tracker(mut self, excluded_tracker: String) -> Self {
185        let mut estimates = Vec::new();
186        let mut residuals = Vec::new();
187        let mut gains = Vec::new();
188        let mut filter_smoother_ratios = Vec::new();
189
190        for (est, (resid_opt, (gain_opt, fsr_opt))) in self.estimates.iter().zip(
191            self.residuals
192                .iter()
193                .zip(self.gains.iter().zip(self.filter_smoother_ratios.iter())),
194        ) {
195            if let Some(resid) = resid_opt {
196                if resid.tracker.is_none() || resid.tracker.as_ref().unwrap() == &excluded_tracker {
197                    continue;
198                }
199            }
200            // Otherwise, include in the result.
201            estimates.push(est.clone());
202            residuals.push(resid_opt.clone());
203            gains.push(gain_opt.clone());
204            filter_smoother_ratios.push(fsr_opt.clone());
205        }
206
207        self.estimates = estimates;
208        self.residuals = residuals;
209        self.gains = gains;
210        self.filter_smoother_ratios = filter_smoother_ratios;
211
212        self
213    }
214
215    /// Split this OD solution per tracker and per measurement type, dropping all time updates.
216    pub fn split(self) -> Vec<Self> {
217        let uniques = self.unique();
218        let mut splt = Vec::with_capacity(uniques.len());
219        for (tracker, msr_type) in uniques {
220            splt.push(
221                self.clone()
222                    .filter_by_tracker(tracker)
223                    .filter_by_msr_type(msr_type),
224            );
225        }
226        splt
227    }
228
229    /// Merge this OD solution with another one, returning a new OD solution.
230    pub fn merge(mut self, mut other: Self) -> Self {
231        self.estimates.append(&mut other.estimates);
232        self.residuals.append(&mut other.residuals);
233        self.gains.append(&mut other.gains);
234        self.filter_smoother_ratios
235            .append(&mut other.filter_smoother_ratios);
236
237        // Sort to ensure chronological order using indices based permutations.
238        // Generate indices representing original positions
239        let mut indices: Vec<usize> = (0..self.estimates.len()).collect();
240
241        // Sort indices based on estimates' epochs
242        indices.sort_by(|&a, &b| {
243            self.estimates[a]
244                .epoch()
245                .partial_cmp(&self.estimates[b].epoch())
246                .expect("Epoch comparison failed")
247        });
248
249        // Apply permutation to both vectors in-place
250        for i in 0..indices.len() {
251            let current = i;
252            while indices[current] != current {
253                let target = indices[current];
254                indices.swap(current, target);
255                self.estimates.swap(current, target);
256                self.residuals.swap(current, target);
257                self.gains.swap(current, target);
258                self.filter_smoother_ratios.swap(current, target);
259            }
260        }
261
262        self
263    }
264
265    pub fn at(&self, epoch: Epoch) -> Option<ODRecord<StateType, EstType, MsrSize>> {
266        if let Ok(index) = self
267            .estimates
268            .binary_search_by(|est| est.epoch().cmp(&epoch))
269        {
270            Some(ODRecord {
271                estimate: self.estimates[index].clone(),
272                residual: self.residuals[index].clone(),
273                gain: self.gains[index].clone(),
274                filter_smoother_ratio: self.filter_smoother_ratios[index].clone(),
275            })
276        } else {
277            None
278        }
279    }
280}
nyx_space/od/process/solution/filter_data.rs

nyx_space/od/process/solution/
filter_data.rs
