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}
52
53impl<M> Residual<M>
54where
55    M: DimName,
56    DefaultAllocator: Allocator<M>,
57{
58    /// An empty estimate. This is useful if wanting to store an estimate outside the scope of a filtering loop.
59    pub fn zeros() -> Self {
60        Self {
61            epoch: Epoch::from_tai_seconds(0.0),
62            prefit: OVector::<f64, M>::zeros(),
63            postfit: OVector::<f64, M>::zeros(),
64            tracker_msr_noise: OVector::<f64, M>::zeros(),
65            ratio: 0.0,
66            rejected: true,
67            tracker: None,
68            msr_types: IndexSet::new(),
69        }
70    }
71
72    /// Flags a Residual as rejected.
73    pub fn rejected(
74        epoch: Epoch,
75        prefit: OVector<f64, M>,
76        ratio: f64,
77        tracker_msr_covar: OVector<f64, M>,
78    ) -> Self {
79        Self {
80            epoch,
81            prefit,
82            postfit: OVector::<f64, M>::zeros(),
83            ratio,
84            tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()),
85            rejected: true,
86            tracker: None,
87            msr_types: IndexSet::new(),
88        }
89    }
90
91    pub fn accepted(
92        epoch: Epoch,
93        prefit: OVector<f64, M>,
94        postfit: OVector<f64, M>,
95        ratio: f64,
96        tracker_msr_covar: OVector<f64, M>,
97    ) -> Self {
98        Self {
99            epoch,
100            prefit,
101            postfit,
102            ratio,
103            tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()),
104            rejected: false,
105            tracker: None,
106            msr_types: IndexSet::new(),
107        }
108    }
109
110    /// Returns the prefit for this measurement type, if available
111    pub fn prefit(&self, msr_type: MeasurementType) -> Option<f64> {
112        self.msr_types
113            .get_index_of(&msr_type)
114            .map(|idx| self.prefit[idx])
115    }
116
117    /// Returns the postfit for this measurement type, if available
118    pub fn postfit(&self, msr_type: MeasurementType) -> Option<f64> {
119        self.msr_types
120            .get_index_of(&msr_type)
121            .map(|idx| self.postfit[idx])
122    }
123
124    /// Returns the tracker noise for this measurement type, if available
125    pub fn trk_noise(&self, msr_type: MeasurementType) -> Option<f64> {
126        self.msr_types
127            .get_index_of(&msr_type)
128            .map(|idx| self.tracker_msr_noise[idx])
129    }
130}
131
132impl<M> fmt::Display for Residual<M>
133where
134    M: DimName,
135    DefaultAllocator: Allocator<M> + Allocator<M>,
136{
137    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
138        write!(
139            f,
140            "Residual of {:?} from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}",
141            self.msr_types,
142            self.tracker.as_ref().unwrap_or(&"Unknown".to_string()),
143            self.epoch,
144            self.ratio,
145            &self.prefit,
146            &self.postfit
147        )
148    }
149}
150
151impl<M> fmt::LowerExp for Residual<M>
152where
153    M: DimName,
154    DefaultAllocator: Allocator<M> + Allocator<M>,
155{
156    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
157        write!(f, "Prefit {:e} Postfit {:e}", &self.prefit, &self.postfit)
158    }
159}
nyx_space/od/estimate/residual.rs

nyx_space/od/estimate/
residual.rs
