diff --git a/README.md b/README.md index ec1a4167..3dfdd879 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, 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 | | [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, Madgwick/Mahony AHRS | +| [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, Madgwick/Mahony AHRS | | [Neural Network](doc/neural_network/README.md) | Layers, activations, losses, model | | [Optimization](doc/optimization/README.md) | Gradient Descent | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | diff --git a/ROADMAP.md b/ROADMAP.md index 7e3e1aa5..1382a3ed 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|------------------------------------------------------|---------------------------|------------| -| 39 | Square-root / Information Kalman filter | `filters/active` | ★★★★☆ | | 40 | Feedback linearization | `nonlinear_control` (new) | ★★★★☆ | | 41 | Backstepping controller | `nonlinear_control` (new) | ★★★★☆ | | 42 | Symmetric eigenvalue solver (Jacobi) | `solvers` | ★★★★★ | diff --git a/doc/filters/README.md b/doc/filters/README.md index eb6bdeba..77491c63 100644 --- a/doc/filters/README.md +++ b/doc/filters/README.md @@ -10,10 +10,11 @@ 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 | +| [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 | ### Passive Filters diff --git a/doc/filters/active/SquareRootKalmanFilter.md b/doc/filters/active/SquareRootKalmanFilter.md new file mode 100644 index 00000000..95614ee7 --- /dev/null +++ b/doc/filters/active/SquareRootKalmanFilter.md @@ -0,0 +1,144 @@ +# Square-Root Kalman Filter + +## Overview & Motivation + +Standard Kalman filter implementations propagate the error covariance matrix $P$ directly. On +resource-constrained hardware — where floating-point arithmetic is limited to 32 bits — roundoff +errors accumulate with every predict-update cycle. After many iterations $P$ can lose symmetry +or positive-definiteness, causing the filter to diverge catastrophically. + +The **square-root Kalman filter** eliminates this failure mode by never storing $P$ explicitly. +Instead it carries a Cholesky factor $S$ such that $P = S S^\top$, and rewrites every +predict/update operation as an orthogonal triangularization. Because orthogonal transforms +preserve matrix geometry exactly, the factor $S$ — and therefore the covariance $P = S S^\top$ +— remains symmetric positive-definite by construction even in single-precision arithmetic. + +A further benefit is numerical conditioning: $\mathrm{cond}(S) = \sqrt{\mathrm{cond}(P)}$, so +roughly twice as many significant bits survive across each filter step compared to the +conventional form. + +## Mathematical Theory + +### State-Space Model + +The underlying discrete-time linear system is identical to the conventional Kalman filter: + +$$x_k = F x_{k-1} + B u_{k-1} + w_{k-1}, \quad w \sim \mathcal{N}(0,Q)$$ +$$z_k = H x_k + v_k, \quad v \sim \mathcal{N}(0,R)$$ + +The square-root filter parameterises uncertainty through lower-triangular factors +$S$, $S_Q$, $S_R$ satisfying $P = S S^\top$, $Q = S_Q S_Q^\top$, $R = S_R S_R^\top$. + +### Predict Step (Time Update) + +The goal is to compute $S^-$ such that $P^- = F P F^\top + Q = S^- (S^-)^\top$ without +forming $P$ or $P^-$. + +Construct the $(2n \times n)$ pre-array: + +$$\mathcal{A} = \begin{bmatrix} (F S)^\top \\ S_Q^\top \end{bmatrix}$$ + +Apply a QR decomposition $\mathcal{A} = Q_\perp R$ (orthogonal $Q_\perp$, upper-triangular $R$). +Then $S^- = R^\top$ is the new lower-triangular factor, because: + +$$R^\top R = \mathcal{A}^\top \mathcal{A} = S^\top F^\top F S + S_Q^\top S_Q = F P F^\top + Q = P^-$$ + +### Update Step (Measurement Update) + +Construct the $((m+n) \times (m+n))$ pre-array: + +$$\mathcal{B} = \begin{bmatrix} S_R & H S \\ 0 & S \end{bmatrix}$$ + +Apply QR triangularization: $\mathcal{B} = Q_\perp \begin{bmatrix} S_y & \tilde{K} \\ 0 & S^+ \end{bmatrix}$ + +The block $S_y$ (upper-left, $m \times m$) is the innovation-covariance factor satisfying +$S_y S_y^\top = H P^- H^\top + R$. The block $S^+$ (lower-right, $n \times n$) is the +updated covariance factor. The Kalman gain emerges from the cross block: + +$$K = \tilde{K}^\top S_y^{-\top}$$ + +solved via a triangular back-substitution rather than an explicit matrix inverse. + +The state update is the standard correction: + +$$x^+ = x^- + K(z - H x^-)$$ + +### QR via Givens Rotations + +Both pre-arrays are triangularized by sequential Givens rotations applied column by column, +annihilating one sub-diagonal entry per rotation. This is cache-friendly, in-place, and +requires only $O(n^2)$ temporary storage — no heap allocation. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|---------|----------------|----------|--------------------------------------| +| Predict | $O(n^3)$ | $O(n^2)$ | Dominated by $F S$ multiply and QR | +| Update | $O((n+m) n^2)$ | $O(n^2)$ | QR on $(m+n) \times (m+n)$ pre-array | + +where $n =$ `StateSize`, $m =$ `MeasurementSize`. All arrays are fixed-size; no heap is used. + +## Step-by-Step Walkthrough + +Consider a 1-state system ($n=1$, $m=1$) with $F=1$, $H=1$, $S=0.5$, $S_Q=0.1$, $S_R=0.3$: + +**Predict:** + +Pre-array: $\mathcal{A} = [0.5;\; 0.1]$ (column vector, already a column). +$R = \sqrt{0.5^2 + 0.1^2} = \sqrt{0.26} \approx 0.5099$. +New factor $S^- = 0.5099$, i.e. $P^- \approx 0.26$. + +**Update** (measurement $z = 1.2$, prior $x^- = 1.0$): + +Pre-array $\mathcal{B} = \begin{bmatrix}0.3 & 0.5099 \\ 0 & 0.5099\end{bmatrix}$. +One Givens rotation annihilates $\mathcal{B}_{10}=0$, yielding upper-triangular form. +$S_y \approx 0.5974$, $S^+ \approx 0.424$, $K \approx 0.741$. +$x^+ = 1.0 + 0.741 \times 0.2 = 1.148$. + +## Pitfalls & Edge Cases + +- **Factor sign convention.** The lower-triangular factor returned by Cholesky has positive + diagonal; the QR step may flip signs. Diagonal entries should be taken as absolute values to + maintain the canonical lower-triangular form. +- **Near-zero $R$.** When measurement noise approaches zero the innovation-covariance factor + $S_y$ tends to zero; the triangular solve must be guarded against near-singular diagonals. +- **Near-zero $Q$.** With $S_Q = 0$ the predict step reduces the pre-array to a single block + $(F S)^\top$; the QR degenerates to a transpose — no numerical issue, covariance only shrinks. +- **Large initial uncertainty.** A near-singular or large $P_0$ with $\mathrm{cond}(P_0) \approx 10^8$ + is handled safely because $\mathrm{cond}(S_0) \approx 10^4$, which 32-bit floats can represent. + +## Variants & Generalizations + +- **Information filter (dual form).** Propagate a factor of $P^{-1}$ instead of $P$. + Cheaper when many measurements are fused per step; trivially encodes unknown initial state + as zero information. +- **Square-root UKF.** Apply the same QR trick to the UKF sigma-point covariance propagation, + replacing the weighted outer-product update with a rank-1 Cholesky update/downdate. +- **Potter / Carlson algorithms.** Older scalar measurement variants that process one + measurement component at a time; simpler but restricted to diagonal $R$. + +## Applications + +- **Multi-sensor fusion on microcontrollers** — the primary motivation; 32-bit floats survive + hundreds of filter steps without divergence. +- **INS/GNSS navigation** — long mission durations where a conventional filter would drift. +- **Safety-critical estimators** — provably PSD covariance at every step simplifies validation. +- **Battery state-of-charge estimation** — small $n$, constrained hardware, precision matters. + +## Connections to Other Algorithms + +- **Kalman Filter** — identical estimates; only the covariance bookkeeping differs. Use the + conventional form when precision is not a concern and code simplicity is preferred. +- **Cholesky Decomposition** — used to convert a full covariance $P_0$ into the initial factor + $S_0$ before constructing this filter. +- **QR Decomposition (Householder/Givens)** — the numerical engine of every predict/update step. +- **Unscented Kalman Filter** — a square-root variant exists (SR-UKF) that applies the same QR + trick to the sigma-point covariance propagation. + +## References & Further Reading + +- P. Kaminski, A. Bryson, S. Schmidt, "Discrete Square Root Filtering: A Survey of Current + Techniques," *IEEE Transactions on Automatic Control*, 16(6), 1971. +- R. van der Merwe and E. Wan, "The Square-Root Unscented Kalman Filter for State and + Parameter-Estimation," *ICASSP*, 2001. +- G. Bierman, *Factorization Methods for Discrete Sequential Estimation*, Academic Press, 1977. diff --git a/numerical/filters/active/CMakeLists.txt b/numerical/filters/active/CMakeLists.txt index 5691e3e5..2dedea98 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 + SquareRootKalmanFilter.hpp UnscentedKalmanFilter.hpp ) @@ -30,6 +31,7 @@ numerical_add_coverage_sources(numerical.filters.active KalmanFilter.cpp KalmanFilterBase.cpp KalmanSmoother.cpp + SquareRootKalmanFilter.cpp UnscentedKalmanFilter.cpp ) diff --git a/numerical/filters/active/SquareRootKalmanFilter.cpp b/numerical/filters/active/SquareRootKalmanFilter.cpp new file mode 100644 index 00000000..8296c02e --- /dev/null +++ b/numerical/filters/active/SquareRootKalmanFilter.cpp @@ -0,0 +1,8 @@ +#include "numerical/filters/active/SquareRootKalmanFilter.hpp" + +namespace filters +{ + template class SquareRootKalmanFilter; + template class SquareRootKalmanFilter; + template class SquareRootKalmanFilter; +} diff --git a/numerical/filters/active/SquareRootKalmanFilter.hpp b/numerical/filters/active/SquareRootKalmanFilter.hpp new file mode 100644 index 00000000..50746b49 --- /dev/null +++ b/numerical/filters/active/SquareRootKalmanFilter.hpp @@ -0,0 +1,225 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/filters/active/KalmanFilterBase.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/GivensRotation.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/TriangularSolve.hpp" +#include +#include +#include +#include + +namespace filters +{ + template + class SquareRootKalmanFilter + { + static_assert(std::is_floating_point_v, "SquareRootKalmanFilter supports floating-point types"); + static_assert(StateSize > 0, "StateSize must be positive"); + static_assert(MeasurementSize > 0, "MeasurementSize must be positive"); + + public: + using StateMatrix = math::SquareMatrix; + using StateVector = math::Vector; + using MeasurementMatrix = math::Matrix; + using MeasurementVector = math::Vector; + using MeasurementCovariance = math::SquareMatrix; + using KalmanGain = math::Matrix; + using ControlMatrix = typename detail::ControlType::Matrix; + using ControlVector = typename detail::ControlType::Vector; + + SquareRootKalmanFilter(const StateVector& initialState, const StateMatrix& initialFactor); + + void SetStateTransition(const StateMatrix& F); + void SetMeasurementMatrix(const MeasurementMatrix& H); + void SetProcessNoiseFactor(const StateMatrix& sqrtQ); + void SetMeasurementNoiseFactor(const MeasurementCovariance& sqrtR); + + void SetControlInputMatrix(const ControlMatrix& B) + requires(ControlSize > 0); + + OPTIMIZE_FOR_SPEED void Predict() + requires(ControlSize == 0); + + OPTIMIZE_FOR_SPEED void Predict(const ControlVector& u) + requires(ControlSize > 0); + + OPTIMIZE_FOR_SPEED void Update(const MeasurementVector& z); + + [[nodiscard]] const StateVector& GetState() const; + [[nodiscard]] StateMatrix GetCovariance() const; + [[nodiscard]] const StateMatrix& GetCovarianceFactor() const; + + private: + static constexpr std::size_t PredictRows = 2 * StateSize; + static constexpr std::size_t UpdatePreRows = MeasurementSize + StateSize; + + using PredictArray = math::Matrix; + using UpdateArray = math::Matrix; + + OPTIMIZE_FOR_SPEED void PredictInternal(); + + template + OPTIMIZE_FOR_SPEED static void QrTriangularize(math::Matrix& A); + + [[nodiscard]] OPTIMIZE_FOR_SPEED static StateMatrix LowerFactor(const StateMatrix& upper); + + StateVector state_; + StateMatrix factor_; + StateMatrix stateTransition_{ StateMatrix::Identity() }; + MeasurementMatrix measurementMatrix_{}; + StateMatrix sqrtQ_{ StateMatrix::Identity() }; + MeasurementCovariance sqrtR_{ MeasurementCovariance::Identity() }; + [[no_unique_address]] ControlMatrix controlInputMatrix_{}; + }; + + template + SquareRootKalmanFilter::SquareRootKalmanFilter( + const StateVector& initialState, const StateMatrix& initialFactor) + : state_(initialState) + , factor_(initialFactor) + {} + + template + void SquareRootKalmanFilter::SetStateTransition(const StateMatrix& F) + { + stateTransition_ = F; + } + + template + void SquareRootKalmanFilter::SetMeasurementMatrix(const MeasurementMatrix& H) + { + measurementMatrix_ = H; + } + + template + void SquareRootKalmanFilter::SetProcessNoiseFactor(const StateMatrix& sqrtQ) + { + sqrtQ_ = sqrtQ; + } + + template + void SquareRootKalmanFilter::SetMeasurementNoiseFactor(const MeasurementCovariance& sqrtR) + { + sqrtR_ = sqrtR; + } + + template + void SquareRootKalmanFilter::SetControlInputMatrix(const ControlMatrix& B) + requires(ControlSize > 0) + { + controlInputMatrix_ = B; + } + + template + template + OPTIMIZE_FOR_SPEED void SquareRootKalmanFilter::QrTriangularize( + math::Matrix& A) + { + for (std::size_t col = 0; col < Cols; ++col) + { + for (std::size_t row = Rows - 1; row > col; --row) + { + auto g = math::ComputeGivens(A.at(row - 1, col), A.at(row, col)); + for (std::size_t k = 0; k < Cols; ++k) + math::ApplyGivens(g, A.at(row - 1, k), A.at(row, k)); + } + } + } + + template + OPTIMIZE_FOR_SPEED typename SquareRootKalmanFilter::StateMatrix + SquareRootKalmanFilter::LowerFactor(const StateMatrix& upper) + { + auto lower = upper.Transpose(); + + for (std::size_t i = 0; i < StateSize; ++i) + for (std::size_t j = i + 1; j < StateSize; ++j) + lower.at(i, j) = T{}; + + return lower; + } + + template + OPTIMIZE_FOR_SPEED void SquareRootKalmanFilter::PredictInternal() + { + PredictArray A{}; + A.SetBlock((stateTransition_ * factor_).Transpose(), 0, 0); + A.SetBlock(sqrtQ_.Transpose(), StateSize, 0); + + QrTriangularize(A); + + factor_ = LowerFactor(A.template GetBlock(0, 0)); + } + + template + OPTIMIZE_FOR_SPEED void SquareRootKalmanFilter::Predict() + requires(ControlSize == 0) + { + state_ = stateTransition_ * state_; + PredictInternal(); + } + + template + OPTIMIZE_FOR_SPEED void SquareRootKalmanFilter::Predict(const ControlVector& u) + requires(ControlSize > 0) + { + state_ = stateTransition_ * state_ + controlInputMatrix_ * u; + PredictInternal(); + } + + template + OPTIMIZE_FOR_SPEED void SquareRootKalmanFilter::Update(const MeasurementVector& z) + { + UpdateArray A{}; + A.SetBlock(sqrtR_.Transpose(), 0, 0); + A.SetBlock((measurementMatrix_ * factor_).Transpose(), MeasurementSize, 0); + A.SetBlock(factor_.Transpose(), MeasurementSize, MeasurementSize); + + QrTriangularize(A); + + auto Sy = A.template GetBlock(0, 0); + auto cross = A.template GetBlock(0, MeasurementSize); + + KalmanGain K{}; + for (std::size_t j = 0; j < StateSize; ++j) + K.SetBlock(math::SolveUpperTriangular(Sy, cross.GetColumn(j)).Transpose(), j, 0); + + auto innov = z - measurementMatrix_ * state_; + state_ = state_ + K * innov; + + factor_ = LowerFactor(A.template GetBlock(MeasurementSize, MeasurementSize)); + } + + template + const typename SquareRootKalmanFilter::StateVector& + SquareRootKalmanFilter::GetState() const + { + return state_; + } + + template + typename SquareRootKalmanFilter::StateMatrix + SquareRootKalmanFilter::GetCovariance() const + { + return factor_ * factor_.Transpose(); + } + + template + const typename SquareRootKalmanFilter::StateMatrix& + SquareRootKalmanFilter::GetCovarianceFactor() const + { + return factor_; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class SquareRootKalmanFilter; + extern template class SquareRootKalmanFilter; + extern template class SquareRootKalmanFilter; +#endif +} diff --git a/numerical/filters/active/test/CMakeLists.txt b/numerical/filters/active/test/CMakeLists.txt index 1896a1f7..6c1cc174 100644 --- a/numerical/filters/active/test/CMakeLists.txt +++ b/numerical/filters/active/test/CMakeLists.txt @@ -14,5 +14,6 @@ target_sources(numerical.filters.active_test PRIVATE TestExtendedKalmanFilter.cpp TestKalmanFilter.cpp TestKalmanSmoother.cpp + TestSquareRootKalmanFilter.cpp TestUnscentedKalmanFilter.cpp ) diff --git a/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp b/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp new file mode 100644 index 00000000..ec55e916 --- /dev/null +++ b/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp @@ -0,0 +1,349 @@ +#include "numerical/filters/active/KalmanFilter.hpp" +#include "numerical/filters/active/SquareRootKalmanFilter.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/solvers/CholeskyDecomposition.hpp" +#include +#include + +namespace +{ + constexpr float dt = 0.1f; + + using SrkfType = filters::SquareRootKalmanFilter; + using KfType = filters::KalmanFilter; + using StateVec = math::Vector; + using StateMat = math::SquareMatrix; + using MeasVec = math::Vector; + using MeasMat = math::Matrix; + using MeasCov = math::SquareMatrix; + + StateMat MakeF() + { + return StateMat{ + { 1.0f, dt }, + { 0.0f, 1.0f } + }; + } + + MeasMat MakeH() + { + return MeasMat{ { 1.0f, 0.0f } }; + } + + StateMat MakeQ() + { + return StateMat{ + { 0.01f, 0.0f }, + { 0.0f, 0.01f } + }; + } + + MeasCov MakeR() + { + return MeasCov{ { 0.5f } }; + } + + StateVec MakeX0() + { + return StateVec{ { 0.0f }, { 0.0f } }; + } + + StateMat MakeP0() + { + return StateMat{ + { 1.0f, 0.0f }, + { 0.0f, 1.0f } + }; + } + + float MatrixTrace(const StateMat& m) + { + return m.at(0, 0) + m.at(1, 1); + } + + bool IsPositiveDefinite(const StateMat& m) + { + float d00 = m.at(0, 0); + float det = m.at(0, 0) * m.at(1, 1) - m.at(0, 1) * m.at(1, 0); + return d00 > 0.0f && det > 0.0f; + } + + class TestSquareRootKalmanFilter + : public ::testing::Test + { + protected: + StateMat S0 = solvers::CholeskyDecomposition(MakeP0()); + + SrkfType filter{ MakeX0(), S0 }; + KfType reference{ MakeX0(), MakeP0() }; + + void SetupBothFilters() + { + filter.SetStateTransition(MakeF()); + filter.SetMeasurementMatrix(MakeH()); + filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + + reference.SetStateTransition(MakeF()); + reference.SetMeasurementMatrix(MakeH()); + reference.SetProcessNoise(MakeQ()); + reference.SetMeasurementNoise(MakeR()); + } + }; +} + +TEST_F(TestSquareRootKalmanFilter, matches_conventional_kalman) +{ + SetupBothFilters(); + + float pos = 0.0f; + float vel = 0.5f; + + for (int step = 0; step < 10; ++step) + { + pos += vel * dt; + MeasVec z{ { pos } }; + + filter.Predict(); + filter.Update(z); + + reference.Predict(); + reference.Update(z); + } + + auto sx = filter.GetState(); + auto rx = reference.GetState(); + EXPECT_NEAR(sx.at(0, 0), rx.at(0, 0), 5e-3f); + EXPECT_NEAR(sx.at(1, 0), rx.at(1, 0), 5e-3f); + + auto sP = filter.GetCovariance(); + auto rP = reference.GetCovariance(); + EXPECT_NEAR(sP.at(0, 0), rP.at(0, 0), 5e-3f); + EXPECT_NEAR(sP.at(1, 1), rP.at(1, 1), 5e-3f); +} + +TEST_F(TestSquareRootKalmanFilter, covariance_stays_positive_definite) +{ + SetupBothFilters(); + + float pos = 0.0f; + float vel = 0.3f; + + for (int step = 0; step < 30; ++step) + { + pos += vel * dt; + MeasVec z{ { pos } }; + + filter.Predict(); + filter.Update(z); + + auto P = filter.GetCovariance(); + EXPECT_TRUE(IsPositiveDefinite(P)); + } +} + +TEST_F(TestSquareRootKalmanFilter, factor_reconstructs_covariance) +{ + SetupBothFilters(); + + MeasVec z{ { 1.0f } }; + filter.Predict(); + filter.Update(z); + filter.Predict(); + filter.Update(z); + + auto S = filter.GetCovarianceFactor(); + auto P = filter.GetCovariance(); + auto SSt = S * S.Transpose(); + + EXPECT_NEAR(SSt.at(0, 0), P.at(0, 0), math::Tolerance()); + EXPECT_NEAR(SSt.at(0, 1), P.at(0, 1), math::Tolerance()); + EXPECT_NEAR(SSt.at(1, 0), P.at(1, 0), math::Tolerance()); + EXPECT_NEAR(SSt.at(1, 1), P.at(1, 1), math::Tolerance()); +} + +TEST_F(TestSquareRootKalmanFilter, predict_grows_uncertainty) +{ + filter.SetStateTransition(MakeF()); + filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + + float traceBefore = MatrixTrace(filter.GetCovariance()); + filter.Predict(); + float traceAfter = MatrixTrace(filter.GetCovariance()); + + EXPECT_GT(traceAfter, traceBefore); +} + +TEST_F(TestSquareRootKalmanFilter, update_shrinks_uncertainty) +{ + SetupBothFilters(); + + filter.Predict(); + float traceAfterPredict = MatrixTrace(filter.GetCovariance()); + + MeasVec z{ { 0.05f } }; + filter.Update(z); + float traceAfterUpdate = MatrixTrace(filter.GetCovariance()); + + EXPECT_LT(traceAfterUpdate, traceAfterPredict); +} + +TEST_F(TestSquareRootKalmanFilter, ill_conditioned_stays_stable) +{ + StateMat P0ill{ + { 1e8f, 0.0f }, + { 0.0f, 1.0f } + }; + StateMat S0ill = solvers::CholeskyDecomposition(P0ill); + + SrkfType illFilter{ MakeX0(), S0ill }; + illFilter.SetStateTransition(MakeF()); + illFilter.SetMeasurementMatrix(MakeH()); + illFilter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + illFilter.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + + for (int step = 0; step < 20; ++step) + { + MeasVec z{ { float(step) * 0.05f } }; + illFilter.Predict(); + illFilter.Update(z); + } + + auto x = illFilter.GetState(); + auto P = illFilter.GetCovariance(); + + EXPECT_TRUE(std::isfinite(x.at(0, 0))); + EXPECT_TRUE(std::isfinite(x.at(1, 0))); + EXPECT_TRUE(IsPositiveDefinite(P)); +} + +TEST_F(TestSquareRootKalmanFilter, perfect_measurement_collapses_variance) +{ + MeasCov sqrtRtiny{ { 1e-5f } }; + + filter.SetStateTransition(MakeF()); + filter.SetMeasurementMatrix(MakeH()); + filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetMeasurementNoiseFactor(sqrtRtiny); + + filter.Predict(); + MeasVec z{ { 0.5f } }; + filter.Update(z); + + auto P = filter.GetCovariance(); + EXPECT_LT(P.at(0, 0), 1e-4f); + EXPECT_TRUE(std::isfinite(filter.GetState().at(0, 0))); +} + +TEST_F(TestSquareRootKalmanFilter, vague_measurement_is_ignored) +{ + MeasCov sqrtRlarge{ { 1e4f } }; + + filter.SetStateTransition(MakeF()); + filter.SetMeasurementMatrix(MakeH()); + filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetMeasurementNoiseFactor(sqrtRlarge); + + filter.Predict(); + auto xPred = filter.GetState(); + + MeasVec z{ { 999.0f } }; + filter.Update(z); + auto xPost = filter.GetState(); + + EXPECT_NEAR(xPost.at(0, 0), xPred.at(0, 0), 0.1f); + EXPECT_NEAR(xPost.at(1, 0), xPred.at(1, 0), 0.1f); +} + +TEST_F(TestSquareRootKalmanFilter, steady_state_gain_converges) +{ + SetupBothFilters(); + + float tracePrev = 1e30f; + float traceConverged = 0.0f; + int convergedSteps = 0; + + for (int step = 0; step < 100; ++step) + { + MeasVec z{ { 1.0f } }; + filter.Predict(); + filter.Update(z); + + float traceNow = MatrixTrace(filter.GetCovariance()); + float diff = std::abs(traceNow - tracePrev); + if (diff < 1e-6f) + ++convergedSteps; + else + convergedSteps = 0; + + tracePrev = traceNow; + if (convergedSteps >= 5) + { + traceConverged = traceNow; + break; + } + } + + EXPECT_GT(convergedSteps, 0); + EXPECT_GT(traceConverged, 0.0f); +} + +TEST_F(TestSquareRootKalmanFilter, control_input_matches_conventional_kalman) +{ + using SrkfCtrl = filters::SquareRootKalmanFilter; + using KfCtrl = filters::KalmanFilter; + using CtrlMat = math::Matrix; + using CtrlVec = math::Vector; + + CtrlMat B{ { 0.5f * dt * dt }, { dt } }; + + SrkfCtrl srkf{ MakeX0(), S0 }; + srkf.SetStateTransition(MakeF()); + srkf.SetMeasurementMatrix(MakeH()); + srkf.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + srkf.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + srkf.SetControlInputMatrix(B); + + KfCtrl reference2{ MakeX0(), MakeP0() }; + reference2.SetStateTransition(MakeF()); + reference2.SetMeasurementMatrix(MakeH()); + reference2.SetProcessNoise(MakeQ()); + reference2.SetMeasurementNoise(MakeR()); + reference2.SetControlInputMatrix(B); + + CtrlVec u{ { 1.0f } }; + float pos = 0.0f; + float vel = 0.0f; + + for (int step = 0; step < 10; ++step) + { + vel += dt; + pos += vel * dt; + MeasVec z{ { pos } }; + + srkf.Predict(u); + srkf.Update(z); + + reference2.Predict(u); + reference2.Update(z); + } + + auto sx = srkf.GetState(); + auto rx = reference2.GetState(); + EXPECT_NEAR(sx.at(0, 0), rx.at(0, 0), 5e-3f); + EXPECT_NEAR(sx.at(1, 0), rx.at(1, 0), 5e-3f); +} + +TEST_F(TestSquareRootKalmanFilter, reset_and_getters) +{ + auto x = filter.GetState(); + auto S = filter.GetCovarianceFactor(); + + EXPECT_NEAR(x.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 0.0f, math::Tolerance()); + + EXPECT_NEAR(S.at(0, 0), S0.at(0, 0), math::Tolerance()); + EXPECT_NEAR(S.at(1, 0), S0.at(1, 0), math::Tolerance()); + EXPECT_NEAR(S.at(0, 1), S0.at(0, 1), math::Tolerance()); + EXPECT_NEAR(S.at(1, 1), S0.at(1, 1), math::Tolerance()); +} diff --git a/roadmap/filters/active/SquareRootKalmanFilter/explanation.md b/roadmap/filters/active/SquareRootKalmanFilter/explanation.md deleted file mode 100644 index f4b8b58a..00000000 --- a/roadmap/filters/active/SquareRootKalmanFilter/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Square-Root / Information Kalman Filter — Overview - -## What it is -A numerically robust reformulation of the Kalman filter that never stores the covariance `P` -directly. Instead it propagates a **Cholesky/QR factor** `S` such that `P = S·Sᵀ` (the *square-root* -form) — or a factor of the inverse `P⁻¹` (the *information* form). The estimates are identical to a -textbook KF; only the bookkeeping changes. - -## Why it matters (embedded) -On reduced-precision hardware a conventional Kalman filter can accumulate round-off until `P` loses -symmetry or positive-definiteness and the filter diverges. Working with a factor makes -`P = S·Sᵀ` **positive-definite by construction** and roughly **halves the condition number**, so the -same result survives in far fewer bits. That robustness is exactly what multi-sensor fusion on an -MCU or a safety-critical estimator needs. - -## How it works (intuition) -Every predict/update is rewritten as an **orthogonal triangularization** (a QR/Givens step) of a -small stacked "pre-array". Orthogonal transforms preserve the covariance's geometry while returning -a fresh triangular factor — you get the new `S` without ever squaring numbers up into `F·P·Fᵀ`, -which is where a naive filter loses precision. The **information form** is the mirror image: it -carries a factor of `P⁻¹`, which is cheaper when you fuse many measurements at once and lets a -totally-unknown initial state be written simply as "zero information". - -## Key parameters -- **S0 = chol(P0)** — initial covariance factor (the seeded uncertainty). -- **sqrtQ, sqrtR** — square-root factors of the process- and measurement-noise covariances. -- **F, H (, B)** — the usual state-space model matrices. -- **form** — square-root (covariance) vs information (inverse-covariance) duality. - -## Reference -P. Kaminski, A. Bryson, S. Schmidt, "Discrete Square Root Filtering: A Survey of Current -Techniques," *IEEE Trans. Automatic Control*, 16(6), 1971. - -## See also -`KalmanFilter` (the conventional covariance form), `CholeskyDecomposition` and `QrDecomposition` -(item 27, the numerical engine), `ExtendedKalmanFilter` (nonlinear extension). diff --git a/roadmap/filters/active/SquareRootKalmanFilter/implementation.md b/roadmap/filters/active/SquareRootKalmanFilter/implementation.md deleted file mode 100644 index 8a84e14e..00000000 --- a/roadmap/filters/active/SquareRootKalmanFilter/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Square-Root / Information Kalman Filter — Implementation Pseudocode - -> Roadmap ref: #39 (Tier 4) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SquareRootKalmanFilter: - Vector x # state estimate - SquareMatrix S # Cholesky factor: P = S * Sᵀ (lower-tri) - SquareMatrix sqrtQ # process-noise factor - SquareMatrix sqrtR # measurement-noise factor - Matrix F # state transition - Matrix H # measurement model - # (control B when ControlSize > 0) -``` - -## Interface - -``` -SquareRootKalmanFilter(Vector x0, SquareMatrix S0) # S0 = chol(P0) -void SetStateTransition(F) / SetMeasurementMatrix(H) -void SetProcessNoiseFactor(sqrtQ) / SetMeasurementNoiseFactor(sqrtR) -void Predict() / Predict(Vector u) requires ControlSize > 0 -void Update(Vector z) -Vector State() const -SquareMatrix Covariance() const # reconstruct S * Sᵀ on demand -SquareMatrix CovarianceFactor() const # return S directly -``` - -## Algorithm (pseudocode) - -``` -function Predict(): # OPTIMIZE_FOR_SPEED (time update via QR) - x = F * x (+ B * u) - # Triangularize the pre-array so P⁻ = S⁻ S⁻ᵀ = F P Fᵀ + Q, never forming P explicitly: - A = [ (F * S)ᵀ ; sqrtQᵀ ] # (2n x n) stacked block - R = QrTriangularize(A) # Householder/Givens -> upper-tri R (item 27) - S = Rᵀ # new lower-tri covariance factor - -function Update(z): # OPTIMIZE_FOR_SPEED (measurement update via QR) - # Pre-array mixes measurement and state factors: - # [ sqrtR H * S ] QR [ Sy K̃ ] - # [ 0 S ] ───────────> [ 0 S⁺ ] - Pre = [ [ sqrtR , H*S ] ; [ 0 , S ] ] - Post = QrTriangularize(Pre) - Sy = Post.topLeft(MeasurementSize) # innovation-covariance factor - Ktilde = Post.topRight() # cross block - S = Post.bottomRight(StateSize) # updated factor, guaranteed lower-tri / PSD - innov = z - H * x - K = Ktilde * Inverse(Sy) # gain via triangular solve (Sy is triangular) - x = x + K * innov -``` - -## Complexity & memory - -- Time: `O((n+m)·n²)` per step, dominated by the QR triangularization (`n = StateSize`, `m = MeasurementSize`). -- Memory: `O(n²)` — all factors and pre-arrays are fixed-size `std::array`-backed `math::Matrix`; no heap. - -## Numerical / embedded notes - -- Propagating a **factor** `S` (never `P`) guarantees `P = S Sᵀ` stays symmetric positive-definite - even in reduced precision — the failure mode that makes a conventional KF "blow up" cannot occur. -- The **effective condition number is halved**: `cond(S) = sqrt(cond(P))`, so more state bits survive. -- Build QR from **Givens rotations** (structured, in-place) or Householder (item 27); both avoid the - squaring of dynamic range inherent in forming `F P Fᵀ`. -- **Information form** is the dual: propagate a factor of `P⁻¹` instead, which is cheaper when many - measurements are fused per step and lets an "uninitialized" state be encoded as zero information. -- Reuses `KalmanFilterBase` (state/noise storage), `CholeskyDecomposition`, and QR (item 27); - `Predict`/`Update` carry `OPTIMIZE_FOR_SPEED`. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/filters/active/SquareRootKalmanFilter.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Predict`/`Update`, and - `extern template class SquareRootKalmanFilter;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/filters/active/SquareRootKalmanFilter.cpp` → - `template class SquareRootKalmanFilter;` -- Test: `numerical/filters/active/test/TestSquareRootKalmanFilter.cpp` -- Doc: `doc/filters/active/SquareRootKalmanFilter.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestSquareRootKalmanFilter.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/filters/active/SquareRootKalmanFilter/tests.md b/roadmap/filters/active/SquareRootKalmanFilter/tests.md deleted file mode 100644 index fd5eaebd..00000000 --- a/roadmap/filters/active/SquareRootKalmanFilter/tests.md +++ /dev/null @@ -1,70 +0,0 @@ -# Square-Root / Information Kalman Filter — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSquareRootKalmanFilter : public ::testing::Test: - # 2-state constant-velocity model, scalar position measurement - SquareRootKalmanFilter filter{ x0, cholesky(P0) } - # reference for cross-checks: - KalmanFilter reference{ x0, P0 } -# each case below is a TEST_F(TestSquareRootKalmanFilter, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -matches_conventional_kalman: - Arrange: identical F, H, Q, R and measurement stream fed to both filters - Act: Predict + Update each step - Assert: State() and Covariance() ≈ reference within tight tolerance - -covariance_stays_positive_definite: - Arrange: run many predict/update cycles - Assert: cholesky(Covariance()) succeeds every step (all pivots > 0) - -factor_reconstructs_covariance: - Arrange: after several updates - Assert: CovarianceFactor() * CovarianceFactorᵀ ≈ Covariance() - -predict_grows_uncertainty: - Arrange: call Predict() with no measurement - Assert: trace(Covariance()) increases by ≈ trace(Q) - -update_shrinks_uncertainty: - Arrange: Predict then Update(z) - Assert: trace(Covariance()) decreases - -ill_conditioned_stays_stable: - Arrange: P0 with cond ≈ 1e8 (a conventional KF would lose PSD) - Assert: filter remains PSD and finite (no NaN), tracks truth - -perfect_measurement_collapses_variance: - Arrange: R -> 0 on the measured axis - Assert: posterior variance of that axis -> ~0, no divide blow-up - -vague_measurement_is_ignored: - Arrange: R very large - Assert: State() ≈ prediction (gain -> 0) - -steady_state_gain_converges: - Arrange: time-invariant system, iterate - Assert: gain and Covariance() converge to fixed values - -reset_and_getters: - Assert: State()/CovarianceFactor() return the constructor-seeded values -``` - -## Reference vectors - -- Cross-checked against `KalmanFilter` (item 39 reuses `KalmanFilterBase`) on a 2-state CV model. -- Scalar steady-state: converged variance matches the algebraic Riccati fixed point. - -## Edge cases - -- `Q = 0` (no process noise) ⇒ covariance only shrinks, factor stays lower-triangular. -- `R -> 0` (perfect sensor) and `R -> ∞` (useless sensor) ⇒ well-defined limits, no blow-up. -- 1-state scalar filter ⇒ QR reduces to a single Givens rotation; matches closed form. -- Near-singular `P0` ⇒ square-root path keeps eigenvalues ≥ 0 where the conventional form fails.