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::{Const, DMatrix, DefaultAllocator, DimMin, DimName, DimSub, OVector};
21use crate::md::trajectory::{Interpolatable, Traj};
22pub use crate::od::estimate::*;
23pub use crate::od::*;
24use log::{info, warn};
25use msr::sensitivity::TrackerSensitivity;
26use snafu::ResultExt;
27use statrs::distribution::{ContinuousCDF, Normal};
28use std::fmt;
29use std::ops::Add;
30
31use super::ODSolution;
32
33#[cfg(feature = "python")]
34use pyo3::prelude::*;
35
36#[cfg_attr(feature = "python", pyclass(from_py_object, get_all))]
37#[derive(Copy, Clone, Debug)]
38pub struct NormalizedConsistency {
39    pub normalized_sum: f64,
40    pub k: f64,
41    pub lower_bound: f64,
42    pub upper_bound: f64,
43    pub is_nees: bool,
44}
45
46#[cfg_attr(feature = "python", pymethods)]
47impl NormalizedConsistency {
48    pub fn name(&self) -> &'static str {
49        if self.is_nees { "NEES" } else { "NIS" }
50    }
51
52    /// Returns true if there are more than 35 degrees of freedom
53    pub fn has_statistical_power(&self) -> bool {
54        self.k > 35.0
55    }
56
57    /// Returns true if the sum metric is within the lower and upper bounds
58    pub fn is_consistent(&self) -> bool {
59        self.normalized_sum < self.upper_bound && self.normalized_sum > self.lower_bound
60    }
61
62    /// Returns true if the normalized sum is less than the lower bound
63    pub fn is_underconfident(&self) -> bool {
64        self.normalized_sum < self.lower_bound
65    }
66
67    /// Returns true if the normalized sum is greater than the upper bound
68    pub fn is_overconfident(&self) -> bool {
69        self.normalized_sum > self.upper_bound
70    }
71
72    /// Log the status to the logger
73    pub fn log(&self) {
74        let name = self.name();
75        let sum = self.normalized_sum;
76        let upper_bound = self.upper_bound;
77        let lower_bound = self.lower_bound;
78        if sum > upper_bound {
79            warn!(
80                "{name} consistency test failed high: {name} sum {sum:.3} > upper bound {upper_bound:.3}."
81            );
82            if self.is_nees {
83                warn!("Estimation errors are larger than the covariance suggests.");
84            } else {
85                warn!("Innovations are larger than expected");
86            }
87            warn!(
88                "Filter is overconfident: P, R, or Q may be too small, or the dynamics/measurement model may be biased."
89            );
90        } else if sum < lower_bound {
91            warn!(
92                "{name} consistency test failed low: {name} sum {sum:.3} < lower bound {lower_bound:.3}."
93            );
94            if self.is_nees {
95                warn!("Estimation errors are smaller than expected.");
96            } else {
97                warn!("Innovations are smaller than expected.")
98            }
99            warn!("Filter is underconfident: P, R, or Q may be too large.");
100        } else {
101            info!("{name} passed")
102        }
103    }
104
105    #[cfg(feature = "python")]
106    fn __str__(&self) -> String {
107        format!("{self}")
108    }
109
110    #[cfg(feature = "python")]
111    fn __repr__(&self) -> String {
112        format!("{self} @ {self:p}")
113    }
114}
115
116impl fmt::Display for NormalizedConsistency {
117    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
118        let is_ok = self.is_consistent();
119        write!(
120            f,
121            "{} consistency {} (k={}; bounds: {:.3} < {:.3} < {:.3})",
122            self.name(),
123            if is_ok { "PASSED" } else { "FAILED" },
124            self.k,
125            self.lower_bound,
126            self.normalized_sum,
127            self.upper_bound
128        )
129    }
130}
131
132impl<StateType, EstType, MsrSize, Trk> ODSolution<StateType, EstType, MsrSize, Trk>
133where
134    StateType: Interpolatable + Add<OVector<f64, <StateType as State>::Size>, Output = StateType>,
135    EstType: Estimate<StateType>,
136    MsrSize: DimName,
137    Trk: TrackerSensitivity<StateType, StateType>,
138    <DefaultAllocator as Allocator<<StateType as State>::VecLength>>::Buffer<f64>: Send,
139    DefaultAllocator: Allocator<<StateType as State>::Size>
140        + Allocator<<StateType as State>::VecLength>
141        + Allocator<MsrSize>
142        + Allocator<MsrSize, <StateType as State>::Size>
143        + Allocator<MsrSize, MsrSize>
144        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
145        + Allocator<<StateType as State>::Size, MsrSize>,
146{
147    /// Returns the root mean square of the prefit residuals
148    pub fn rms_prefit_residuals(&self) -> f64 {
149        let mut sum = 0.0;
150        for residual in self.residuals.iter().flatten() {
151            sum += residual.prefit.dot(&residual.prefit);
152        }
153        (sum / (self.residuals.len() as f64)).sqrt()
154    }
155
156    /// Returns the root mean square of the postfit residuals
157    pub fn rms_postfit_residuals(&self) -> f64 {
158        let mut sum = 0.0;
159        for residual in self.residuals.iter().flatten() {
160            sum += residual.postfit.dot(&residual.postfit);
161        }
162        (sum / (self.residuals.len() as f64)).sqrt()
163    }
164
165    /// Returns the root mean square of the prefit residual ratios
166    pub fn rms_residual_ratios(&self) -> f64 {
167        let mut sum = 0.0;
168        for residual in self.residuals.iter().flatten() {
169            sum += residual.ratio.powi(2);
170        }
171        (sum / (self.residuals.len() as f64)).sqrt()
172    }
173
174    /// Computes the fraction of residual ratios that lie within ±threshold.
175    pub fn residual_ratio_within_threshold(&self, threshold: f64) -> Result<f64, ODError> {
176        let mut total = 0;
177        let mut count_within = 0;
178        for residual in self.residuals.iter().flatten() {
179            total += 1;
180            if residual.ratio.abs() <= threshold {
181                count_within += 1;
182            }
183        }
184        if total > 0 {
185            Ok(count_within as f64 / total as f64)
186        } else {
187            Err(ODError::ODNoResiduals {
188                action: "percentage of residuals within threshold",
189            })
190        }
191    }
192
193    /// Computes the Kolmogorov–Smirnov statistic for the aggregated residual ratios of the accepted residuals.
194    ///
195    /// Returns Ok(ks_statistic) if residuals are available.
196    pub fn ks_test_normality(&self) -> Result<f64, ODError> {
197        let mut residual_ratios = self
198            .accepted_residuals()
199            .iter()
200            .flat_map(|resid| resid.whitened_resid.into_iter().copied())
201            .collect::<Vec<f64>>();
202
203        if residual_ratios.is_empty() {
204            return Err(ODError::ODNoResiduals {
205                action: "percentage of residuals within threshold",
206            });
207        }
208        residual_ratios.sort_by(|a, b| a.partial_cmp(b).unwrap());
209        let n = residual_ratios.len() as f64;
210        let mean = residual_ratios.iter().sum::<f64>() / n;
211        let variance = residual_ratios
212            .iter()
213            .map(|v| (v - mean).powi(2))
214            .sum::<f64>()
215            / n;
216        let std_dev = variance.sqrt();
217
218        // Create a normal distribution based on the empirical mean and std deviation.
219        let normal_dist = Normal::new(mean, std_dev).unwrap();
220
221        // Compute the maximum difference between the empirical CDF and the normal CDF.
222        let mut d_stat = 0.0;
223        for (i, &value) in residual_ratios.iter().enumerate() {
224            let empirical_cdf = (i + 1) as f64 / n;
225            let model_cdf = normal_dist.cdf(value);
226            let diff = (empirical_cdf - model_cdf).abs();
227            if diff > d_stat {
228                d_stat = diff;
229            }
230        }
231        Ok(d_stat)
232    }
233
234    /// Checks whether the whitened residuals of the accepted residuals pass a normality test at a given significance level `alpha`, default to 0.05.
235    ///
236    /// This uses a simplified KS-test threshold: D_alpha = c(α) / √n.
237    /// For example, for α = 0.05, c(α) is approximately 1.36.
238    /// α=0.05 means a 5% probability of Type I error (incorrectly rejecting the null hypothesis when it is true).
239    /// This threshold is standard in many statistical tests to balance sensitivity and false positives
240    ///
241    /// 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
242    ///
243    /// Returns Ok(true) if the residuals are consistent with a normal distribution,
244    /// Ok(false) if not, or None if no residuals are available.
245    pub fn is_normal(&self, alpha: Option<f64>) -> Result<bool, ODError> {
246        let n = self.accepted_residuals().len();
247        if n == 0 {
248            return Err(ODError::ODNoResiduals {
249                action: "evaluating residual normality",
250            });
251        } else if n < 35 {
252            warn!("KS normality test unreliable for n={n} < 35; skipping");
253        }
254
255        let ks_stat = self.ks_test_normality()?;
256
257        // Default to 5% probability
258        let alpha = alpha.unwrap_or(0.05);
259
260        // Compute the c_alpha
261        let c_alpha = (-(alpha * 0.5).ln() * 0.5).sqrt();
262
263        let d_critical = c_alpha / (n as f64).sqrt();
264
265        Ok(ks_stat <= d_critical)
266    }
267
268    /// Checks whether the filter innovations are statistically consistent
269    /// by performing a Chi-squared test on the Normalized Innovation Squared (NIS).
270    ///
271    /// For each accepted residual, NIS is computed as:
272    /// ```text
273    ///     prefit^T * S_k^-1 * prefit
274    /// ```
275    ///
276    /// The sum of NIS values should fall within the confidence interval of a
277    /// Chi-squared distribution with degrees of freedom `k = n * m`, where `n`
278    /// is the number of residuals and `m` is the measurement dimension.
279    ///
280    /// Returns Ok(true) if the filter is consistent, Ok(false) if the filter
281    /// is over-confident or under-confident, or an error if no residuals are available.
282    pub fn nis_consistency(&self, alpha: Option<f64>) -> Result<NormalizedConsistency, ODError> {
283        let n = self.accepted_residuals().len();
284
285        if n == 0 {
286            return Err(ODError::ODNoResiduals {
287                action: "evaluating NIS consistency",
288            });
289        }
290
291        // Sum the NIS across all residuals.
292        // Assuming your Residual struct has an `nis` field or a method to compute it
293        // from the ratio: `ratio.powi(2) * measurement_dim`
294        let nis_sum: f64 = self.accepted_residuals().iter().map(|r| r.nis()).sum();
295
296        // Total degrees of freedom: number of residuals * measurement dimension.
297        // Adjust `M::DIM` based on how you access the dimension in this scope.
298        let k = (n * MsrSize::DIM) as f64;
299        if k < 35.0 {
300            warn!("NIS consistency test lacks statistical power for n*MsrSize={k} < 35");
301        }
302        // Default to a 5% probability of Type I error (95% confidence interval)
303        let alpha = alpha.unwrap_or(0.05);
304
305        // For a two-sided test, we need the standard normal quantile for 1 - (alpha / 2).
306        // If alpha = 0.05, the critical z-score is approximately 1.95996.
307        let z_critical = Normal::new(0.0, 1.0)
308            .unwrap()
309            .inverse_cdf(1.0 - alpha / 2.0);
310
311        // Use the Wilson-Hilferty transformation to approximate the Chi-squared
312        // lower and upper critical bounds.
313        let factor = 2.0 / (9.0 * k);
314        let lower_bound = k * (1.0 - factor - z_critical * factor.sqrt()).powi(3);
315        let upper_bound = k * (1.0 - factor + z_critical * factor.sqrt()).powi(3);
316
317        Ok(NormalizedConsistency {
318            normalized_sum: nis_sum,
319            lower_bound,
320            upper_bound,
321            k,
322            is_nees: false,
323        })
324    }
325
326    /// Checks whether the filter innovations are statistically consistent
327    /// by performing a Chi-squared test on the Normalized Innovation Squared (NIS).
328    ///
329    /// For each accepted residual, NIS is computed as:
330    /// ```text
331    ///     prefit^T * S_k^-1 * prefit
332    /// ```
333    ///
334    /// The sum of NIS values should fall within the confidence interval of a
335    /// Chi-squared distribution with degrees of freedom `k = n * m`, where `n`
336    /// is the number of residuals and `m` is the measurement dimension.
337    ///
338    /// Returns Ok(true) if the filter is consistent, Ok(false) if the filter
339    /// is over-confident or under-confident, or an error if no residuals are available.
340    #[deprecated(since = "2.4.2", note = "use `nis_consistency` instead")]
341    pub fn is_nis_consistent(&self, alpha: Option<f64>) -> Result<bool, ODError> {
342        self.nis_consistency(alpha).map(|ok| ok.is_consistent())
343    }
344
345    /// Checks whether the filter estimates are statistically consistent
346    /// by performing a Chi-squared test on the Normalized Estimation Error Squared (NEES).
347    ///
348    /// For each estimate, NEES is computed as:
349    /// ```text
350    ///     error^T * P^-1 * error
351    /// ```
352    /// where `error` is the difference between the estimated state and the true state,
353    /// and `P` is the estimated state covariance matrix.
354    ///
355    /// The sum of NEES values should fall within the confidence interval of a
356    /// Chi-squared distribution with degrees of freedom `k = n * dim`, where `n`
357    /// is the number of estimates and `dim` is the actively estimated state dimension.
358    pub fn nees_consistency(
359        &self,
360        truth_traj: &Traj<StateType>,
361        alpha: Option<f64>,
362    ) -> Result<NormalizedConsistency, ODError>
363    where
364        StateType::Size: DimMin<StateType::Size>,
365        <StateType::Size as DimMin<StateType::Size>>::Output: DimSub<Const<1>>,
366        <<StateType as State>::Size as DimMin<<StateType as State>::Size>>::Output:
367            DimSub<Const<1>>,
368        DefaultAllocator: Allocator<StateType::Size, Const<1>>
369            + Allocator<Const<1>, <StateType as State>::Size>
370            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output, StateType::Size>
371            + Allocator<StateType::Size, <StateType::Size as DimMin<StateType::Size>>::Output>
372            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output>
373            + Allocator<
374                <<StateType::Size as DimMin<StateType::Size>>::Output as DimSub<Const<1>>>::Output,
375            >,
376    {
377        let n_total = self.estimates.len();
378
379        // We will skip the apriori estimate.
380        if n_total <= 1 {
381            return Err(ODError::ODNoResiduals {
382                action: "evaluating NEES consistency",
383            });
384        }
385
386        // The exact number of epochs evaluated in the NEES sum
387        let n = n_total - 1;
388
389        let mut nees_sum = 0.0;
390        let mut est_size = None;
391
392        for est in self.estimates.iter().skip(1) {
393            let epoch = est.epoch();
394
395            let truth_state = truth_traj.at(epoch).context(ODTrajSnafu {
396                details: "interpolating truth during NEES test",
397            })?;
398
399            // Extract the state vectors
400            let x_est = est.state().to_state_vector();
401            let x_true = truth_state.to_state_vector();
402            let error = x_est - x_true;
403            let cov = est.covar();
404            let cov = 0.5 * (&cov + cov.transpose());
405
406            if est_size.is_none() {
407                // Deduce est_size by physically inspecting the diagonal variances.
408                // Unestimated deterministic parameters will have zero variance.
409                let mut active_count = 0;
410                for i in 0..StateType::Size::dim() {
411                    // Check if variance is strictly positive
412                    if cov[(i, i)] > 0.0 {
413                        active_count += 1;
414                    } else {
415                        // Estimated parameters are contiguous from index 0
416                        break;
417                    }
418                }
419                if active_count < 6 {
420                    warn!("Detected est_size of {active_count} < 6; forcing bound to 6");
421                    active_count = 6;
422                }
423                est_size = Some(active_count);
424            }
425
426            // At this point, est_size is set and is valid
427            let true_est_size = est_size.unwrap();
428
429            // Dynamically slice the actively estimated block
430            let cov_e = cov
431                .view((0, 0), (true_est_size, true_est_size))
432                .into_owned();
433            let err_e = error.rows(0, true_est_size).into_owned();
434
435            // Execute Symmetric Eigendecomposition.
436            // This is mathematically safe because cov was forced to be symmetric via 0.5 * (P + P^T)
437            let eigen = cov_e.symmetric_eigen();
438
439            // Define the numerical noise floor relative to the largest valid physical variance
440            let max_lambda = eigen.eigenvalues.max();
441            let epsilon = max_lambda * (true_est_size as f64) * f64::EPSILON;
442
443            // Construct the inverted eigenvalue spectrum
444            let mut inv_eigenvalues = eigen.eigenvalues.clone();
445            for i in 0..true_est_size {
446                // By strictly checking > epsilon, we explicitly trap BOTH:
447                // 1. Positive numerical noise (e.g., +1e-19)
448                // 2. Severe negative numerical drift (e.g., -1e-5)
449                if inv_eigenvalues[i] > epsilon {
450                    inv_eigenvalues[i] = 1.0 / inv_eigenvalues[i];
451                } else {
452                    // Clamp corrupted non-PSD subspaces exactly to zero
453                    inv_eigenvalues[i] = 0.0;
454                }
455            }
456
457            // Reconstruct the symmetric positive semi-definite pseudo-inverse: P^+ = Q * Lambda^+ * Q^T
458            let p_inv = &eigen.eigenvectors
459                * DMatrix::from_diagonal(&inv_eigenvalues)
460                * eigen.eigenvectors.transpose();
461
462            // Compute quadratic form: e^T * P^+ * e
463            let nees_matrix = err_e.transpose() * p_inv * err_e;
464            nees_sum += nees_matrix[(0, 0)];
465        }
466
467        // Total degrees of freedom: N evaluated epochs * actively estimated dimension
468        let k = (n * est_size.unwrap()) as f64;
469
470        if k < 35.0 {
471            warn!("NEES consistency test lacks statistical power for n*StateDim={k} < 35");
472        }
473
474        // Default to a 5% probability of Type I error (95% confidence interval)
475        let alpha = alpha.unwrap_or(0.05);
476
477        // For a two-sided test, we need the standard normal quantile for 1 - (alpha / 2).
478        let z_critical = Normal::new(0.0, 1.0)
479            .unwrap()
480            .inverse_cdf(1.0 - alpha / 2.0);
481
482        // Use the Wilson-Hilferty transformation to approximate the Chi-squared lower and upper critical bounds.
483        let factor = 2.0 / (9.0 * k);
484        let lower_bound = k * (1.0 - factor - z_critical * factor.sqrt()).powi(3);
485        let upper_bound = k * (1.0 - factor + z_critical * factor.sqrt()).powi(3);
486
487        Ok(NormalizedConsistency {
488            normalized_sum: nees_sum,
489            lower_bound,
490            upper_bound,
491            k,
492            is_nees: true,
493        })
494    }
495
496    /// Checks whether the filter estimates are statistically consistent
497    /// by performing a Chi-squared test on the Normalized Estimation Error Squared (NEES).
498    ///
499    /// For each estimate, NEES is computed as:
500    /// ```text
501    ///     error^T * P^-1 * error
502    /// ```
503    /// where `error` is the difference between the estimated state and the true state,
504    /// and `P` is the estimated state covariance matrix.
505    ///
506    /// The sum of NEES values should fall within the confidence interval of a
507    /// Chi-squared distribution with degrees of freedom `k = n * dim`, where `n`
508    /// is the number of estimates and `dim` is the actively estimated state dimension.
509    #[deprecated(since = "2.4.2", note = "use `nees_consistency` instead")]
510    pub fn is_nees_consistent(
511        &self,
512        truth_traj: &Traj<StateType>,
513        alpha: Option<f64>,
514    ) -> Result<bool, ODError>
515    where
516        StateType::Size: DimMin<StateType::Size>,
517        <StateType::Size as DimMin<StateType::Size>>::Output: DimSub<Const<1>>,
518        <<StateType as State>::Size as DimMin<<StateType as State>::Size>>::Output:
519            DimSub<Const<1>>,
520        DefaultAllocator: Allocator<StateType::Size, Const<1>>
521            + Allocator<Const<1>, <StateType as State>::Size>
522            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output, StateType::Size>
523            + Allocator<StateType::Size, <StateType::Size as DimMin<StateType::Size>>::Output>
524            + Allocator<<StateType::Size as DimMin<StateType::Size>>::Output>
525            + Allocator<
526                <<StateType::Size as DimMin<StateType::Size>>::Output as DimSub<Const<1>>>::Output,
527            >,
528    {
529        self.nees_consistency(truth_traj, alpha)
530            .map(|ok| ok.is_consistent())
531    }
532}
nyx_space/od/process/solution/stats.rs

nyx_space/od/process/solution/
stats.rs
