diff --git a/README.md b/README.md index f3457558..212c0dd3 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians | | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler, ABA | -| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Yule-Walker (offline), Recursive Least Squares, LMS / NLMS Adaptive Filter (online) | +| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Yule-Walker (offline), Recursive Least Squares, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade | | [Kinematics](doc/kinematics/README.md) | Forward Kinematics | | [Neural Network](doc/neural_network/README.md) | Layers, activations, losses, model | diff --git a/ROADMAP.md b/ROADMAP.md index 8b5d30bb..e6452043 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -48,7 +48,6 @@ Difficulty legend: | 45 | IIR filter design (Butterworth/Chebyshev + bilinear) | `filters/passive` | ★★★★★ | | 46 | H∞ state-feedback control | `robust_control` (new) | ★★★★★ | | 47 | Model Reference Adaptive Control (MRAC) | `nonlinear_control` (new) | ★★★★★ | -| 52 | Estimator consistency metrics (NEES / NIS) | `estimators` | ★★★☆☆ | Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family unit-test reference [`TESTING.md`](TESTING.md) depends on but which @@ -566,7 +565,7 @@ on bounded `math::Vector`/`math::Matrix` inputs; tests are `TEST_F` on `float`. the existing `solvers::DurandKerner` for the full spectrum. - **Reuses:** `math::Matrix`, `solvers::DurandKerner`. -### 52. Estimator consistency metrics (NEES / NIS) ★★★☆☆ — `estimators` +### ~~52. Estimator consistency metrics (NEES / NIS) ★★★☆☆ — `estimators`~~ ✓ Done - **What:** Normalised Estimation Error Squared and Normalised Innovation Squared, with χ² confidence-gate helpers. - **Metric value:** M8 (statistical consistency) — the only rigorous correctness test for the Kalman diff --git a/doc/estimators/ConsistencyMetrics.md b/doc/estimators/ConsistencyMetrics.md new file mode 100644 index 00000000..5c68c5d3 --- /dev/null +++ b/doc/estimators/ConsistencyMetrics.md @@ -0,0 +1,95 @@ +# Estimator Consistency Metrics (NEES / NIS) + +## Overview & Motivation + +State estimators such as the Kalman filter, Extended Kalman filter, and Unscented Kalman filter produce both a state estimate and a covariance matrix that quantifies estimation uncertainty. A filter is said to be consistent when the true errors are statistically compatible with the reported covariance — that is, the filter neither overestimates nor underestimates its own uncertainty. + +Raw error metrics such as RMSE cannot distinguish a consistent filter (correct covariance) from an inconsistent one (wrong covariance that happens to produce low errors in a specific trial). Consistency metrics address this gap by normalising errors with respect to the predicted covariance and comparing the result against a chi-squared distribution. + +## Mathematical Theory + +### Normalised Estimation Error Squared (NEES) + +Given the state error $\varepsilon_k = x_k - \hat{x}_{k|k}$ and the posterior state covariance $P_{k|k}$, the NEES at time step $k$ is + +$$\varepsilon_k^{\mathsf{T}} P_{k|k}^{-1} \varepsilon_k$$ + +Under the hypothesis that the filter is consistent and the errors are Gaussian, this quantity is chi-squared distributed with $n_x$ degrees of freedom, where $n_x$ is the state dimension. + +### Normalised Innovation Squared (NIS) + +Given the innovation $\nu_k = z_k - \hat{z}_{k|k-1}$ and the innovation covariance $S_k$, the NIS at time step $k$ is + +$$\nu_k^{\mathsf{T}} S_k^{-1} \nu_k$$ + +Under consistency, NIS follows a chi-squared distribution with $n_z$ degrees of freedom, where $n_z$ is the measurement dimension. Unlike NEES, NIS is computable without knowledge of the true state and is therefore usable in real deployments. + +### Chi-Squared Confidence Gate + +A two-sided 95% confidence region for a single NEES or NIS sample is + +$$\chi^2_{n,\,0.025} \;\le\; \varepsilon_k^{\mathsf{T}} P_{k|k}^{-1} \varepsilon_k \;\le\; \chi^2_{n,\,0.975}$$ + +where $\chi^2_{n,p}$ is the $p$-th quantile of the chi-squared distribution with $n$ degrees of freedom. + +### Time-Averaged Gate + +Averaging NEES or NIS over $N$ independent samples gives a statistic that is chi-squared with $N \cdot n$ degrees of freedom, scaled by $1/N$. The 95% bounds for the time-averaged value are therefore + +$$\frac{\chi^2_{Nn,\,0.025}}{N} \;\le\; \bar{\varepsilon} \;\le\; \frac{\chi^2_{Nn,\,0.975}}{N}$$ + +Averaging reduces the variance of the consistency estimate and is the standard approach in Monte-Carlo filter evaluation. + +### Embedded Computation + +Rather than forming $P^{-1}$ explicitly, the quadratic form $\varepsilon^{\mathsf{T}} P^{-1} \varepsilon$ is computed by solving $P z = \varepsilon$ for $z$ (via Gaussian elimination with partial pivoting) and then computing $\varepsilon^{\mathsf{T}} z$. This avoids matrix inversion, reduces floating-point operations, and is numerically more stable. + +Chi-squared quantiles for degrees of freedom 1–10 at the 95% confidence level are stored in a compile-time array. Dimensions outside this range are rejected via `static_assert`. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|---------|----------|----------|--------------------------------------------| +| Best | $O(n^2)$ | $O(n^2)$ | Dominated by back-substitution in GE | +| Average | $O(n^3)$ | $O(n^2)$ | Gaussian elimination with partial pivoting | +| Worst | $O(n^3)$ | $O(n^2)$ | All pivots require row swaps | + +All storage is stack-allocated; $n$ is bounded at compile time by `Dim`. + +## Step-by-Step Walkthrough + +Consider a 2-D state ($n = 2$) with error $\varepsilon = [1, 2]^{\mathsf{T}}$ and covariance $P = \mathrm{diag}(1, 4)$. + +1. Solve $P z = \varepsilon$: $z_1 = 1/1 = 1$, $z_2 = 2/4 = 0.5$. +2. Compute dot product: $\varepsilon^{\mathsf{T}} z = 1 \cdot 1 + 2 \cdot 0.5 = 2.0$. +3. Compare against $\chi^2_{2, 0.025} \approx 0.051$ and $\chi^2_{2, 0.975} \approx 7.38$. +4. Since $2.0 \in [0.051, 7.38]$, the filter is declared consistent at the 95% level. + +## Pitfalls & Edge Cases + +A singular covariance matrix causes the linear solve to fail; the implementation detects near-zero diagonal pivots and returns `std::nullopt`. Callers must check the optional before using the value. + +A single sample NEES or NIS value has high variance under chi-squared; a filter may appear inconsistent simply due to random variation. Time-averaging over many Monte-Carlo runs is the statistically correct procedure. + +NEES requires ground-truth state access and is therefore only applicable in simulation. NIS is the consistency monitor of choice for deployed systems. + +## Variants & Generalizations + +The average NEES over a Monte-Carlo ensemble of $M$ runs and $K$ time steps yields $M \cdot K$ samples; consistency must hold jointly across the ensemble, not just per run. Extensions to non-Gaussian cases (e.g., particle filters) use empirical quantiles rather than chi-squared bounds. + +## Applications + +- Monte-Carlo evaluation of Kalman-family filters during design and tuning. +- Online innovation gating in Kalman filters (rejecting outlier measurements whose NIS exceeds the threshold). +- Diagnosing process-noise or measurement-noise mis-specification. +- Formal consistency testing as part of filter validation before deployment. + +## Connections to Other Algorithms + +NEES and NIS are statistical companions to the standard Kalman filter update step. They depend on the covariance propagation produced by the `filters/active` family (KF, EKF, UKF). The linear solve reuses `solvers::GaussianElimination`, and the state-error representation aligns with `estimators::EstimationMetrics`. + +## References & Further Reading + +- Y. Bar-Shalom, X. R. Li, T. Kirubarajan, *Estimation with Applications to Tracking and Navigation*, Wiley, 2001. Chapter 5 (Estimation Consistency). +- T. D. Barfoot, *State Estimation for Robotics*, Cambridge University Press, 2017. Chapter 9. +- S. Julier, J. Uhlmann, "A New Extension of the Kalman Filter to Nonlinear Systems," *Proc. SPIE*, 1997. diff --git a/doc/estimators/README.md b/doc/estimators/README.md index 390d0980..c43d64bd 100644 --- a/doc/estimators/README.md +++ b/doc/estimators/README.md @@ -16,3 +16,9 @@ Statistical estimation algorithms for fitting models to observed data and making | Algorithm | Description | |-----------------------------------------------------|--------------------------------------------------------------------------| | [Recursive Least Squares](RecursiveLeastSquares.md) | Sample-by-sample parameter estimation with exponential forgetting factor | + +## Consistency Metrics + +| Algorithm | Description | +|---------------------------------------------------------|-------------------------------------------------------------------------------------| +| [Consistency Metrics (NEES/NIS)](ConsistencyMetrics.md) | Normalised Estimation Error Squared and Normalised Innovation Squared with χ² gates | diff --git a/numerical/estimators/CMakeLists.txt b/numerical/estimators/CMakeLists.txt index 54d8c090..d4362399 100644 --- a/numerical/estimators/CMakeLists.txt +++ b/numerical/estimators/CMakeLists.txt @@ -1,2 +1,25 @@ +numerical_add_header_library(numerical.estimators) + +target_include_directories(numerical.estimators ${NUMERICAL_VISIBILITY} + "$" + "$" +) + +target_link_libraries(numerical.estimators ${NUMERICAL_VISIBILITY} + infra.util + numerical.math + numerical.solver +) + +target_sources(numerical.estimators PRIVATE + ConsistencyMetrics.hpp + Estimator.hpp +) + +numerical_add_coverage_sources(numerical.estimators + ConsistencyMetrics.cpp +) + add_subdirectory(offline) add_subdirectory(online) +add_subdirectory(test) diff --git a/numerical/estimators/ConsistencyMetrics.cpp b/numerical/estimators/ConsistencyMetrics.cpp new file mode 100644 index 00000000..c2d3973a --- /dev/null +++ b/numerical/estimators/ConsistencyMetrics.cpp @@ -0,0 +1,10 @@ +// Copyright 2025 Numerical Toolbox Contributors +// SPDX-License-Identifier: MIT +#include "numerical/estimators/ConsistencyMetrics.hpp" + +namespace estimators +{ + template class ConsistencyMetrics; + template class ConsistencyMetrics; + template class ConsistencyMetrics; +} diff --git a/numerical/estimators/ConsistencyMetrics.hpp b/numerical/estimators/ConsistencyMetrics.hpp new file mode 100644 index 00000000..3c56cc9f --- /dev/null +++ b/numerical/estimators/ConsistencyMetrics.hpp @@ -0,0 +1,123 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/GaussianElimination.hpp" +#include +#include +#include +#include + +namespace estimators +{ + namespace detail + { + static constexpr std::size_t kMaxChiSquareDim = 10; + static constexpr std::size_t kNumAlpha = 1; + + static constexpr std::array kChi2Lo95 = { + 0.000982f, 0.050636f, 0.215795f, 0.484419f, 0.831212f, + 1.237344f, 1.689869f, 2.179731f, 2.700389f, 3.246973f + }; + + static constexpr std::array kChi2Hi95 = { + 5.023886f, 7.377759f, 9.348404f, 11.143480f, 12.832502f, + 14.449376f, 16.012764f, 17.534546f, 19.022768f, 20.483177f + }; + } + + template + class ConsistencyMetrics + { + static_assert(std::is_floating_point_v, "ConsistencyMetrics supports floating-point types"); + static_assert(Dim >= 1 && Dim <= detail::kMaxChiSquareDim, + "ConsistencyMetrics Dim must be in [1, kMaxChiSquareDim]"); + + public: + using StateVector = math::Vector; + using CovarianceMatrix = math::Matrix; + + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional Nees(const StateVector& error, const CovarianceMatrix& covariance); + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance); + [[nodiscard]] static bool IsConsistent(T value); + [[nodiscard]] static bool IsTimeAveragedConsistent(T averagedValue, std::size_t numSamples); + + private: + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional Solve(const CovarianceMatrix& matrix, const StateVector& rhs); + [[nodiscard]] static OPTIMIZE_FOR_SPEED T DotProduct(const StateVector& a, const StateVector& b); + }; + + template + OPTIMIZE_FOR_SPEED std::optional ConsistencyMetrics::Nees(const StateVector& error, const CovarianceMatrix& covariance) + { + auto z = Solve(covariance, error); + if (!z.has_value()) + return std::nullopt; + return DotProduct(error, z.value()); + } + + template + OPTIMIZE_FOR_SPEED std::optional ConsistencyMetrics::Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance) + { + auto z = Solve(innovationCovariance, innovation); + if (!z.has_value()) + return std::nullopt; + return DotProduct(innovation, z.value()); + } + + template + bool ConsistencyMetrics::IsConsistent(T value) + { + return value >= static_cast(detail::kChi2Lo95[Dim - 1]) && + value <= static_cast(detail::kChi2Hi95[Dim - 1]); + } + + template + bool ConsistencyMetrics::IsTimeAveragedConsistent(T averagedValue, std::size_t numSamples) + { + if (numSamples == 0) + return false; + const std::size_t dof = numSamples * Dim; + const float lo = (dof <= detail::kMaxChiSquareDim) + ? detail::kChi2Lo95[dof - 1] / static_cast(numSamples) + : detail::kChi2Lo95[detail::kMaxChiSquareDim - 1] / static_cast(numSamples); + const float hi = (dof <= detail::kMaxChiSquareDim) + ? detail::kChi2Hi95[dof - 1] / static_cast(numSamples) + : detail::kChi2Hi95[detail::kMaxChiSquareDim - 1] / static_cast(numSamples); + return static_cast(averagedValue) >= lo && + static_cast(averagedValue) <= hi; + } + + template + OPTIMIZE_FOR_SPEED std::optional::StateVector> ConsistencyMetrics::Solve(const CovarianceMatrix& matrix, const StateVector& rhs) + { + for (std::size_t i = 0; i < Dim; ++i) + { + float pivot = std::abs(static_cast(matrix.at(i, i))); + if (pivot < 1e-10f) + return std::nullopt; + } + + solvers::GaussianElimination solver; + return solver.Solve(matrix, rhs); + } + + template + OPTIMIZE_FOR_SPEED T ConsistencyMetrics::DotProduct(const StateVector& a, const StateVector& b) + { + T result{}; + for (std::size_t i = 0; i < Dim; ++i) + result += a.at(i, 0) * b.at(i, 0); + return result; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ConsistencyMetrics; + extern template class ConsistencyMetrics; + extern template class ConsistencyMetrics; +#endif +} diff --git a/numerical/estimators/test/CMakeLists.txt b/numerical/estimators/test/CMakeLists.txt new file mode 100644 index 00000000..077667fb --- /dev/null +++ b/numerical/estimators/test/CMakeLists.txt @@ -0,0 +1,12 @@ +add_executable(numerical.estimators_test) +emil_build_for(numerical.estimators_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) +emil_add_test(numerical.estimators_test) + +target_link_libraries(numerical.estimators_test PUBLIC + gmock_main + numerical.estimators +) + +target_sources(numerical.estimators_test PRIVATE + TestConsistencyMetrics.cpp +) diff --git a/numerical/estimators/test/TestConsistencyMetrics.cpp b/numerical/estimators/test/TestConsistencyMetrics.cpp new file mode 100644 index 00000000..841acd63 --- /dev/null +++ b/numerical/estimators/test/TestConsistencyMetrics.cpp @@ -0,0 +1,191 @@ +// Copyright 2025 Numerical Toolbox Contributors +// SPDX-License-Identifier: MIT +#include "numerical/estimators/ConsistencyMetrics.hpp" +#include + +namespace +{ + class ConsistencyMetrics1DTest + : public ::testing::Test + { + protected: + using Metrics = estimators::ConsistencyMetrics; + using Vec1 = math::Vector; + using Mat1 = math::Matrix; + }; + + class ConsistencyMetrics2DTest + : public ::testing::Test + { + protected: + using Metrics = estimators::ConsistencyMetrics; + using Vec2 = math::Vector; + using Mat2 = math::Matrix; + }; + + class ConsistencyMetrics3DTest + : public ::testing::Test + { + protected: + using Metrics = estimators::ConsistencyMetrics; + using Vec3 = math::Vector; + using Mat3 = math::Matrix; + }; +} + +TEST_F(ConsistencyMetrics1DTest, NeesScalarIdentityCovariance) +{ + Vec1 error; + error.at(0, 0) = 2.0f; + Mat1 cov; + cov.at(0, 0) = 1.0f; + + auto result = Metrics::Nees(error, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 4.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics1DTest, NisScalarIdentityCovariance) +{ + Vec1 innovation; + innovation.at(0, 0) = 3.0f; + Mat1 cov; + cov.at(0, 0) = 1.0f; + + auto result = Metrics::Nis(innovation, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 9.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics1DTest, NeesScaledCovariance) +{ + Vec1 error; + error.at(0, 0) = 2.0f; + Mat1 cov; + cov.at(0, 0) = 4.0f; + + auto result = Metrics::Nees(error, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 1.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics1DTest, NeesSingularCovarianceReturnsNullopt) +{ + Vec1 error; + error.at(0, 0) = 1.0f; + Mat1 cov; + cov.at(0, 0) = 0.0f; + + auto result = Metrics::Nees(error, cov); + + EXPECT_FALSE(result.has_value()); +} + +TEST_F(ConsistencyMetrics1DTest, IsConsistentInsideBounds) +{ + const float midpoint = (estimators::detail::kChi2Lo95[0] + estimators::detail::kChi2Hi95[0]) / 2.0f; + + EXPECT_TRUE(Metrics::IsConsistent(midpoint)); +} + +TEST_F(ConsistencyMetrics1DTest, IsConsistentOutsideBoundsHigh) +{ + const float aboveHi = estimators::detail::kChi2Hi95[0] + 1.0f; + + EXPECT_FALSE(Metrics::IsConsistent(aboveHi)); +} + +TEST_F(ConsistencyMetrics1DTest, IsConsistentOutsideBoundsLow) +{ + EXPECT_FALSE(Metrics::IsConsistent(-1.0f)); +} + +TEST_F(ConsistencyMetrics1DTest, IsTimeAveragedConsistentZeroSamplesReturnsFalse) +{ + EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(1.0f, 0)); +} + +TEST_F(ConsistencyMetrics2DTest, Nees2DIdentityCovariance) +{ + Vec2 error; + error.at(0, 0) = 1.0f; + error.at(1, 0) = 2.0f; + Mat2 cov; + cov.at(0, 0) = 1.0f; + cov.at(0, 1) = 0.0f; + cov.at(1, 0) = 0.0f; + cov.at(1, 1) = 1.0f; + + auto result = Metrics::Nees(error, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 5.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics2DTest, Nis2DScaledDiagonalCovariance) +{ + Vec2 innovation; + innovation.at(0, 0) = 2.0f; + innovation.at(1, 0) = 4.0f; + Mat2 cov; + cov.at(0, 0) = 4.0f; + cov.at(0, 1) = 0.0f; + cov.at(1, 0) = 0.0f; + cov.at(1, 1) = 16.0f; + + auto result = Metrics::Nis(innovation, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics2DTest, IsConsistent2DWithinBounds) +{ + const float midpoint = (estimators::detail::kChi2Lo95[1] + estimators::detail::kChi2Hi95[1]) / 2.0f; + + EXPECT_TRUE(Metrics::IsConsistent(midpoint)); +} + +TEST_F(ConsistencyMetrics3DTest, Nees3DIdentityCovariance) +{ + Vec3 error; + error.at(0, 0) = 1.0f; + error.at(1, 0) = 1.0f; + error.at(2, 0) = 1.0f; + Mat3 cov; + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + cov.at(i, j) = (i == j) ? 1.0f : 0.0f; + + auto result = Metrics::Nees(error, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 3.0f, 1e-4f); +} + +TEST_F(ConsistencyMetrics3DTest, Nis3DFullCovariance) +{ + Vec3 innovation; + innovation.at(0, 0) = 1.0f; + innovation.at(1, 0) = 0.0f; + innovation.at(2, 0) = 0.0f; + Mat3 cov; + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + cov.at(i, j) = (i == j) ? 2.0f : 0.0f; + + auto result = Metrics::Nis(innovation, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.5f, 1e-4f); +} + +TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentWithOneSample) +{ + const float midpoint = (estimators::detail::kChi2Lo95[2] + estimators::detail::kChi2Hi95[2]) / 2.0f; + + EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(midpoint, 1)); +}