nyx_space/od/estimate/
residual.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, OVector};
21use crate::od::msr::MeasurementType;
22use hifitime::Epoch;
23use indexmap::IndexSet;
24use std::fmt;
25
26/// Stores an Estimate, as the result of a `time_update` or `measurement_update`.
27#[derive(Debug, Clone, PartialEq)]
28pub struct Residual<M>
29where
30    M: DimName,
31    DefaultAllocator: Allocator<M>,
32{
33    /// Date time of this Residual
34    pub epoch: Epoch,
35    /// The prefit residual in the units of the measurement type
36    pub prefit: OVector<f64, M>,
37    /// The postfit residual in the units of the measurement type
38    pub postfit: OVector<f64, M>,
39    /// The prefit residual ratio computed as the Mahalanobis distance, i.e. it is always positive
40    /// and computed as `r' * (H*P*H')^-1 * r`, where `r` is the prefit residual, `H` is the sensitivity matrix, and `P` is the covariance matrix.
41    /// To assess the performance, look at the Chi Square distribution for the number of measurements, e.g. 2 for range and range-rate.
42    pub ratio: f64,
43    /// The tracker measurement noise (variance)) for this tracker at this time.
44    pub tracker_msr_noise: OVector<f64, M>,
45    /// Whether or not this was rejected
46    pub rejected: bool,
47    /// Name of the tracker that caused this residual
48    pub tracker: Option<String>,
49    /// Measurement types used to compute this residual (in order)
50    pub msr_types: IndexSet<MeasurementType>,
51    /// The real observation from the tracking arc.
52    pub real_obs: OVector<f64, M>,
53    /// The computed observation as expected from the dynamics of the filter.
54    pub computed_obs: OVector<f64, M>,
55}
56
57impl<M> Residual<M>
58where
59    M: DimName,
60    DefaultAllocator: Allocator<M>,
61{
62    /// An empty estimate. This is useful if wanting to store an estimate outside the scope of a filtering loop.
63    pub fn zeros() -> Self {
64        Self {
65            epoch: Epoch::from_tai_seconds(0.0),
66            prefit: OVector::<f64, M>::zeros(),
67            postfit: OVector::<f64, M>::zeros(),
68            tracker_msr_noise: OVector::<f64, M>::zeros(),
69            ratio: 0.0,
70            rejected: true,
71            tracker: None,
72            msr_types: IndexSet::new(),
73            real_obs: OVector::<f64, M>::zeros(),
74            computed_obs: OVector::<f64, M>::zeros(),
75        }
76    }
77
78    /// Flags a Residual as rejected.
79    pub fn rejected(
80        epoch: Epoch,
81        prefit: OVector<f64, M>,
82        ratio: f64,
83        tracker_msr_covar: OVector<f64, M>,
84        real_obs: OVector<f64, M>,
85        computed_obs: OVector<f64, M>,
86    ) -> Self {
87        Self {
88            epoch,
89            prefit,
90            postfit: OVector::<f64, M>::zeros(),
91            ratio,
92            tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()),
93            rejected: true,
94            tracker: None,
95            msr_types: IndexSet::new(),
96            real_obs,
97            computed_obs,
98        }
99    }
100
101    pub fn accepted(
102        epoch: Epoch,
103        prefit: OVector<f64, M>,
104        postfit: OVector<f64, M>,
105        ratio: f64,
106        tracker_msr_covar: OVector<f64, M>,
107        real_obs: OVector<f64, M>,
108        computed_obs: OVector<f64, M>,
109    ) -> Self {
110        Self {
111            epoch,
112            prefit,
113            postfit,
114            ratio,
115            tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()),
116            rejected: false,
117            tracker: None,
118            msr_types: IndexSet::new(),
119            real_obs,
120            computed_obs,
121        }
122    }
123
124    /// Returns the prefit for this measurement type, if available
125    pub fn prefit(&self, msr_type: MeasurementType) -> Option<f64> {
126        self.msr_types
127            .get_index_of(&msr_type)
128            .map(|idx| self.prefit[idx])
129    }
130
131    /// Returns the postfit for this measurement type, if available
132    pub fn postfit(&self, msr_type: MeasurementType) -> Option<f64> {
133        self.msr_types
134            .get_index_of(&msr_type)
135            .map(|idx| self.postfit[idx])
136    }
137
138    /// Returns the tracker noise for this measurement type, if available
139    pub fn trk_noise(&self, msr_type: MeasurementType) -> Option<f64> {
140        self.msr_types
141            .get_index_of(&msr_type)
142            .map(|idx| self.tracker_msr_noise[idx])
143    }
144
145    /// Returns the real observation for this measurement type, if available
146    pub fn real_obs(&self, msr_type: MeasurementType) -> Option<f64> {
147        self.msr_types
148            .get_index_of(&msr_type)
149            .map(|idx| self.real_obs[idx])
150    }
151
152    /// Returns the computed/expected observation for this measurement type, if available
153    pub fn computed_obs(&self, msr_type: MeasurementType) -> Option<f64> {
154        self.msr_types
155            .get_index_of(&msr_type)
156            .map(|idx| self.computed_obs[idx])
157    }
158}
159
160impl<M> fmt::Display for Residual<M>
161where
162    M: DimName,
163    DefaultAllocator: Allocator<M> + Allocator<M>,
164{
165    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
166        write!(
167            f,
168            "Residual of {:?} from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}",
169            self.msr_types,
170            self.tracker.as_ref().unwrap_or(&"Unknown".to_string()),
171            self.epoch,
172            self.ratio,
173            &self.prefit,
174            &self.postfit
175        )
176    }
177}
178
179impl<M> fmt::LowerExp for Residual<M>
180where
181    M: DimName,
182    DefaultAllocator: Allocator<M> + Allocator<M>,
183{
184    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
185        write!(f, "Prefit {:e} Postfit {:e}", &self.prefit, &self.postfit)
186    }
187}
nyx_space/od/estimate/residual.rs

nyx_space/od/estimate/
residual.rs
