nyx_space/od/process/solution/
stats.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, DimMin, DimName, DimSub};
21use crate::md::trajectory::{Interpolatable, Traj};
22pub use crate::od::estimate::*;
23pub use crate::od::*;
24use log::warn;
25use msr::sensitivity::TrackerSensitivity;
26use nalgebra::Const;
27use snafu::ResultExt;
28use statrs::distribution::{ContinuousCDF, Normal};
29use std::ops::Add;
30
31use super::ODSolution;
32
33impl<StateType, EstType, MsrSize, Trk> ODSolution<StateType, EstType, MsrSize, Trk>
34where
35    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
36    EstType: Estimate<StateType>,
37    MsrSize: DimName,
38    Trk: TrackerSensitivity<StateType, StateType>,
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    /// Returns the root mean square of the prefit residuals
49    pub fn rms_prefit_residuals(&self) -> f64 {
50        let mut sum = 0.0;
51        for residual in self.residuals.iter().flatten() {
52            sum += residual.prefit.dot(&residual.prefit);
53        }
54        (sum / (self.residuals.len() as f64)).sqrt()
55    }
56
57    /// Returns the root mean square of the postfit residuals
58    pub fn rms_postfit_residuals(&self) -> f64 {
59        let mut sum = 0.0;
60        for residual in self.residuals.iter().flatten() {
61            sum += residual.postfit.dot(&residual.postfit);
62        }
63        (sum / (self.residuals.len() as f64)).sqrt()
64    }
65
66    /// Returns the root mean square of the prefit residual ratios
67    pub fn rms_residual_ratios(&self) -> f64 {
68        let mut sum = 0.0;
69        for residual in self.residuals.iter().flatten() {
70            sum += residual.ratio.powi(2);
71        }
72        (sum / (self.residuals.len() as f64)).sqrt()
73    }
74
75    /// Computes the fraction of residual ratios that lie within ±threshold.
76    pub fn residual_ratio_within_threshold(&self, threshold: f64) -> Result<f64, ODError> {
77        let mut total = 0;
78        let mut count_within = 0;
79        for residual in self.residuals.iter().flatten() {
80            total += 1;
81            if residual.ratio.abs() <= threshold {
82                count_within += 1;
83            }
84        }
85        if total > 0 {
86            Ok(count_within as f64 / total as f64)
87        } else {
88            Err(ODError::ODNoResiduals {
89                action: "percentage of residuals within threshold",
90            })
91        }
92    }
93
94    /// Computes the Kolmogorov–Smirnov statistic for the aggregated residual ratios of the accepted residuals.
95    ///
96    /// Returns Ok(ks_statistic) if residuals are available.
97    pub fn ks_test_normality(&self) -> Result<f64, ODError> {
98        let mut residual_ratios = self
99            .accepted_residuals()
100            .iter()
101            .flat_map(|resid| resid.whitened_resid.into_iter().copied())
102            .collect::<Vec<f64>>();
103
104        if residual_ratios.is_empty() {
105            return Err(ODError::ODNoResiduals {
106                action: "percentage of residuals within threshold",
107            });
108        }
109        residual_ratios.sort_by(|a, b| a.partial_cmp(b).unwrap());
110        let n = residual_ratios.len() as f64;
111        let mean = residual_ratios.iter().sum::<f64>() / n;
112        let variance = residual_ratios
113            .iter()
114            .map(|v| (v - mean).powi(2))
115            .sum::<f64>()
116            / n;
117        let std_dev = variance.sqrt();
118
119        // Create a normal distribution based on the empirical mean and std deviation.
120        let normal_dist = Normal::new(mean, std_dev).unwrap();
121
122        // Compute the maximum difference between the empirical CDF and the normal CDF.
123        let mut d_stat = 0.0;
124        for (i, &value) in residual_ratios.iter().enumerate() {
125            let empirical_cdf = (i + 1) as f64 / n;
126            let model_cdf = normal_dist.cdf(value);
127            let diff = (empirical_cdf - model_cdf).abs();
128            if diff > d_stat {
129                d_stat = diff;
130            }
131        }
132        Ok(d_stat)
133    }
134
135    /// Checks whether the whitened residuals of the accepted residuals pass a normality test at a given significance level `alpha`, default to 0.05.
136    ///
137    /// This uses a simplified KS-test threshold: D_alpha = c(α) / √n.
138    /// For example, for α = 0.05, c(α) is approximately 1.36.
139    /// α=0.05 means a 5% probability of Type I error (incorrectly rejecting the null hypothesis when it is true).
140    /// This threshold is standard in many statistical tests to balance sensitivity and false positives
141    ///
142    /// The computation of the c(alpha) is from https://en.wikipedia.org/w/index.php?title=Kolmogorov%E2%80%93Smirnov_test&oldid=1280260701#Two-sample_Kolmogorov%E2%80%93Smirnov_test
143    ///
144    /// Returns Ok(true) if the residuals are consistent with a normal distribution,
145    /// Ok(false) if not, or None if no residuals are available.
146    pub fn is_normal(&self, alpha: Option<f64>) -> Result<bool, ODError> {
147        let n = self.residuals.iter().flatten().count();
148        if n == 0 {
149            return Err(ODError::ODNoResiduals {
150                action: "evaluating residual normality",
151            });
152        } else if n < 35 {
153            warn!("KS normality test unreliable for n={n} < 35; skipping");
154        }
155        let ks_stat = self.ks_test_normality()?;
156
157        // Default to 5% probability
158        let alpha = alpha.unwrap_or(0.05);
159
160        // Compute the c_alpha
161        let c_alpha = (-(alpha * 0.5).ln() * 0.5).sqrt();
162
163        let d_critical = c_alpha / (n as f64).sqrt();
164
165        Ok(ks_stat <= d_critical)
166    }
167
168    /// Checks whether the filter innovations are statistically consistent
169    /// by performing a Chi-squared test on the Normalized Innovation Squared (NIS).
170    ///
171    /// For each accepted residual, NIS is computed as:
172    /// ```text
173    ///     prefit^T * S_k^-1 * prefit
174    /// ```
175    ///
176    /// The sum of NIS values should fall within the confidence interval of a
177    /// Chi-squared distribution with degrees of freedom `k = n * m`, where `n`
178    /// is the number of residuals and `m` is the measurement dimension.
179    ///
180    /// Returns Ok(true) if the filter is consistent, Ok(false) if the filter
181    /// is over-confident or under-confident, or an error if no residuals are available.
182    pub fn is_nis_consistent(&self, alpha: Option<f64>) -> Result<bool, ODError> {
183        let n = self.accepted_residuals().len();
184
185        if n == 0 {
186            return Err(ODError::ODNoResiduals {
187                action: "evaluating NIS consistency",
188            });
189        }
190
191        // Sum the NIS across all residuals.
192        // Assuming your Residual struct has an `nis` field or a method to compute it
193        // from the ratio: `ratio.powi(2) * measurement_dim`
194        let nis_sum: f64 = self.accepted_residuals().iter().map(|r| r.nis()).sum();
195
196        // Total degrees of freedom: number of residuals * measurement dimension.
197        // Adjust `M::DIM` based on how you access the dimension in this scope.
198        let k = (n * MsrSize::DIM) as f64;
199        if k < 35.0 {
200            warn!("NIS consistency test lacks statistical power for n*MsrSize={k} < 35");
201        }
202        // Default to a 5% probability of Type I error (95% confidence interval)
203        let alpha = alpha.unwrap_or(0.05);
204
205        // For a two-sided test, we need the standard normal quantile for 1 - (alpha / 2).
206        // If alpha = 0.05, the critical z-score is approximately 1.95996.
207        let z_critical = Normal::new(0.0, 1.0)
208            .unwrap()
209            .inverse_cdf(1.0 - alpha / 2.0);
210
211        // Use the Wilson-Hilferty transformation to approximate the Chi-squared
212        // lower and upper critical bounds.
213        let factor = 2.0 / (9.0 * k);
214        let lower_bound = k * (1.0 - factor - z_critical * factor.sqrt()).powi(3);
215        let upper_bound = k * (1.0 - factor + z_critical * factor.sqrt()).powi(3);
216
217        if nis_sum > upper_bound {
218            warn!(
219                "NIS consistency test failed high: NIS sum {nis_sum:.6} > upper bound {upper_bound:.6}. Innovations are larger than expected."
220            );
221            warn!(
222                "Filter may be overconfident: P, R, or Q may be too small, or the dynamics/measurement model may be biased."
223            );
224            Ok(false)
225        } else if nis_sum < lower_bound {
226            warn!(
227                "NIS consistency test failed low: NIS sum {nis_sum:.6} < lower bound {lower_bound:.6}. Innovations are smaller than expected."
228            );
229            warn!("Filter may be underconfident: P, R, or Q may be too large.");
230            Ok(false)
231        } else {
232            Ok(true)
233        }
234    }
235
236    /// Checks whether the filter estimates are statistically consistent
237    /// by performing a Chi-squared test on the Normalized Estimation Error Squared (NEES).
238    ///
239    /// For each estimate, NEES is computed as:
240    /// ```text
241    ///     error^T * P^-1 * error
242    /// ```
243    /// where `error` is the difference between the estimated state and the true state,
244    /// and `P` is the estimated state covariance matrix.
245    ///
246    /// The sum of NEES values should fall within the confidence interval of a
247    /// Chi-squared distribution with degrees of freedom `k = n * dim`, where `n`
248    /// is the number of estimates and `dim` is the state dimension.
249    ///
250    /// Returns Ok(true) if the filter is consistent, Ok(false) if the filter
251    /// is over-confident or under-confident, or an error if no estimates are available.
252    pub fn is_nees_consistent(
253        &self,
254        truth_traj: &Traj<StateType>,
255        alpha: Option<f64>,
256    ) -> Result<bool, ODError>
257    where
258        StateType::Size: DimMin<StateType::Size>,
259        <StateType::Size as DimMin<StateType::Size>>::Output: DimSub<Const<1>>,
260        <<StateType as State>::Size as DimMin<<StateType as State>::Size>>::Output:
261            DimSub<Const<1>>,
262        DefaultAllocator: Allocator<StateType::Size, Const<1>>
263            + Allocator<Const<1>, <StateType as State>::Size>
264            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output, StateType::Size>
265            + Allocator<StateType::Size, <StateType::Size as DimMin<StateType::Size>>::Output>
266            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output>
267            + Allocator<
268                <<StateType::Size as DimMin<StateType::Size>>::Output as DimSub<Const<1>>>::Output,
269            >,
270    {
271        let n = self.estimates.len();
272
273        if n == 0 {
274            return Err(ODError::ODNoResiduals {
275                action: "evaluating NEES consistency",
276            });
277        }
278
279        let mut nees_sum = 0.0;
280        let mut state_dim_f64 = 0.0_f64;
281
282        for est in &self.estimates {
283            let epoch = est.epoch();
284
285            let truth_state = truth_traj.at(epoch).context(ODTrajSnafu {
286                details: "interpolating truth during NEES test",
287            })?;
288
289            // Extract the state vectors
290            let x_est = est.state().to_state_vector();
291            let x_true = truth_state.to_state_vector();
292            let error = x_est - x_true;
293
294            // Extract the covariance and compute its inverse
295            // Extract the covariance
296            let p_mat = est.covar();
297
298            // Compute the Singular Value Decomposition (SVD) instead of ivnerting as-is.
299            // This enables support for non-estimated parameters.
300            let svd = p_mat.svd(true, true);
301
302            // Define a small tolerance for zero-variance detection
303            let epsilon = 1e-12;
304
305            // Compute the Moore-Penrose pseudo-inverse
306            let p_inv = svd
307                .clone()
308                .pseudo_inverse(epsilon)
309                .map_err(|_| ODError::SingularInformationMatrix)?;
310
311            // Dynamically determine the rank (number of actively estimated parameters)
312            // nalgebra's SVD provides a built-in rank evaluation
313            let active_dim = svd.rank(epsilon) as f64;
314
315            // Update the computed degree of freedom, it's fixed but the SVD might cause weird rounding
316            state_dim_f64 = state_dim_f64.max(active_dim);
317
318            // Compute NEES for this epoch: error^T * P^+ * error
319            let nees_matrix = error.transpose() * p_inv * &error;
320            nees_sum += nees_matrix[(0, 0)];
321
322            // Capture the state dimension for the degrees of freedom calculation
323            if state_dim_f64 == 0.0 {
324                state_dim_f64 = error.nrows() as f64;
325            }
326        }
327
328        // Total degrees of freedom: number of estimates * state dimension
329        let k = (n as f64) * state_dim_f64;
330
331        if k < 35.0 {
332            warn!("NEES consistency test lacks statistical power for n*StateDim={k} < 35");
333        }
334
335        // Default to a 5% probability of Type I error (95% confidence interval)
336        let alpha = alpha.unwrap_or(0.05);
337
338        // For a two-sided test, we need the standard normal quantile for 1 - (alpha / 2).
339        let z_critical = Normal::new(0.0, 1.0)
340            .unwrap()
341            .inverse_cdf(1.0 - alpha / 2.0);
342
343        // Use the Wilson-Hilferty transformation to approximate the Chi-squared
344        // lower and upper critical bounds.
345        let factor = 2.0 / (9.0 * k);
346        let lower_bound = k * (1.0 - factor - z_critical * factor.sqrt()).powi(3);
347        let upper_bound = k * (1.0 - factor + z_critical * factor.sqrt()).powi(3);
348
349        if nees_sum > upper_bound {
350            warn!(
351                "NEES consistency test failed high: NEES sum {nees_sum:.6} > upper bound {upper_bound:.6}. Estimation errors are larger than the covariance suggests."
352            );
353            warn!(
354                "Filter is overconfident: P is too small. Process noise Q may be too low, or there are unmodeled dynamic biases."
355            );
356            Ok(false)
357        } else if nees_sum < lower_bound {
358            warn!(
359                "NEES consistency test failed low: NEES sum {nees_sum:.6} < lower bound {lower_bound:.6}. Estimation errors are smaller than expected."
360            );
361            warn!(
362                "Filter is underconfident: P is too large. Process noise Q may be too high, or measurement noise R is overestimated."
363            );
364            Ok(false)
365        } else {
366            Ok(true)
367        }
368    }
369}
nyx_space/od/process/solution/stats.rs

nyx_space/od/process/solution/
stats.rs
