From c1a669bd94ebad3a7922dbc3865af0e02a5905f4 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 18:44:04 +0000 Subject: [PATCH] feat(filters): add ResidualKalmanUpdate (NT74) Stateless Kalman measurement update for estimators that own their state: the caller passes the residual z (-) h(x), the Jacobian and the noise. The innovation covariance is factored once with Cholesky to give the normalised innovation squared, a chi-square gate rejects outliers without touching the state, the gain comes from triangular solves, and the covariance uses the symmetrised Joseph form. Nis alone scores candidate associations. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 20 +-- doc/filters/README.md | 15 +-- doc/filters/active/ResidualKalmanUpdate.md | 104 ++++++++++++++++ numerical/filters/active/CMakeLists.txt | 2 + .../filters/active/ResidualKalmanUpdate.cpp | 10 ++ .../filters/active/ResidualKalmanUpdate.hpp | 101 +++++++++++++++ numerical/filters/active/test/CMakeLists.txt | 1 + .../active/test/TestResidualKalmanUpdate.cpp | 117 ++++++++++++++++++ roadmap/README.md | 2 +- .../ResidualKalmanUpdate/explanation.md | 35 ------ .../ResidualKalmanUpdate/implementation.md | 94 -------------- .../active/ResidualKalmanUpdate/tests.md | 49 -------- 13 files changed, 348 insertions(+), 204 deletions(-) create mode 100644 doc/filters/active/ResidualKalmanUpdate.md create mode 100644 numerical/filters/active/ResidualKalmanUpdate.cpp create mode 100644 numerical/filters/active/ResidualKalmanUpdate.hpp create mode 100644 numerical/filters/active/test/TestResidualKalmanUpdate.cpp delete mode 100644 roadmap/filters/active/ResidualKalmanUpdate/explanation.md delete mode 100644 roadmap/filters/active/ResidualKalmanUpdate/implementation.md delete mode 100644 roadmap/filters/active/ResidualKalmanUpdate/tests.md diff --git a/README.md b/README.md index f5df22bc..6e0a3761 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, Continuous-to-Discrete, Transfer Function ↔ State Space | | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, 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, Friction Compensation (Coulomb + Viscous + Stribeck) | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) | -| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root 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, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | +| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Residual-Form Kalman Update, 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, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log), SE(2) Transform (Planar Twists, Exp/Log) | diff --git a/ROADMAP.md b/ROADMAP.md index add2919e..3c225e07 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | -| 74 | Residual-form Kalman update with χ² gating | `filters/active` | ★★☆☆☆ | | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | | 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ | | 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ | @@ -35,9 +34,9 @@ Difficulty legend: | 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ | | 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ | -Items 67–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as -NT67–NT74; NT53–NT66 are deployed). They serve its mobile, aerial, legged, marine and localization -families and also have non-robotics consumers. Items 67, 70 and 74 have specs under +Items 67–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as +NT67–NT73; NT53–NT66 and NT74 are deployed). They serve its mobile, aerial, legged, marine and +localization families and also have non-robotics consumers. Items 67 and 70 have specs under [`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none yet. All are *(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family @@ -56,19 +55,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `SingularValueDecomposition`. - **Spec:** [roadmap/solvers/PointSetAlignment](roadmap/solvers/PointSetAlignment/implementation.md). -### 74. Residual-form Kalman update with χ² gating -- **What:** Stateless Kalman measurement update from a caller-formed residual `z ⊖ h(x)`, Jacobian and - noise: Joseph-form correction, normalised innovation squared, and a χ² gate that rejects outliers - without touching the state. -- **Embedded value:** Covers what `ExtendedKalmanFilter` cannot: wrapped angle residuals (bearings, - headings), a measurement model chosen per observation (landmarks), and gating and data association - by Mahalanobis distance. -- **Algorithm / paper:** P. Maybeck, *Stochastic Models, Estimation, and Control*, Vol. 1 (1979), Ch. 5; - Y. Bar-Shalom, X.-R. Li, T. Kirubarajan, *Estimation with Applications to Tracking and Navigation* - (2001), Ch. 5. -- **Reuses:** `CholeskyDecomposition`, `ConsistencyMetrics`, `MatrixOperations`; item 54 for angle residuals. -- **Spec:** [roadmap/filters/active/ResidualKalmanUpdate](roadmap/filters/active/ResidualKalmanUpdate/implementation.md). - --- ## Tier 3 — Moderate ★★★☆☆ diff --git a/doc/filters/README.md b/doc/filters/README.md index 77491c63..f2629290 100644 --- a/doc/filters/README.md +++ b/doc/filters/README.md @@ -8,13 +8,14 @@ Digital filtering algorithms for noise reduction, state estimation, and signal c Filters that incorporate feedback and a dynamic internal model of the system. -| Algorithm | Description | -|------------------------------------------------------------|--------------------------------------------------------------------------| -| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise | -| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) | -| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required | -| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence | -| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness | +| Algorithm | Description | +|---------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------| +| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise | +| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) | +| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required | +| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence | +| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness | +| [Residual-Form Kalman Update](active/ResidualKalmanUpdate.md) | Stateless Joseph-form update from a caller-formed residual, with normalised innovation squared and a χ² gate for outliers and association | ### Passive Filters diff --git a/doc/filters/active/ResidualKalmanUpdate.md b/doc/filters/active/ResidualKalmanUpdate.md new file mode 100644 index 00000000..ce39191c --- /dev/null +++ b/doc/filters/active/ResidualKalmanUpdate.md @@ -0,0 +1,104 @@ +# Residual-Form Kalman Update with χ² Gating + +## Overview & Motivation + +A classical Kalman filter owns its state and its measurement model: it predicts the measurement $h(x)$, subtracts it from $z$ and corrects. Many estimators cannot work that way. + +- A **bearing or heading** residual must be wrapped to $(-\pi, \pi]$. Subtracting $z - h(x)$ directly turns a $2^\circ$ error near $\pm 180^\circ$ into a $358^\circ$ one. +- A **landmark** filter picks a different measurement model for every observation. +- A robust filter must **reject outliers** and **associate** measurements before applying them, using the innovation covariance that a self-contained filter keeps hidden. + +The residual-form update is a stateless building block for these cases. The caller owns the state, forms the residual $r = z \ominus h(x)$ in whatever way its geometry needs, and passes the Jacobian and noise of the chosen model. +The update returns the **normalised innovation squared** (NIS), and either applies a Joseph-form correction or rejects the measurement at a χ² gate without touching the state. + +## Mathematical Theory + +### Innovation Covariance and NIS + +For state covariance $P$, measurement Jacobian $H$ and measurement noise $R$: + +$$S = H P H^T + R, \qquad \text{NIS} = r^T S^{-1} r$$ + +$S$ is factored once with Cholesky, $S = L L^T$, and every product with $S^{-1}$ uses two triangular solves. A non-positive-definite $S$ is reported as singular, and the state is left unchanged. + +### χ² Gating + +Under a consistent filter, the NIS follows a χ² distribution with $m$ degrees of freedom for an $m$-dimensional measurement. A measurement is rejected when its NIS exceeds the chosen quantile: + +| Measurement size | 95 % gate | 99 % gate | +|------------------|-----------|-----------| +| 1 | 3.84 | 6.63 | +| 2 | 5.99 | 9.21 | + +A NIS exactly at the gate is applied. The default gate is infinite, so no measurement is rejected. + +### Joseph-Form Update + +The gain is computed without an explicit inverse, from $S K^T = H P$: + +$$K = P H^T S^{-1}, \qquad x \leftarrow x + K r$$ + +The covariance uses the Joseph form, which stays symmetric and positive semi-definite under rounding for any gain: + +$$P \leftarrow (I - K H)\,P\,(I - K H)^T + K R K^T$$ + +The result is symmetrised to remove the asymmetry left by the products. The short form $(I - KH)P$ is cheaper but can lose positive-definiteness in single precision. + +### Residual Convention + +The residual is $z \ominus h(x)$. For angle components it is the wrapped difference, and after the update the caller re-wraps any angle in the state, since only it knows which components are angles. Independent measurements can be applied one after another, each with its own $H$ and $R$; for a linear model this equals one stacked update. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|--------------------------|----------|-------------------------------------------------| +| NIS only | $O(N^2 M + N M^2 + M^3)$ | $O(M^2)$ | Innovation covariance and one Cholesky solve | +| Update | $O(N^2 M + N M^2 + M^3)$ | $O(N M)$ | One factorisation, $N$ solve pairs for the gain | + +For $N$ states and $M$ measurements. The localization cases ($N \le 4$, $M \le 2$) take under 200 multiply-adds. + +## Step-by-Step Walkthrough + +State $x = (1, 0.5)$ with covariance $P = \begin{pmatrix} 0.5 & 0.1 \\ 0.1 & 0.3 \end{pmatrix}$, measuring the first component with noise $R = 0.2$: + +1. **Scalar update**: the measurement $1.4$ gives the residual $0.4$, $S = 0.7$ and $K = (0.714, 0.143)$. The state becomes $(1.286, 0.557)$, the first variance drops to $0.143$, and the NIS is $0.229$. +2. **Vector update**: also measuring the sum $x_1 + x_2 = 1.2$ with noise $0.1$ gives the residual $(0.4, -0.3)$ and NIS $0.934$, and moves the state to $(1.098, 0.212)$. +3. **Association**: the NIS can be computed alone to score several candidate measurements before committing one. +4. **Gating**: an outlier at $4.0$ has NIS $12.86$, above the 99 % gate of $6.63$. It is rejected and the state is unchanged. +5. **Singular case**: with $P = 0$ and $R = 0$, $S$ is not positive definite. The update reports it and changes nothing. +6. **Angle residual**: a heading estimate of $3.1$ rad measured at $-3.1$ rad has a wrapped residual of $0.083$. The corrected heading wraps to $-3.117$ rad. The raw difference $-6.2$ would have dragged the estimate to $-1.86$ rad. +7. **Long runs**: after 100 updates the covariance is still exactly symmetric and positive definite. + +## Pitfalls & Edge Cases + +- **Unwrapped angle residuals**: the classic cause of bearing-only EKF divergence. Always wrap angle components of the residual. +- **Re-wrapping the state**: the update adds $K r$ to an angle without wrapping it. Wrap angle states afterwards. +- **Gate choice**: a tight gate rejects good measurements after a disturbance and can lock the filter out. Monitor the gating rate. +- **Correlated measurements**: sequential updates assume independent noise. Stack correlated measurements into one update. +- **Inconsistent noise**: an underestimated $R$ inflates the NIS and makes the gate reject valid data. + +## Variants & Generalizations + +- **Self-contained filters**: the Kalman and extended Kalman filters, which own their state and measurement model. +- **Square-root updates**: propagating a Cholesky factor of $P$ for extra robustness. +- **Information form**: accumulating $H^T R^{-1} H$ for many simultaneous measurements. +- **Multi-hypothesis association**: joint probabilistic data association and multiple-hypothesis tracking built on the NIS. + +## Applications + +- Wheel and gyroscope odometry fusion with heading measurements. +- Landmark-based localization with range and bearing observations. +- Target tracking with gating and nearest-neighbour association. +- Fault detection by monitoring the NIS of each sensor. + +## Connections to Other Algorithms + +- [Kalman Filter](KalmanFilter.md): the self-contained linear filter with the same update equations. +- [Extended Kalman Filter](ExtendedKalmanFilter.md): forms the residual itself and fixes the measurement model at construction. +- [Square-Root Kalman Filter](SquareRootKalmanFilter.md): a factored alternative for poorly conditioned problems. + +## References & Further Reading + +- Maybeck, P.S. (1979). *Stochastic Models, Estimation, and Control*, Vol. 1. Academic Press. Chapter 5. +- Bar-Shalom, Y., Li, X.-R. and Kirubarajan, T. (2001). *Estimation with Applications to Tracking and Navigation*. Wiley. Chapter 5. +- Bucy, R.S. and Joseph, P.D. (1968). *Filtering for Stochastic Processes with Applications to Guidance*. Interscience. diff --git a/numerical/filters/active/CMakeLists.txt b/numerical/filters/active/CMakeLists.txt index 2dedea98..dd63a434 100644 --- a/numerical/filters/active/CMakeLists.txt +++ b/numerical/filters/active/CMakeLists.txt @@ -19,6 +19,7 @@ target_sources(numerical.filters.active PRIVATE KalmanFilter.hpp KalmanFilterBase.hpp KalmanSmoother.hpp + ResidualKalmanUpdate.hpp SquareRootKalmanFilter.hpp UnscentedKalmanFilter.hpp ) @@ -31,6 +32,7 @@ numerical_add_coverage_sources(numerical.filters.active KalmanFilter.cpp KalmanFilterBase.cpp KalmanSmoother.cpp + ResidualKalmanUpdate.cpp SquareRootKalmanFilter.cpp UnscentedKalmanFilter.cpp ) diff --git a/numerical/filters/active/ResidualKalmanUpdate.cpp b/numerical/filters/active/ResidualKalmanUpdate.cpp new file mode 100644 index 00000000..ac809037 --- /dev/null +++ b/numerical/filters/active/ResidualKalmanUpdate.cpp @@ -0,0 +1,10 @@ +#include "numerical/filters/active/ResidualKalmanUpdate.hpp" + +namespace filters +{ + template class ResidualKalmanUpdate; + template class ResidualKalmanUpdate; + template class ResidualKalmanUpdate; + template class ResidualKalmanUpdate; + template class ResidualKalmanUpdate; +} diff --git a/numerical/filters/active/ResidualKalmanUpdate.hpp b/numerical/filters/active/ResidualKalmanUpdate.hpp new file mode 100644 index 00000000..c7d90e19 --- /dev/null +++ b/numerical/filters/active/ResidualKalmanUpdate.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include "numerical/math/CholeskyDecomposition.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/ConsistencyMetrics.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/MatrixOperations.hpp" +#include "numerical/math/TriangularSolve.hpp" +#include +#include +#include +#include +#include + +namespace filters +{ + enum class KalmanUpdateStatus : std::uint8_t + { + Applied, + Gated, + Singular + }; + + template + struct KalmanUpdateResult + { + static_assert(std::is_floating_point_v, "KalmanUpdateResult supports floating-point types"); + + KalmanUpdateStatus status{ KalmanUpdateStatus::Singular }; + T nis{}; + }; + + template + class ResidualKalmanUpdate + { + static_assert(std::is_floating_point_v, "ResidualKalmanUpdate supports floating-point types"); + static_assert(StateSize > 0 && MeasurementSize > 0, "ResidualKalmanUpdate needs a state and a measurement"); + + public: + using StateVector = math::Vector; + using Covariance = math::SquareMatrix; + using Residual = math::Vector; + using Jacobian = math::Matrix; + using MeasurementNoise = math::SquareMatrix; + + static std::optional Nis(const Residual& residual, const Jacobian& jacobian, const Covariance& covariance, const MeasurementNoise& noise); + OPTIMIZE_FOR_SPEED static KalmanUpdateResult Apply(StateVector& state, Covariance& covariance, const Residual& residual, const Jacobian& jacobian, + const MeasurementNoise& noise, T gate = std::numeric_limits::infinity()); + + private: + using Factor = math::SquareMatrix; + using Gain = math::Matrix; + + static Residual Solve(const Factor& lower, const Factor& upper, const Residual& rhs); + }; + + template + std::optional ResidualKalmanUpdate::Nis(const Residual& residual, const Jacobian& jacobian, const Covariance& covariance, const MeasurementNoise& noise) + { + return math::ConsistencyMetrics::Nis(residual, math::CongruenceTransform(jacobian, covariance) + noise); + } + + template + OPTIMIZE_FOR_SPEED KalmanUpdateResult ResidualKalmanUpdate::Apply(StateVector& state, Covariance& covariance, const Residual& residual, + const Jacobian& jacobian, const MeasurementNoise& noise, T gate) + { + const auto innovationCovariance{ math::CongruenceTransform(jacobian, covariance) + noise }; + const auto lower{ math::CholeskyDecomposition::TryFactor(innovationCovariance) }; + if (!lower) + return KalmanUpdateResult{ KalmanUpdateStatus::Singular, T{} }; + + const Factor upper{ lower->Transpose() }; + const T nis{ math::DotProduct(residual, Solve(*lower, upper, residual)) }; + if (nis > gate) + return KalmanUpdateResult{ KalmanUpdateStatus::Gated, nis }; + + const auto jacobianCovariance{ jacobian * covariance }; + Gain gain{}; + for (std::size_t row = 0; row < StateSize; ++row) + gain.SetBlock(Solve(*lower, upper, jacobianCovariance.template GetBlock(0, row)).Transpose(), row, 0); + + state = state + gain * residual; + const auto correction{ Covariance::Identity() - gain * jacobian }; + covariance = math::Symmetrize(math::CongruenceTransform(correction, covariance) + math::CongruenceTransform(gain, noise)); + return KalmanUpdateResult{ KalmanUpdateStatus::Applied, nis }; + } + + template + auto ResidualKalmanUpdate::Solve(const Factor& lower, const Factor& upper, const Residual& rhs) -> Residual + { + return math::SolveUpperTriangular(upper, math::SolveLowerTriangular(lower, rhs)); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ResidualKalmanUpdate; + extern template class ResidualKalmanUpdate; + extern template class ResidualKalmanUpdate; + extern template class ResidualKalmanUpdate; + extern template class ResidualKalmanUpdate; +#endif +} diff --git a/numerical/filters/active/test/CMakeLists.txt b/numerical/filters/active/test/CMakeLists.txt index 81a31298..b16050a1 100644 --- a/numerical/filters/active/test/CMakeLists.txt +++ b/numerical/filters/active/test/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(numerical.filters.active_test PRIVATE TestExtendedKalmanFilter.cpp TestKalmanFilter.cpp TestKalmanSmoother.cpp + TestResidualKalmanUpdate.cpp TestSquareRootKalmanFilter.cpp TestUnscentedKalmanFilter.cpp ) diff --git a/numerical/filters/active/test/TestResidualKalmanUpdate.cpp b/numerical/filters/active/test/TestResidualKalmanUpdate.cpp new file mode 100644 index 00000000..66b0a876 --- /dev/null +++ b/numerical/filters/active/test/TestResidualKalmanUpdate.cpp @@ -0,0 +1,117 @@ +#include "numerical/filters/active/ResidualKalmanUpdate.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include +#include + +namespace +{ + using Scalar = filters::ResidualKalmanUpdate; + using Vector = filters::ResidualKalmanUpdate; + using Angle = filters::ResidualKalmanUpdate; + using filters::KalmanUpdateStatus; + + class TestResidualKalmanUpdate + : public ::testing::Test + { + protected: + static void ExpectState(const Scalar::StateVector& actual, float first, float second) + { + EXPECT_NEAR(actual.at(0, 0), first, 1e-5f); + EXPECT_NEAR(actual.at(1, 0), second, 1e-5f); + } + + static void ExpectCovariance(const Scalar::Covariance& actual, float diagonal0, float offDiagonal, float diagonal1) + { + EXPECT_NEAR(actual.at(0, 0), diagonal0, 1e-5f); + EXPECT_NEAR(actual.at(0, 1), offDiagonal, 1e-5f); + EXPECT_NEAR(actual.at(1, 0), offDiagonal, 1e-5f); + EXPECT_NEAR(actual.at(1, 1), diagonal1, 1e-5f); + } + + Scalar::StateVector x{ 1.0f, 0.5f }; + Scalar::Covariance P{ { 0.5f, 0.1f }, { 0.1f, 0.3f } }; + Scalar::Jacobian H{ { 1.0f, 0.0f } }; + Scalar::MeasurementNoise R{ { 0.2f } }; + Scalar::Residual r{ 0.4f }; + }; +} + +TEST_F(TestResidualKalmanUpdate, scalar_update_matches_the_kalman_equations) +{ + const auto result{ Scalar::Apply(x, P, r, H, R) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Applied); + EXPECT_NEAR(result.nis, 0.228571f, 1e-5f); + ExpectState(x, 1.285714f, 0.557143f); + ExpectCovariance(P, 0.142857f, 0.028571f, 0.285714f); +} + +TEST_F(TestResidualKalmanUpdate, vector_update_matches_the_kalman_equations) +{ + const Vector::Jacobian jacobian{ { 1.0f, 0.0f }, { 1.0f, 1.0f } }; + const Vector::MeasurementNoise noise{ { 0.2f, 0.0f }, { 0.0f, 0.1f } }; + + const auto result{ Vector::Apply(x, P, Vector::Residual{ 0.4f, -0.3f }, jacobian, noise) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Applied); + EXPECT_NEAR(result.nis, 0.934146f, 1e-5f); + ExpectState(x, 1.097561f, 0.212195f); + ExpectCovariance(P, 0.092683f, -0.063415f, 0.117073f); +} + +TEST_F(TestResidualKalmanUpdate, nis_matches_the_applied_update) +{ + const auto nis{ Scalar::Nis(r, H, P, R) }; + + ASSERT_TRUE(nis.has_value()); + EXPECT_NEAR(*nis, 0.228571f, 1e-5f); + EXPECT_EQ(*nis, Scalar::Apply(x, P, r, H, R).nis); +} + +TEST_F(TestResidualKalmanUpdate, gate_rejects_an_outlier_without_touching_the_state) +{ + const auto result{ Scalar::Apply(x, P, Scalar::Residual{ 3.0f }, H, R, 6.63f) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Gated); + EXPECT_NEAR(result.nis, 12.857143f, 1e-4f); + ExpectState(x, 1.0f, 0.5f); + ExpectCovariance(P, 0.5f, 0.1f, 0.3f); +} + +TEST_F(TestResidualKalmanUpdate, singular_innovation_covariance_is_reported) +{ + Scalar::Covariance zero{}; + const Scalar::MeasurementNoise noNoise{}; + + const auto result{ Scalar::Apply(x, zero, r, H, noNoise) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Singular); + EXPECT_EQ(result.nis, 0.0f); + ExpectState(x, 1.0f, 0.5f); + ExpectCovariance(zero, 0.0f, 0.0f, 0.0f); + EXPECT_FALSE(Scalar::Nis(r, H, zero, noNoise).has_value()); +} + +TEST_F(TestResidualKalmanUpdate, wrapped_residual_keeps_an_angle_estimate_sane) +{ + Angle::StateVector heading{ 3.1f }; + Angle::Covariance variance{ { 0.04f } }; + const float measured{ -3.1f }; + + const auto result{ Angle::Apply(heading, variance, Angle::Residual{ math::AngleDifference(measured, heading.at(0, 0)) }, Angle::Jacobian{ { 1.0f } }, Angle::MeasurementNoise{ { 0.01f } }) }; + heading.at(0, 0) = math::WrapToPi(heading.at(0, 0)); + + EXPECT_NEAR(heading.at(0, 0), -3.116637f, 1e-5f); + EXPECT_NEAR(variance.at(0, 0), 0.008f, 1e-6f); + EXPECT_NEAR(result.nis, 0.138396f, 1e-4f); +} + +TEST_F(TestResidualKalmanUpdate, joseph_form_stays_symmetric) +{ + for (std::size_t k = 0; k < 100; ++k) + Scalar::Apply(x, P, r, H, R); + + EXPECT_EQ(P.at(0, 1), P.at(1, 0)); + EXPECT_GT(P.at(0, 0) + P.at(1, 1), 0.0f); + EXPECT_GT(P.at(0, 0) * P.at(1, 1) - P.at(0, 1) * P.at(1, 0), 0.0f); +} diff --git a/roadmap/README.md b/roadmap/README.md index 1a97c3fb..9ec99f02 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -80,7 +80,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `ExponentialMovingAverage` (1) · `MovingAverage` (2) · `MedianFilter` (6) · `CicFilter` (14) · `BiquadCascade` (15) · `NotchCombFilter` (16) · `SavitzkyGolayFilter` (22) · `IirFilterDesign` (45) ### `filters/active` -`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) · `ParticleFilter` (70) · `ResidualKalmanUpdate` (74) +`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) · `ParticleFilter` (70) ### `controllers` `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) diff --git a/roadmap/filters/active/ResidualKalmanUpdate/explanation.md b/roadmap/filters/active/ResidualKalmanUpdate/explanation.md deleted file mode 100644 index 782285bd..00000000 --- a/roadmap/filters/active/ResidualKalmanUpdate/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Residual-Form Kalman Update — Overview - -## What it is -The measurement-update half of a Kalman filter, packaged as a standalone step. The caller states how far -the measurement is from the prediction (the residual), how that measurement depends on the state and how -noisy it is. The step corrects the state and its uncertainty, or refuses if the measurement is -implausible. - -## Why it matters (embedded) -Real estimators rarely fit a fixed "measure, subtract, correct" mould. A bearing of 179° against a -prediction of −179° is a 2° error, not 358°. A robot seeing one landmark among many needs a different -measurement model each time. A reflection or a mismatched landmark must be thrown out before it -corrupts the estimate. Letting the caller form the residual and checking it against the predicted -uncertainty covers all three in a few dozen operations for small states. - -## How it works (intuition) -Combining the predicted uncertainty with the measurement noise gives the expected spread of the residual. -Dividing the actual residual by that spread (in the matrix sense) yields the normalised innovation -squared — how many "standard deviations" away the measurement is. If that exceeds a χ² gate the -measurement is ignored. Otherwise the Kalman gain blends prediction and measurement, and the covariance is -updated in Joseph form, which keeps it a valid covariance even with rounding. - -## Key parameters -- **residual** `z ⊖ h(x)` — formed by the caller (wrapped for angles). -- **measurement Jacobian `H`** and **noise `R`**. -- **gate** — χ² threshold on the NIS (e.g. 9.21 for 2-D at 99 %). - -## Reference -P. S. Maybeck, *Stochastic Models, Estimation, and Control*, Vol. 1 (1979), Ch. 5 (Joseph form); -Y. Bar-Shalom, X.-R. Li, T. Kirubarajan, *Estimation with Applications to Tracking and Navigation* (2001), -Ch. 5 (validation gates). - -## See also -`ExtendedKalmanFilter` (fixed-model filter), `ConsistencyMetrics` (NIS/NEES), `AngleUtilities` (item 54); -robotics-toolbox-cpp's odometry fusion (E1) and landmark localization (E3). diff --git a/roadmap/filters/active/ResidualKalmanUpdate/implementation.md b/roadmap/filters/active/ResidualKalmanUpdate/implementation.md deleted file mode 100644 index cef8c9c4..00000000 --- a/roadmap/filters/active/ResidualKalmanUpdate/implementation.md +++ /dev/null @@ -1,94 +0,0 @@ -# Residual-Form Kalman Update with χ² Gating — Implementation Pseudocode - -> Roadmap ref: #74 (Tier 2) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) - -A stateless Kalman measurement update for estimators that own their state and must form the -innovation themselves. Typical cases are angles that must be wrapped (bearings, headings), a measurement -model chosen per observation (one landmark among many), and measurements that must be gated or -associated by Mahalanobis distance before they are applied. The caller passes the residual `r = z ⊖ h(x)`, -the Jacobian `H` and the noise `R`. The update returns the normalised innovation squared (NIS) and either -applies the Joseph-form update or rejects it at the gate. `ExtendedKalmanFilter` cannot serve these -cases: it forms `z − h(x)` itself, fixes `h` at construction, and hides `S`. - -## Data structures - -```cpp -enum class KalmanUpdateStatus : std::uint8_t { applied, gated, singular } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct KalmanUpdateResult: - KalmanUpdateStatus status - T nis # rᵀ S⁻¹ r (0 when singular) - -template -class ResidualKalmanUpdate # stateless; static members only - using StateVector = math::Vector; using Covariance = math::SquareMatrix - using Residual = math::Vector; using Jacobian = math::Matrix - using MeasurementNoise = math::SquareMatrix -``` - -## Interface - -```cpp -static std::optional Nis(const Residual& r, const Jacobian& H, const Covariance& P, const MeasurementNoise& R) -static KalmanUpdateResult Apply(StateVector& x, Covariance& P, const Residual& r, const Jacobian& H, - const MeasurementNoise& R, T gate = std::numeric_limits::infinity()) # hot path -``` - -`Nis` lets a caller score several candidate associations before committing one with `Apply`. The caller -normalises the state afterwards (for example re-wraps a heading), because only it knows which components -are angles. - -## Algorithm (pseudocode) - -```text -function Nis(r, H, P, R): - S = CongruenceTransform(H, P) + R # H·P·Hᵀ + R; upstream MatrixOperations - return ConsistencyMetrics::Nis(r, S) # Cholesky solve; nullopt if S is not SPD - -function Apply(x, P, r, H, R, gate): # OPTIMIZE_FOR_SPEED - S = CongruenceTransform(H, P) + R - L = CholeskyDecomposition::TryFactor(S) - if L is nullopt: return { singular, 0 } # x, P untouched - y = L-solve(S, r); nis = r·y # S⁻¹r via the factor - if nis > gate: return { gated, nis } # x, P untouched - PHt = P·Hᵀ - K = (L-solve(S, PHtᵀ))ᵀ # K = P·Hᵀ·S⁻¹, M triangular-solve pairs, no explicit inverse - x += K·r - A = I − K·H - P = Symmetrize(A·P·Aᵀ + K·R·Kᵀ) # Joseph form: stays symmetric PSD in float - return { applied, nis } -``` - -## Complexity & memory - -- `O(N²·M + N·M² + M³)`: one `M×M` Cholesky and `N`-by-`M` products. For the localization cases - (`N ≤ 4`, `M ≤ 2`) that is under 200 multiply-adds. -- Stack only: `S`, its factor, `K`, `A`. No heap. - -## Numerical / embedded notes - -- **Gate values.** Gate against the χ² quantile for `M` degrees of freedom: 6.63 (M = 1) and 9.21 (M = 2) - at 99 %, 3.84 / 5.99 at 95 %. `ConsistencyMetrics` holds the two-sided 95 % bounds for NEES/NIS - monitoring; gating uses only the upper tail. -- **Joseph form.** `(I − KH)P(I − KH)ᵀ + KRKᵀ` is valid for any gain and stays PSD under `float` rounding, - unlike `(I − KH)P`. `Symmetrize` removes the asymmetry the products leave. -- **Residual convention.** The residual is `z ⊖ h(x)`: for an angle component, the caller wraps it with - `AngleDifference` (item 54). Passing an unwrapped `±2π` jump is the classic bearing-EKF divergence, and - the test shows it. -- **Sequential updates.** Independent measurements (several landmarks) can be applied one after - another, each with its own `H` and `R`; for a linear model that equals one stacked update. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/filters/active/ResidualKalmanUpdate.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Apply`, and `extern template class ResidualKalmanUpdate;` / `` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/filters/active/ResidualKalmanUpdate.cpp` → the same instantiations. -- Test: `numerical/filters/active/test/TestResidualKalmanUpdate.cpp` -- Doc: `doc/filters/active/ResidualKalmanUpdate.md` (per `doc/TEMPLATE.md`) + row in `doc/filters/README.md` -- CMake: `.hpp` → `target_sources(numerical.filters.active PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.filters.active ...)`; test → `numerical.filters.active_test`. -- Reuses: `CholeskyDecomposition`, `ConsistencyMetrics`, `MatrixOperations` (`CongruenceTransform`, `Symmetrize`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/filters/active/ResidualKalmanUpdate/tests.md b/roadmap/filters/active/ResidualKalmanUpdate/tests.md deleted file mode 100644 index 6307b763..00000000 --- a/roadmap/filters/active/ResidualKalmanUpdate/tests.md +++ /dev/null @@ -1,49 +0,0 @@ -# Residual-Form Kalman Update — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestResidualKalmanUpdate : public ::testing::Test: - Vector x{ 1.0, 0.5 } - SquareMatrix P{ { 0.5, 0.1 }, { 0.1, 0.3 } } - Matrix H{ { 1, 0 } }; SquareMatrix R{ 0.2 } -# each case below is a TEST_F(TestResidualKalmanUpdate, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -scalar_update_matches_the_kalman_equations: - Act: Apply(x, P, r = (0.4), H, R) # z = 1.4 - Assert: status applied, nis ≈ 0.228571; x ≈ (1.285714, 0.557143); P ≈ [[0.142857, 0.028571], [0.028571, 0.285714]] -vector_update_matches_the_kalman_equations: - Arrange: H₂ = [[1, 0], [1, 1]], R₂ = diag(0.2, 0.1), r = (1.4, 1.2) − H₂x = (0.4, −0.3) - Assert: nis ≈ 0.934146; x ≈ (1.097561, 0.212195); P ≈ [[0.092683, −0.063415], [−0.063415, 0.117073]] -nis_matches_the_applied_update: - Assert: Nis((0.4), H, P, R) ≈ 0.228571 == Apply(...).nis -gate_rejects_an_outlier_without_touching_the_state: - Act: Apply(x, P, r = (3.0), H, R, 6.63) # z = 4.0 - Assert: status gated, nis ≈ 12.857143; x and P unchanged -singular_innovation_covariance_is_reported: - Arrange: P = 0, R = 0 - Assert: status singular; x and P unchanged; Nis(...) == nullopt -wrapped_residual_keeps_an_angle_estimate_sane: - Arrange: x = (3.1), P = (0.04), R = (0.01), z = −3.1 - Act: Apply with r = AngleDifference(z, 3.1) = 0.083185, then WrapToPi(x) - Assert: x ≈ −3.116637; P ≈ 0.008; nis ≈ 0.138396 # the raw residual −6.2 would drive x to −1.86 -joseph_form_stays_symmetric: - Assert: after 100 scalar updates of the fixture, P(0,1) == P(1,0) and both eigenvalues > 0 -``` - -## Reference vectors - -- Fixture: `S = 0.7`, `K = (0.714286, 0.142857)`. -- 2-D: `S = [[0.7, 0.6], [0.6, 1.1]]`, `NIS = 0.934146`. -- Angle: residual `0.083185` (wrapped) against `−6.2` (raw). - -## Edge cases - -- `gate = ∞` (default) never gates. -- `nis` exactly at the gate is applied (`>` test).