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    /// Whitened prefit innovation: L^-1 * prefit, where S = L * L^T
40    /// Dimensionless, components should be approximately ~N(0, 1) if the model and covariance are consistent.
41    pub whitened_resid: OVector<f64, M>,
42    /// The prefit residual ratio computed as the Mahalanobis distance, i.e. it is always positive
43    /// 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.
44    /// To assess the performance, look at the Chi Square distribution for the number of measurements, e.g. 2 for range and range-rate.
45    pub ratio: f64,
46    /// The tracker measurement noise (variance)) for this tracker at this time.
47    pub tracker_msr_noise: OVector<f64, M>,
48    /// Whether or not this was rejected
49    pub rejected: bool,
50    /// Name of the tracker that caused this residual
51    pub tracker: Option<String>,
52    /// Measurement types used to compute this residual (in order)
53    pub msr_types: IndexSet<MeasurementType>,
54    /// The real observation from the tracking arc.
55    pub real_obs: OVector<f64, M>,
56    /// The computed observation as expected from the dynamics of the filter.
57    pub computed_obs: OVector<f64, M>,
58}
59
60impl<M> Residual<M>
61where
62    M: DimName,
63    DefaultAllocator: Allocator<M>,
64{
65    /// An empty estimate. This is useful if wanting to store an estimate outside the scope of a filtering loop.
66    pub fn zeros() -> Self {
67        Self {
68            epoch: Epoch::from_tai_seconds(0.0),
69            prefit: OVector::<f64, M>::zeros(),
70            postfit: OVector::<f64, M>::zeros(),
71            whitened_resid: OVector::<f64, M>::zeros(),
72            tracker_msr_noise: OVector::<f64, M>::zeros(),
73            ratio: 0.0,
74            rejected: true,
75            tracker: None,
76            msr_types: IndexSet::new(),
77            real_obs: OVector::<f64, M>::zeros(),
78            computed_obs: OVector::<f64, M>::zeros(),
79        }
80    }
81
82    /// Flags a Residual as rejected.
83    pub fn rejected(
84        epoch: Epoch,
85        prefit: OVector<f64, M>,
86        whitened_resid: OVector<f64, M>,
87        ratio: f64,
88        tracker_msr_noise: OVector<f64, M>,
89        real_obs: OVector<f64, M>,
90        computed_obs: OVector<f64, M>,
91    ) -> Self {
92        Self {
93            epoch,
94            prefit,
95            postfit: OVector::<f64, M>::zeros() * f64::NAN,
96            whitened_resid,
97            ratio,
98            tracker_msr_noise,
99            rejected: true,
100            tracker: None,
101            msr_types: IndexSet::new(),
102            real_obs,
103            computed_obs,
104        }
105    }
106
107    pub fn accepted(
108        epoch: Epoch,
109        prefit: OVector<f64, M>,
110        whitened_resid: OVector<f64, M>,
111        postfit: OVector<f64, M>,
112        ratio: f64,
113        tracker_msr_noise: OVector<f64, M>,
114        real_obs: OVector<f64, M>,
115        computed_obs: OVector<f64, M>,
116    ) -> Self {
117        Self {
118            epoch,
119            prefit,
120            whitened_resid,
121            postfit,
122            ratio,
123            tracker_msr_noise,
124            rejected: false,
125            tracker: None,
126            msr_types: IndexSet::new(),
127            real_obs,
128            computed_obs,
129        }
130    }
131
132    /// Returns the prefit for this measurement type, if available
133    pub fn prefit(&self, msr_type: MeasurementType) -> Option<f64> {
134        self.msr_types
135            .get_index_of(&msr_type)
136            .map(|idx| self.prefit[idx])
137    }
138
139    /// Returns the postfit for this measurement type, if available
140    pub fn postfit(&self, msr_type: MeasurementType) -> Option<f64> {
141        self.msr_types
142            .get_index_of(&msr_type)
143            .map(|idx| self.postfit[idx])
144    }
145
146    /// Returns the tracker noise for this measurement type, if available
147    pub fn trk_noise(&self, msr_type: MeasurementType) -> Option<f64> {
148        self.msr_types
149            .get_index_of(&msr_type)
150            .map(|idx| self.tracker_msr_noise[idx])
151    }
152
153    /// Returns the real observation for this measurement type, if available
154    pub fn real_obs(&self, msr_type: MeasurementType) -> Option<f64> {
155        self.msr_types
156            .get_index_of(&msr_type)
157            .map(|idx| self.real_obs[idx])
158    }
159
160    /// Returns the computed/expected observation for this measurement type, if available
161    pub fn computed_obs(&self, msr_type: MeasurementType) -> Option<f64> {
162        self.msr_types
163            .get_index_of(&msr_type)
164            .map(|idx| self.computed_obs[idx])
165    }
166
167    /// Returns the whitened residual for this measurement type, if available
168    pub fn whitened_resid(&self, msr_type: MeasurementType) -> Option<f64> {
169        self.msr_types
170            .get_index_of(&msr_type)
171            .map(|idx| self.whitened_resid[idx])
172    }
173
174    /// Returns the normalized innovation squared (NIS) as the norm squares of the whitened residual
175    pub fn nis(&self) -> f64 {
176        self.whitened_resid.norm_squared()
177    }
178}
179
180impl<M> fmt::Display for Residual<M>
181where
182    M: DimName,
183    DefaultAllocator: Allocator<M> + Allocator<M>,
184{
185    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
186        write!(
187            f,
188            "Residual of {:?} from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}",
189            self.msr_types,
190            self.tracker.as_ref().unwrap_or(&"Unknown".to_string()),
191            self.epoch,
192            self.ratio,
193            &self.prefit,
194            &self.postfit
195        )
196    }
197}
198
199impl<M> fmt::LowerExp for Residual<M>
200where
201    M: DimName,
202    DefaultAllocator: Allocator<M> + Allocator<M>,
203{
204    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
205        write!(f, "Prefit {:e} Postfit {:e}", &self.prefit, &self.postfit)
206    }
207}
nyx_space/od/estimate/residual.rs

nyx_space/od/estimate/
residual.rs
