feat: add consistency metrics

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                                   |

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

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.

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 |

numerical/estimators/CMakeLists.txt

@@ -1,2 +1,25 @@
+numerical_add_header_library(numerical.estimators)
+
+target_include_directories(numerical.estimators ${NUMERICAL_VISIBILITY}
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/../../>"
+    "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>"
+)
+
+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)

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<float, 1>;
+    template class ConsistencyMetrics<float, 2>;
+    template class ConsistencyMetrics<float, 3>;
+}

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 <array>
+#include <cmath>
+#include <cstddef>
+#include <optional>
+
+namespace estimators
+{
+    namespace detail
+    {
+        static constexpr std::size_t kMaxChiSquareDim = 10;
+        static constexpr std::size_t kNumAlpha = 1;
+
+        static constexpr std::array<float, kMaxChiSquareDim> 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<float, kMaxChiSquareDim> kChi2Hi95 = {
+            5.023886f, 7.377759f, 9.348404f, 11.143480f, 12.832502f,
+            14.449376f, 16.012764f, 17.534546f, 19.022768f, 20.483177f
+        };
+    }
+
+    template<typename T, std::size_t Dim>
+    class ConsistencyMetrics
+    {
+        static_assert(std::is_floating_point_v<T>, "ConsistencyMetrics supports floating-point types");
+        static_assert(Dim >= 1 && Dim <= detail::kMaxChiSquareDim,
+            "ConsistencyMetrics Dim must be in [1, kMaxChiSquareDim]");
+
+    public:
+        using StateVector = math::Vector<T, Dim>;
+        using CovarianceMatrix = math::Matrix<T, Dim, Dim>;
+
+        [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional<T> Nees(const StateVector& error, const CovarianceMatrix& covariance);
+        [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional<T> 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<StateVector> Solve(const CovarianceMatrix& matrix, const StateVector& rhs);
+        [[nodiscard]] static OPTIMIZE_FOR_SPEED T DotProduct(const StateVector& a, const StateVector& b);
+    };
+
+    template<typename T, std::size_t Dim>
+    OPTIMIZE_FOR_SPEED std::optional<T> ConsistencyMetrics<T, Dim>::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<typename T, std::size_t Dim>
+    OPTIMIZE_FOR_SPEED std::optional<T> ConsistencyMetrics<T, Dim>::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<typename T, std::size_t Dim>
+    bool ConsistencyMetrics<T, Dim>::IsConsistent(T value)
+    {
+        return value >= static_cast<T>(detail::kChi2Lo95[Dim - 1]) &&
+               value <= static_cast<T>(detail::kChi2Hi95[Dim - 1]);
+    }
+
+    template<typename T, std::size_t Dim>
+    bool ConsistencyMetrics<T, Dim>::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<float>(numSamples)
+                             : detail::kChi2Lo95[detail::kMaxChiSquareDim - 1] / static_cast<float>(numSamples);
+        const float hi = (dof <= detail::kMaxChiSquareDim)
+                             ? detail::kChi2Hi95[dof - 1] / static_cast<float>(numSamples)
+                             : detail::kChi2Hi95[detail::kMaxChiSquareDim - 1] / static_cast<float>(numSamples);
+        return static_cast<float>(averagedValue) >= lo &&
+               static_cast<float>(averagedValue) <= hi;
+    }
+
+    template<typename T, std::size_t Dim>
+    OPTIMIZE_FOR_SPEED std::optional<typename ConsistencyMetrics<T, Dim>::StateVector> ConsistencyMetrics<T, Dim>::Solve(const CovarianceMatrix& matrix, const StateVector& rhs)
+    {
+        for (std::size_t i = 0; i < Dim; ++i)
+        {
+            float pivot = std::abs(static_cast<float>(matrix.at(i, i)));
+            if (pivot < 1e-10f)
+                return std::nullopt;
+        }
+
+        solvers::GaussianElimination<T, Dim> solver;
+        return solver.Solve(matrix, rhs);
+    }
+
+    template<typename T, std::size_t Dim>
+    OPTIMIZE_FOR_SPEED T ConsistencyMetrics<T, Dim>::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<float, 1>;
+    extern template class ConsistencyMetrics<float, 2>;
+    extern template class ConsistencyMetrics<float, 3>;
+#endif
+}

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
+)