#![allow(clippy::many_single_char_names, clippy::cast_precision_loss)]
#[must_use]
pub fn normalized_squared<const M: usize>(sqrt: &[[f64; M]; M], residual: &[f64; M]) -> f64 {
let mut acc = 0.0;
let mut y = [0.0; M];
for i in 0..M {
let mut s = residual[i];
for k in 0..i {
s -= sqrt[i][k] * y[k];
}
y[i] = s / sqrt[i][i];
acc += y[i] * y[i];
}
acc
}
#[must_use]
pub fn scalar_nis(innovation: f64, innovation_variance: f64) -> f64 {
innovation * innovation / innovation_variance
}
#[must_use]
pub fn chi_square_scale_factor(mean_normalized: f64, dof: usize) -> f64 {
mean_normalized / dof as f64
}
#[must_use]
pub fn chi_square_mean_interval(dof: usize, runs: usize, z: f64) -> (f64, f64) {
let k = (dof * runs) as f64;
let a = 2.0 / (9.0 * k);
let wilson_hilferty = |zp: f64| {
let t = 1.0 - a + zp * libm::sqrt(a);
k * t * t * t
};
let n = runs as f64;
(wilson_hilferty(-z) / n, wilson_hilferty(z) / n)
}
#[must_use]
pub fn is_consistent(mean_normalized: f64, dof: usize, runs: usize, z: f64) -> bool {
let (lo, hi) = chi_square_mean_interval(dof, runs, z);
(lo..=hi).contains(&mean_normalized)
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used)]
use super::*;
use crate::SquareRootKalman;
#[test]
fn normalized_squared_reduces_to_the_squared_norm_for_identity_covariance() {
let id = [[1.0, 0.0], [0.0, 1.0]];
let r = [3.0, 4.0];
assert!((normalized_squared(&id, &r) - 25.0).abs() < 1e-12);
}
#[test]
fn normalized_squared_matches_the_explicit_quadratic_form() {
let l = [[2.0, 0.0], [1.0, 3.0]];
let r = [1.0, 1.0];
let expected = 10.0 / 36.0;
assert!((normalized_squared(&l, &r) - expected).abs() < 1e-12);
}
#[test]
fn nees_from_a_filters_covariance_root_is_finite_and_correct() {
let filter =
SquareRootKalman::from_covariance([0.0, 0.0], &[[4.0, 0.0], [0.0, 9.0]]).unwrap();
let error = [2.0, 3.0]; let nees = normalized_squared(&filter.covariance_sqrt(), &error);
assert!((nees - 2.0).abs() < 1e-12, "nees {nees}");
}
#[test]
fn scalar_nis_is_the_squared_sigma_ratio() {
assert!((scalar_nis(2.0, 1.0) - 4.0).abs() < 1e-12);
}
#[test]
fn scalar_nis_separates_the_square_from_a_sum_and_the_division_from_a_product() {
assert!((scalar_nis(3.0, 4.0) - 2.25).abs() < 1e-12);
}
#[test]
fn scale_factor_flags_over_and_under_confidence() {
assert!((chi_square_scale_factor(4.0, 2) - 2.0).abs() < 1e-12); assert!((chi_square_scale_factor(1.0, 2) - 0.5).abs() < 1e-12); }
#[test]
fn mean_interval_matches_known_chi_square_quantiles() {
let (lo, hi) = chi_square_mean_interval(2, 50, 1.96);
assert!((lo - 1.4844).abs() < 1e-2, "lo {lo}");
assert!((hi - 2.5912).abs() < 1e-2, "hi {hi}");
assert!(lo < 2.0 && 2.0 < hi);
}
#[test]
fn is_consistent_accepts_the_expected_value_and_rejects_a_gross_deviation() {
assert!(is_consistent(2.0, 2, 50, 1.96)); assert!(!is_consistent(5.0, 2, 50, 1.96)); }
}