From 0b08891cf2f9284235fe9e6631fd9c37cb1111e3 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 2 Aug 2026 06:19:23 +0000 Subject: [PATCH 1/5] add tls --- README.md | 2 +- doc/estimators/README.md | 1 + doc/estimators/TotalLeastSquares.md | 92 +++++++++++ numerical/estimators/offline/CMakeLists.txt | 2 + .../estimators/offline/TotalLeastSquares.cpp | 7 + .../estimators/offline/TotalLeastSquares.hpp | 85 +++++++++++ .../estimators/offline/test/CMakeLists.txt | 1 + .../offline/test/TestTotalLeastSquares.cpp | 143 ++++++++++++++++++ .../solvers/SingularValueDecomposition.hpp | 4 +- 9 files changed, 335 insertions(+), 2 deletions(-) create mode 100644 doc/estimators/TotalLeastSquares.md create mode 100644 numerical/estimators/offline/TotalLeastSquares.cpp create mode 100644 numerical/estimators/offline/TotalLeastSquares.hpp create mode 100644 numerical/estimators/offline/test/TestTotalLeastSquares.cpp diff --git a/README.md b/README.md index 2bf3218a..f3d09c47 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic 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 | +| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, 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, 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 | diff --git a/doc/estimators/README.md b/doc/estimators/README.md index c43d64bd..08814c0f 100644 --- a/doc/estimators/README.md +++ b/doc/estimators/README.md @@ -8,6 +8,7 @@ Statistical estimation algorithms for fitting models to observed data and making |--------------------------------------------------------|---------------------------------------------------------------------------| | [Linear Regression](LinearRegression.md) | Ordinary least-squares regression using the normal equation | | [Polynomial Fitting](PolynomialFitting.md) | Degree-d polynomial fit via Vandermonde normal equations | +| [Total Least Squares](TotalLeastSquares.md) | Errors-in-variables fit (noisy regressors) via the SVD of `[A | b]` | | [Yule-Walker](YuleWalker.md) | Autoregressive model parameter estimation via the Yule-Walker equations | | [Expectation-Maximization](ExpectationMaximization.md) | EM algorithm for Kalman filter parameter identification (Shumway-Stoffer) | diff --git a/doc/estimators/TotalLeastSquares.md b/doc/estimators/TotalLeastSquares.md new file mode 100644 index 00000000..76bd988d --- /dev/null +++ b/doc/estimators/TotalLeastSquares.md @@ -0,0 +1,92 @@ +# Total Least Squares + +## Overview & Motivation + +Ordinary least squares (OLS) assumes the regressors are known exactly and only the response is +noisy — it minimizes the *vertical* residuals. Real embedded systems violate that assumption: +calibrating one measured quantity against another (two drifting sensors, current vs. torque, +strain vs. force) is an **errors-in-variables** problem where *every* channel is noisy. Fitting +such data with OLS produces a systematically **biased** (attenuated) slope, a phenomenon known as +regression dilution. + +Total Least Squares (TLS) removes that bias. Geometrically it minimizes the **orthogonal** +(perpendicular) distance from each point to the fitted hyperplane rather than the vertical +distance, which is the maximum-likelihood estimate when the noise is equal on all channels. + +## Mathematical Theory + +### The Model + +Given a design matrix $A \in \mathbb{R}^{m \times n}$ and a response $b \in \mathbb{R}^{m}$, TLS +seeks the smallest perturbation $[\,\Delta A \mid \Delta b\,]$ (in Frobenius norm) that makes the +system consistent: + +$$\min_{\Delta A,\ \Delta b} \bigl\|[\,\Delta A \mid \Delta b\,]\bigr\|_F +\quad\text{s.t.}\quad (A + \Delta A)\,x = b + \Delta b$$ + +### SVD Solution + +Stack the data into the augmented matrix $M = [\,A \mid b\,] \in \mathbb{R}^{m \times (n+1)}$ and +take its singular value decomposition $M = U \Sigma V^{\top}$. The right-singular vector +$v_{n+1}$ associated with the **smallest** singular value $\sigma_{n+1}$ spans the direction of +least variance — the normal of the best-fit hyperplane. Partitioning + +$$v_{n+1} = \begin{bmatrix} v_{1:n} \\ v_{n+1,\,n+1} \end{bmatrix}, +\qquad x = -\,\frac{v_{1:n}}{v_{n+1,\,n+1}}$$ + +recovers the coefficients. The construction is exactly the Golub–Van Loan (1980) result: the +minimal perturbation is $\sigma_{n+1}\, u_{n+1} v_{n+1}^{\top}$, and $\sigma_{n+1}$ is the +orthogonal residual norm. + +### Existence + +The solution exists (is *generic*) only when the last entry $v_{n+1,\,n+1} \neq 0$. If it vanishes, +the smallest singular direction lies entirely in the column space of $A$ (e.g. a rank-deficient or +all-zero regressor column) and no finite coefficient vector satisfies the fit — `Fit` returns +`false`. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|---------|---------------------|------------------|---------------------------------------------------| +| Best | $O(m\,n^2)$ | $O(m\,n)$ | Golub–Kahan bidiagonalization dominates | +| Average | $O(m\,n^2)$ | $O(m\,n)$ | Implicit-QR sweeps converge in $O(n)$ per value | +| Worst | $O(m\,n^2)$ | $O(m\,n)$ | Bounded iteration cap in the SVD engine | + +All storage is stack-allocated (`std::array`-backed `math::Matrix`); no heap, no recursion. + +## Step-by-Step Walkthrough + +Fit $b = 2a$ from four clean points $a = (1,2,3,4)$, $b = (2,4,6,8)$. + +1. Augment: $M = \begin{bmatrix} 1 & 2 \\ 2 & 4 \\ 3 & 6 \\ 4 & 8 \end{bmatrix}$ — exactly rank 1. +2. SVD gives $\sigma_1 \approx 12.25$, $\sigma_2 = 0$ (the data hugs a line). +3. The smallest right-singular vector is $v_2 = \tfrac{1}{\sqrt5}(2, -1)$. +4. Partition: $x = -\,v_{2,1} / v_{2,2} = -\,\tfrac{2/\sqrt5}{-1/\sqrt5} = 2$. ✓ + +## Pitfalls & Edge Cases + +- **Column scaling.** The SVD is dominated by the largest-norm column. Scale the columns of + `A` and `b` to comparable magnitudes before fitting, otherwise the TLS/OLS distinction is lost. +- **Singular-value gap.** A tiny gap between the two smallest singular values signals an ill-posed + (near-degenerate) fit; the recovered coefficients become sensitive to noise. +- **Degeneracy.** An all-zero or linearly dependent regressor column drives the last entry of the + smallest singular vector to zero; `Fit` reports `false` rather than dividing by zero. +- **Minimal system.** Requires `Samples >= Features + 1` (enforced by `static_assert`). + +## Variants & Generalizations + +- **Weighted / generalized TLS** — scale rows/columns by known noise covariances before the SVD. +- **Regularized (truncated) TLS** — discard trailing singular directions for ill-conditioned data. +- **OLS counterparts** — [Linear Regression](LinearRegression.md) and + [Polynomial Fitting](PolynomialFitting.md) solve the noise-free-regressor case. + +## Reference + +G. H. Golub, C. F. Van Loan, "An Analysis of the Total Least Squares Problem," +*SIAM J. Numer. Anal.*, 17(6), 1980. + +## See Also + +- [`SingularValueDecomposition`](../solvers/SingularValueDecomposition.md) — the numerical engine. +- [Linear Regression](LinearRegression.md), [Polynomial Fitting](PolynomialFitting.md) — OLS fits. diff --git a/numerical/estimators/offline/CMakeLists.txt b/numerical/estimators/offline/CMakeLists.txt index 4672b5d8..e20be88d 100644 --- a/numerical/estimators/offline/CMakeLists.txt +++ b/numerical/estimators/offline/CMakeLists.txt @@ -16,6 +16,7 @@ target_sources(numerical.estimators.offline PRIVATE ExpectationMaximization.hpp LinearRegression.hpp PolynomialFitting.hpp + TotalLeastSquares.hpp YuleWalker.hpp ) @@ -23,6 +24,7 @@ numerical_add_coverage_sources(numerical.estimators.offline ExpectationMaximization.cpp LinearRegression.cpp PolynomialFitting.cpp + TotalLeastSquares.cpp YuleWalker.cpp ) diff --git a/numerical/estimators/offline/TotalLeastSquares.cpp b/numerical/estimators/offline/TotalLeastSquares.cpp new file mode 100644 index 00000000..ee5a16e6 --- /dev/null +++ b/numerical/estimators/offline/TotalLeastSquares.cpp @@ -0,0 +1,7 @@ +#include "numerical/estimators/offline/TotalLeastSquares.hpp" + +namespace estimators +{ + template class TotalLeastSquares; + template class TotalLeastSquares; +} diff --git a/numerical/estimators/offline/TotalLeastSquares.hpp b/numerical/estimators/offline/TotalLeastSquares.hpp new file mode 100644 index 00000000..31658eee --- /dev/null +++ b/numerical/estimators/offline/TotalLeastSquares.hpp @@ -0,0 +1,85 @@ +#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/SingularValueDecomposition.hpp" +#include +#include +#include + +namespace estimators +{ + template + class TotalLeastSquares + { + static_assert(std::is_floating_point_v, "TotalLeastSquares supports floating-point types only"); + static_assert(Samples >= Features + 1, "Samples must be >= Features + 1"); + + public: + using CoefficientsVector = math::Vector; + using DesignMatrix = math::Matrix; + using SamplesVector = math::Vector; + using FeaturesVector = math::Vector; + + TotalLeastSquares() = default; + + OPTIMIZE_FOR_SPEED bool Fit(const DesignMatrix& a, const SamplesVector& b); + T Predict(const FeaturesVector& x) const; + const CoefficientsVector& Coefficients() const; + + private: + static constexpr std::size_t Augmented = Features + 1; + + CoefficientsVector coefficients{}; + }; + + template + OPTIMIZE_FOR_SPEED bool TotalLeastSquares::Fit(const DesignMatrix& a, const SamplesVector& b) + { + math::Matrix m; + for (std::size_t i = 0; i < Samples; ++i) + { + for (std::size_t j = 0; j < Features; ++j) + m.at(i, j) = a.at(i, j); + m.at(i, Features) = b.at(i, 0); + } + + solvers::SingularValueDecomposition svd; + svd.Decompose(m); + + const auto& v = svd.V(); + T denom = v.at(Features, Features); + if (std::abs(denom) < T{ 1e-6f }) + return false; + + for (std::size_t i = 0; i < Features; ++i) + coefficients.at(i, 0) = -v.at(i, Features) / denom; + + return true; + } + + template + T TotalLeastSquares::Predict(const FeaturesVector& x) const + { + T acc{}; + for (std::size_t i = 0; i < Features; ++i) + acc += coefficients.at(i, 0) * x.at(i, 0); + return acc; + } + + template + const typename TotalLeastSquares::CoefficientsVector& + TotalLeastSquares::Coefficients() const + { + return coefficients; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class TotalLeastSquares; + extern template class TotalLeastSquares; +#endif +} diff --git a/numerical/estimators/offline/test/CMakeLists.txt b/numerical/estimators/offline/test/CMakeLists.txt index 249c26fb..9f12550f 100644 --- a/numerical/estimators/offline/test/CMakeLists.txt +++ b/numerical/estimators/offline/test/CMakeLists.txt @@ -11,5 +11,6 @@ target_sources(numerical.estimators.offline_test PRIVATE TestExpectationMaximization.cpp TestLinearRegression.cpp TestPolynomialFitting.cpp + TestTotalLeastSquares.cpp TestYuleWalker.cpp ) diff --git a/numerical/estimators/offline/test/TestTotalLeastSquares.cpp b/numerical/estimators/offline/test/TestTotalLeastSquares.cpp new file mode 100644 index 00000000..c3358a2c --- /dev/null +++ b/numerical/estimators/offline/test/TestTotalLeastSquares.cpp @@ -0,0 +1,143 @@ +#include "numerical/estimators/offline/TotalLeastSquares.hpp" +#include "numerical/math/Tolerance.hpp" +#include +#include + +namespace +{ + class TestTotalLeastSquares : public ::testing::Test + { + protected: + estimators::TotalLeastSquares tls; + estimators::TotalLeastSquares tls2; + }; +} + +TEST_F(TestTotalLeastSquares, recovers_exact_line_no_noise) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 8; ++i) + { + float ai = 1.0f + static_cast(i); + a.at(i, 0) = ai; + b.at(i, 0) = 2.0f * ai; + } + + ASSERT_TRUE(tls.Fit(a, b)); + EXPECT_NEAR(tls.Coefficients().at(0, 0), 2.0f, math::Tolerance()); +} + +TEST_F(TestTotalLeastSquares, symmetric_noise_beats_ols) +{ + static constexpr float na[] = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, -1.0f, -1.0f, 1.0f }; + static constexpr float nb[] = { 1.0f, -1.0f, 1.0f, -1.0f, -1.0f, 1.0f, -1.0f, 1.0f }; + + math::Matrix a; + math::Vector b; + + float saa = 0.0f; + float sab = 0.0f; + for (std::size_t i = 0; i < 8; ++i) + { + float ai = (1.0f + static_cast(i)) + na[i]; + float bi = 2.0f * (1.0f + static_cast(i)) + nb[i]; + a.at(i, 0) = ai; + b.at(i, 0) = bi; + saa += ai * ai; + sab += ai * bi; + } + + ASSERT_TRUE(tls.Fit(a, b)); + + float ols = sab / saa; + float tlsSlope = tls.Coefficients().at(0, 0); + + EXPECT_LT(std::abs(tlsSlope - 2.0f), std::abs(ols - 2.0f)); + EXPECT_NEAR(tlsSlope, 2.0f, math::Tolerance()); +} + +TEST_F(TestTotalLeastSquares, matches_ols_when_regressors_clean) +{ + static constexpr float nb[] = { 0.05f, -0.04f, 0.03f, -0.02f, 0.04f, -0.05f, 0.02f, -0.03f }; + + math::Matrix a; + math::Vector b; + + float saa = 0.0f; + float sab = 0.0f; + for (std::size_t i = 0; i < 8; ++i) + { + float ai = 1.0f + static_cast(i); + float bi = 2.0f * ai + nb[i]; + a.at(i, 0) = ai; + b.at(i, 0) = bi; + saa += ai * ai; + sab += ai * bi; + } + + ASSERT_TRUE(tls.Fit(a, b)); + + float ols = sab / saa; + EXPECT_NEAR(tls.Coefficients().at(0, 0), ols, 1e-2f); +} + +TEST_F(TestTotalLeastSquares, multivariate_plane_fit) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 10; ++i) + { + float a1 = 1.0f + static_cast(i); + float a2 = 3.0f - 0.5f * static_cast(i) + static_cast(i % 3); + a.at(i, 0) = a1; + a.at(i, 1) = a2; + b.at(i, 0) = 1.5f * a1 - 0.5f * a2; + } + + ASSERT_TRUE(tls2.Fit(a, b)); + EXPECT_NEAR(tls2.Coefficients().at(0, 0), 1.5f, math::Tolerance()); + EXPECT_NEAR(tls2.Coefficients().at(1, 0), -0.5f, math::Tolerance()); +} + +TEST_F(TestTotalLeastSquares, degenerate_returns_false) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 8; ++i) + { + a.at(i, 0) = 0.0f; + b.at(i, 0) = 1.0f + static_cast(i); + } + + EXPECT_FALSE(tls.Fit(a, b)); +} + +TEST_F(TestTotalLeastSquares, predict_matches_dot_product) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 10; ++i) + { + float a1 = 1.0f + static_cast(i); + float a2 = 3.0f - 0.5f * static_cast(i) + static_cast(i % 3); + a.at(i, 0) = a1; + a.at(i, 1) = a2; + b.at(i, 0) = 1.5f * a1 - 0.5f * a2; + } + + ASSERT_TRUE(tls2.Fit(a, b)); + + math::Vector x; + x.at(0, 0) = 2.0f; + x.at(1, 0) = -1.0f; + + const auto& c = tls2.Coefficients(); + float expected = c.at(0, 0) * x.at(0, 0) + c.at(1, 0) * x.at(1, 0); + + EXPECT_NEAR(tls2.Predict(x), expected, math::Tolerance()); +} diff --git a/numerical/solvers/SingularValueDecomposition.hpp b/numerical/solvers/SingularValueDecomposition.hpp index f8b02578..0348940b 100644 --- a/numerical/solvers/SingularValueDecomposition.hpp +++ b/numerical/solvers/SingularValueDecomposition.hpp @@ -235,7 +235,9 @@ namespace solvers OPTIMIZE_FOR_SPEED void SingularValueDecomposition::QrSweep( std::size_t p, std::size_t q) { - T mu = WilkinsonShift(q); + T mu = (std::abs(sigma.at(q + 1, 0)) <= T{ 1e-6f } * std::abs(sigma.at(q, 0))) + ? T{} + : WilkinsonShift(q); T f = sigma.at(p, 0) * sigma.at(p, 0) - mu; T g = sigma.at(p, 0) * superdiag.at(p, 0); From 325bdc0ed294348d30d83ad617d3e003b4b9b3b9 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 2 Aug 2026 06:20:58 +0000 Subject: [PATCH 2/5] remove algo that are implemented --- .../explanation.md | 32 -------- .../implementation.md | 74 ------------------ .../DynamicParameterIdentification/tests.md | 57 -------------- .../offline/TotalLeastSquares/explanation.md | 31 -------- .../TotalLeastSquares/implementation.md | 71 ----------------- .../offline/TotalLeastSquares/tests.md | 56 -------------- .../online/MomentumObserver/explanation.md | 33 -------- .../online/MomentumObserver/implementation.md | 69 ----------------- .../online/MomentumObserver/tests.md | 63 --------------- .../passive/MovingAverage/explanation.md | 28 ------- .../passive/MovingAverage/implementation.md | 61 --------------- .../filters/passive/MovingAverage/tests.md | 51 ------------ .../solvers/JacobiEigenSolver/explanation.md | 28 ------- .../JacobiEigenSolver/implementation.md | 77 ------------------- roadmap/solvers/JacobiEigenSolver/tests.md | 58 -------------- 15 files changed, 789 deletions(-) delete mode 100644 roadmap/estimators/offline/DynamicParameterIdentification/explanation.md delete mode 100644 roadmap/estimators/offline/DynamicParameterIdentification/implementation.md delete mode 100644 roadmap/estimators/offline/DynamicParameterIdentification/tests.md delete mode 100644 roadmap/estimators/offline/TotalLeastSquares/explanation.md delete mode 100644 roadmap/estimators/offline/TotalLeastSquares/implementation.md delete mode 100644 roadmap/estimators/offline/TotalLeastSquares/tests.md delete mode 100644 roadmap/estimators/online/MomentumObserver/explanation.md delete mode 100644 roadmap/estimators/online/MomentumObserver/implementation.md delete mode 100644 roadmap/estimators/online/MomentumObserver/tests.md delete mode 100644 roadmap/filters/passive/MovingAverage/explanation.md delete mode 100644 roadmap/filters/passive/MovingAverage/implementation.md delete mode 100644 roadmap/filters/passive/MovingAverage/tests.md delete mode 100644 roadmap/solvers/JacobiEigenSolver/explanation.md delete mode 100644 roadmap/solvers/JacobiEigenSolver/implementation.md delete mode 100644 roadmap/solvers/JacobiEigenSolver/tests.md diff --git a/roadmap/estimators/offline/DynamicParameterIdentification/explanation.md b/roadmap/estimators/offline/DynamicParameterIdentification/explanation.md deleted file mode 100644 index 190e1c4d..00000000 --- a/roadmap/estimators/offline/DynamicParameterIdentification/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Dynamic Parameter Identification — Overview - -## What it is -An offline (batch) estimator that recovers a manipulator's **link inertial parameters** — masses, -centers of mass, and inertia tensors (in their identifiable *base-parameter* combinations) — from -recorded motion. It exploits the fact that the inverse-dynamics torque is **linear in these -parameters**: `Y(q, q̇, q̈)·a = τ`. - -## Why it matters (embedded) -Model-based controllers (computed-torque, impedance, momentum observers) are only as good as the -dynamic model they use. Datasheet inertias are approximate and change when a tool or payload is -attached. Running an excitation trajectory and identifying the parameters on-device produces an -accurate, up-to-date model without a CAD teardown. - -## How it works (intuition) -Command a rich *excitation trajectory* and log joint positions, velocities, accelerations, and -torques. At each sample the Recursive Newton-Euler regressor produces a matrix `Y` such that -`Y·a = τ`. Stacking many samples over-determines `a`, which is then found by **least squares**. -Only combinations the trajectory actually excites (the base parameters) are identifiable. - -## Key parameters -- **Excitation trajectory** — must be persistently exciting (multi-frequency) to make `YᵀY` full-rank. -- **Base parameters** — the identifiable linear combinations; the raw inertial set is rank-deficient. -- **Signal filtering** — consistent filtering of `q̇, q̈, τ` to suppress differentiation noise. - -## Reference -C. G. Atkeson, C. H. An, J. M. Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads -and Links," *Int. J. Robotics Research*, 5(3), 1986. - -## See also -`RecursiveNewtonEuler` (supplies the regressor), `PolynomialFitting` / `LinearRegression` -(least-squares kin), `SlotineLiAdaptiveControl` (online parameter adaptation). diff --git a/roadmap/estimators/offline/DynamicParameterIdentification/implementation.md b/roadmap/estimators/offline/DynamicParameterIdentification/implementation.md deleted file mode 100644 index 84cc7c5d..00000000 --- a/roadmap/estimators/offline/DynamicParameterIdentification/implementation.md +++ /dev/null @@ -1,74 +0,0 @@ -# Dynamic Parameter Identification — Implementation Pseudocode - -> Roadmap ref: #M22 (Tier 4) · Target: `numerical/estimators/offline` · Namespace `estimators` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class DynamicParameterIdentification: - Matrix A = 0 # accumulated Σ Yᵀ Y - Vector b = 0 # accumulated Σ Yᵀ τ - Vector parameters # estimated base parameters â - Rnea& model # regressor source (injected) -``` - -## Interface - -``` -DynamicParameterIdentification(Rnea& model) -void Accumulate(Vector q, Vector qd, Vector qdd, Vector tau) # per sample -bool Estimate() # solve once trajectory captured -const Vector& Parameters() -void Reset() -``` - -## Algorithm (pseudocode) - -``` -function Accumulate(q, qd, qdd, tau): # OPTIMIZE_FOR_SPEED - # Linear-in-parameters regressor from inverse dynamics: Y(q,q̇,q̈) · a = τ - Y = model.Regressor(q, qd, qdd) # (Dof x Params) - A += Yᵀ * Y # fold sample into normal equations - b += Yᵀ * tau # never store the full stacked system - -function Estimate(): # OPTIMIZE_FOR_SPEED - # Least-squares base-parameter solve: (Σ YᵀY) â = Σ Yᵀτ - if not PositiveDefinite(A): return false # trajectory not exciting enough - parameters = solvers::SolveSystem(A, b) # or QR on the stacked form - return true - -function Reset(): - A = 0 ; b = 0 -``` - -## Complexity & memory - -- Accumulate: `O(Dof·Params²)` per sample; runs online over the excitation trajectory. -- Estimate: one `O(Params³)` solve after the trajectory is captured. -- Memory: `O(Params²)` for the running normal equations — **independent of sample count**; no heap. - -## Numerical / embedded notes - -- Only the **base parameters** (identifiable combinations) are observable; the full inertial set is - rank-deficient. A non-positive-definite `A` means the trajectory under-excites some parameters — - enrich the excitation (more frequencies) rather than regularizing blindly. -- Accumulating `Σ YᵀY` keeps memory constant but **squares the condition number**; prefer a **QR** - update on the stacked `[Y; …]` when conditioning is marginal. -- Filter `q̈` (and torque) consistently — differentiation noise biases the regressor. -- 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/estimators/offline/DynamicParameterIdentification.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Accumulate`/`Estimate`, and - `extern template class DynamicParameterIdentification;` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/estimators/offline/DynamicParameterIdentification.cpp` → - `template class DynamicParameterIdentification;` -- Test: `numerical/estimators/offline/test/TestDynamicParameterIdentification.cpp` -- Doc: `doc/estimators/DynamicParameterIdentification.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestDynamicParameterIdentification.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/estimators/offline/DynamicParameterIdentification/tests.md b/roadmap/estimators/offline/DynamicParameterIdentification/tests.md deleted file mode 100644 index 47ddc84b..00000000 --- a/roadmap/estimators/offline/DynamicParameterIdentification/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# Dynamic Parameter Identification — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class MockRnea : public Rnea: - MOCK_METHOD((Matrix), Regressor, (q, qd, qdd), (const, override)) - -class TestDynamicParameterId : public ::testing::Test: - StrictMock model - DynamicParameterIdentification id{ model } - Vector trueParams = {…} # ground-truth base parameters -# each case below is a TEST_F(TestDynamicParameterId, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -recovers_known_parameters_noise_free: - Arrange: model.Regressor returns Y_k; tau_k = Y_k · trueParams - Act: Accumulate all samples; Estimate() - Assert: parameters ≈ trueParams - -underexcited_trajectory_returns_false: - Arrange: all regressor rows collinear (one posture repeated) - Act: Accumulate; Estimate() - Assert: Estimate() == false (A not positive-definite) - -reset_clears_accumulators: - Arrange: Accumulate samples, Reset() - Assert: A and b zero; a fresh Estimate on new data ignores old samples - -least_squares_averages_noise: - Arrange: tau = Y·trueParams + small zero-mean noise - Assert: estimate within tolerance; error shrinks with more samples - -accumulation_is_order_independent: - Arrange: feed the same samples in two different orders - Assert: identical A, b, and estimate - -single_link_scalar_case: - Arrange: Dof = 1, one inertial parameter - Assert: recovers τ = I·q̈ inertia exactly -``` - -## Reference vectors - -- 2-DOF planar arm, known `[m₁, m₂, l_c, I₁, I₂]` base set ⇒ published regressor rows reproduce τ. -- Diagonal single-parameter case ⇒ `â = mean(τ / Y)`. - -## Edge cases - -- Rank-deficient regressor ⇒ `Estimate()` reports failure, no divide-by-zero. -- Very stiff parameter scaling (inertia ≪ mass) ⇒ column-scale before solving. -- Zero acceleration/velocity sample ⇒ contributes only the gravity/mass columns. diff --git a/roadmap/estimators/offline/TotalLeastSquares/explanation.md b/roadmap/estimators/offline/TotalLeastSquares/explanation.md deleted file mode 100644 index 2f0daf4c..00000000 --- a/roadmap/estimators/offline/TotalLeastSquares/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Total Least Squares — Overview - -## What it is -An errors-in-variables fitting method: it finds the model that best explains the data when **both** -the inputs (regressors) and the outputs are corrupted by noise. Where ordinary least squares -assumes the inputs are exact and only `y` is noisy, TLS treats every measurement as uncertain. - -## Why it matters (embedded) -Real sensors are noisy on every channel. Calibrating one measured quantity against another -(e.g. two drifting sensors, current-vs-torque, strain-vs-force) is an errors-in-variables problem; -using OLS there produces a systematically **biased** slope. TLS removes that bias, giving more -accurate calibration constants and system-identification parameters. - -## How it works (intuition) -Geometrically, OLS minimizes vertical distances to the fitted line; TLS minimizes the -**perpendicular** distances. Algebraically, stack the inputs and output into one augmented matrix -`[A | b]` and take its singular value decomposition. The direction of **least variance** — the -right-singular vector of the smallest singular value — defines the hyperplane the data hugs, and -the coefficients drop out of that vector. - -## Key parameters -- **Column scaling** — inputs and output must be normalized to comparable magnitudes first. -- **Singular-value gap** — the separation of the two smallest σ indicates how well-posed the fit is. - -## Reference -G. H. Golub, C. F. Van Loan, "An Analysis of the Total Least Squares Problem," -*SIAM J. Numer. Anal.*, 17(6), 1980. - -## See also -`SingularValueDecomposition` (the numerical engine), `PolynomialFitting` / `LinearRegression` -(ordinary least-squares counterparts), `estimators/offline`. diff --git a/roadmap/estimators/offline/TotalLeastSquares/implementation.md b/roadmap/estimators/offline/TotalLeastSquares/implementation.md deleted file mode 100644 index 79819537..00000000 --- a/roadmap/estimators/offline/TotalLeastSquares/implementation.md +++ /dev/null @@ -1,71 +0,0 @@ -# Total Least Squares — Implementation Pseudocode - -> Roadmap ref: #44 (Tier 5) · Target: `numerical/estimators/offline` · Namespace `estimators` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class TotalLeastSquares: - Vector coefficients - static_assert Samples >= Features + 1 -``` - -## Interface - -``` -TotalLeastSquares() -bool Fit(Matrix A, Vector b) # hot path; false if degenerate -T Predict(Vector x) -const Vector& Coefficients() -``` - -## Algorithm (pseudocode) - -``` -function Fit(A, b): # OPTIMIZE_FOR_SPEED - # Stack the augmented system M = [A | b] (Samples x Features+1) - M = concat_columns(A, b) - # Thin SVD: M = U Σ Vᵀ (reuse solvers::SingularValueDecomposition) - (U, sigma, V) = SVD(M) - # Smallest singular value -> its right-singular vector v (length Features+1) - v = V.column(Features) # column of smallest σ - denom = v[Features] # last entry - if |denom| < eps: return false # b lies in span(A): no TLS solution - for i in 0 .. Features-1: - coefficients[i] = -v[i] / denom # partition x = −v₁ / v₂ - return true - -function Predict(x): - return dot(coefficients, x) -``` - -## Complexity & memory - -- Fit: dominated by the SVD of the `Samples × (Features+1)` augmented matrix, - `O(Samples·Features²)` (Golub-Kahan bidiagonalization + implicit-QR sweeps). -- Memory: `O(Samples·Features)` for `M` and the SVD factors on the stack; no heap. - -## Numerical / embedded notes - -- TLS minimizes the **orthogonal** (perpendicular) distance to the fit, unlike ordinary least - squares which minimizes vertical residuals — the correct model when the regressors `A` are noisy. -- The solution is the right-singular vector of the **smallest** singular value; a tiny gap between - the two smallest σ signals an ill-posed problem (near-degenerate solution). -- Column-scale `A` and `b` to comparable magnitudes first, otherwise the SVD is dominated by the - largest column and the TLS/OLS distinction is lost. -- 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/estimators/offline/TotalLeastSquares.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Fit`, and - `extern template class TotalLeastSquares;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/estimators/offline/TotalLeastSquares.cpp` → - `template class TotalLeastSquares; template class TotalLeastSquares;` -- Test: `numerical/estimators/offline/test/TestTotalLeastSquares.cpp` -- Doc: `doc/estimators/TotalLeastSquares.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestTotalLeastSquares.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/estimators/offline/TotalLeastSquares/tests.md b/roadmap/estimators/offline/TotalLeastSquares/tests.md deleted file mode 100644 index 57df30ae..00000000 --- a/roadmap/estimators/offline/TotalLeastSquares/tests.md +++ /dev/null @@ -1,56 +0,0 @@ -# Total Least Squares — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestTotalLeastSquares : public ::testing::Test: - TotalLeastSquares tls # 8 samples, single regressor - TotalLeastSquares tls2 # multivariate -# each case below is a TEST_F(TestTotalLeastSquares, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -recovers_exact_line_no_noise: - Arrange: y = 2x from clean data - Act: Fit(A, b) - Assert: coefficient ≈ 2 - -symmetric_noise_beats_ols: - Arrange: line with equal noise added to BOTH x and y - Assert: TLS slope closer to truth than the OLS slope - -matches_ols_when_regressors_clean: - Arrange: noise only on b - Assert: TLS ≈ OLS solution (within tol) - -multivariate_plane_fit: - Arrange: b = 1.5·a1 − 0.5·a2, clean - Assert: coefficients ≈ [1.5, -0.5] - -degenerate_returns_false: - Arrange: b exactly in span(A) so last v-entry ≈ 0 - Assert: Fit returns false - -scaling_invariance_after_normalization: - Arrange: columns scaled to equal norm - Assert: solution consistent with the unscaled reference - -predict_matches_dot_product: - Arrange: known coefficients - Assert: Predict(x) == dot(coeff, x) -``` - -## Reference vectors - -- `b = 2a` clean ⇒ slope `2`, orthogonal residual `0`. -- Known 2×2 example from Golub & Van Loan (1980) ⇒ documented TLS solution. - -## Edge cases - -- Two smallest singular values nearly equal ⇒ ill-posed; assert detectable/flagged. -- `Samples == Features + 1` (minimal system). -- All-zero column in `A` ⇒ rank deficiency handled without divide-by-zero. diff --git a/roadmap/estimators/online/MomentumObserver/explanation.md b/roadmap/estimators/online/MomentumObserver/explanation.md deleted file mode 100644 index cc9cb780..00000000 --- a/roadmap/estimators/online/MomentumObserver/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Momentum-Based Collision-Detection Observer — Overview - -## What it is -An online observer that estimates the **external torques** acting on a robot's joints — from a -collision, a human contact, or an unmodeled load — **without any joint-torque or acceleration -sensors**. It watches the robot's *generalized momentum* and flags the discrepancy between how the -momentum actually evolves and how the model says it should. - -## Why it matters (embedded) -Safe human-robot interaction needs fast, cheap contact detection. Torque sensors on every joint -are expensive and fragile; numerically differentiating position twice to get acceleration is noisy. -The momentum observer needs only signals already available — position, velocity, and motor torque — -and runs in a few vector operations per cycle, making it ideal for real-time safety loops. - -## How it works (intuition) -The generalized momentum `p = M(q)·q̇` changes according to the applied torque plus any external -torque. By integrating the *known* right-hand side of the momentum equation and comparing it with -the *measured* momentum, the observer forms a **residual** `r`. With no contact the residual sits -at zero; when an external torque appears, `r` tracks it as a first-order (low-pass) estimate whose -speed is set by the observer gain. Crucially the acceleration term cancels out, so no `q̈` is needed. - -## Key parameters -- **Observer gain `K_O`** — sets the residual bandwidth: fast response vs. noise rejection. -- **Detection threshold** — per-joint residual level that declares a collision. -- **Model quality** — accurate `M, C, g` (and friction) keep the contact-free residual near zero. - -## Reference -A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, "Collision Detection and Safe Reaction -with the DLR-III Lightweight Manipulator Arm," *IEEE/RSJ IROS*, 2006. - -## See also -`RecursiveNewtonEuler` (supplies `M, C, g`), `DynamicParameterIdentification` (accurate model -parameters), `DisturbanceObserver` (general disturbance estimation). diff --git a/roadmap/estimators/online/MomentumObserver/implementation.md b/roadmap/estimators/online/MomentumObserver/implementation.md deleted file mode 100644 index f1adb91c..00000000 --- a/roadmap/estimators/online/MomentumObserver/implementation.md +++ /dev/null @@ -1,69 +0,0 @@ -# Momentum-Based Collision-Detection Observer — Implementation Pseudocode - -> Roadmap ref: #M16 (Tier 3) · Target: `numerical/estimators/online` · Namespace `estimators` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class MomentumObserver: - Dynamics& model # supplies M(q), Cᵀ(q,q̇)q̇, g(q) - Vector gain # observer bandwidth K_O (diagonal, > 0) - Vector integral = 0 # ∫(τ + Cᵀq̇ − g + r) dt - Vector residual = 0 # r ≈ estimated external torque - Vector p0 # generalized momentum captured at reset -``` - -## Interface - -``` -MomentumObserver(Dynamics& model, Vector gain) -Vector Update(q, qd, tau, dt) # hot path; returns residual r -void Reset(q, qd) # capture p0 = M(q)·q̇, clear integral -bool CollisionDetected(threshold) # |r_i| > threshold_i on any joint -``` - -## Algorithm (pseudocode) - -``` -function Update(q, qd, tau, dt): # OPTIMIZE_FOR_SPEED - p = model.MassMatrix(q) * qd # generalized momentum p = M(q) q̇ - # β(q,q̇) = Cᵀ(q,q̇) q̇ − g(q) (uses skew-symmetry Ṁ = C + Cᵀ) - beta = model.CoriolisTranspose(q, qd) - model.Gravity(q) - integral += (tau + beta + residual) * dt # rectangular integration - residual = elementwise(gain, p - p0 - integral) # r = K_O ⊙ (p − p0 − ∫) - return residual - -function Reset(q, qd): - p0 = model.MassMatrix(q) * qd - integral = 0 ; residual = 0 -``` - -## Complexity & memory - -- Update: one `M(q)` / `Cᵀq̇` / `g(q)` evaluation (`O(Dof²)`–`O(Dof)` via RNEA) plus `O(Dof)` glue. -- Memory: a few `Dof`-vectors; no heap, no matrix inversion, no acceleration term. - -## Numerical / embedded notes - -- **No q̈, no torque sensors:** the observer avoids noisy acceleration estimates entirely; each - residual channel `r_i` behaves like a **first-order low-pass of the true external torque τ_ext** - with bandwidth set by `gain_i` — larger gain = faster but noisier, smaller = smoother but laggier. -- Decoupling: with diagonal `K_O` the residual dynamics decouple per joint ⇒ a collision on link - `j` shows up predominantly in `r_j` (and downstream joints), aiding localization. -- Integrate with a fixed `dt`; model error (gravity/friction bias) drifts the `integral`, so - bias-compensate or periodically re-`Reset()` when the arm is known to be contact-free. -- 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/estimators/online/MomentumObserver.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Update`, and - `extern template class MomentumObserver;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/estimators/online/MomentumObserver.cpp` → `template class MomentumObserver;` -- Test: `numerical/estimators/online/test/TestMomentumObserver.cpp` -- Doc: `doc/estimators/MomentumObserver.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestMomentumObserver.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/estimators/online/MomentumObserver/tests.md b/roadmap/estimators/online/MomentumObserver/tests.md deleted file mode 100644 index 52cd3d22..00000000 --- a/roadmap/estimators/online/MomentumObserver/tests.md +++ /dev/null @@ -1,63 +0,0 @@ -# Momentum-Based Collision Observer — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class MockDynamics : public Dynamics: - MOCK_METHOD((Matrix), MassMatrix, (q), (const, override)) - MOCK_METHOD((Vector), CoriolisTranspose, (q, qd), (const, override)) - MOCK_METHOD((Vector), Gravity, (q), (const, override)) - -class TestMomentumObserver : public ::testing::Test: - StrictMock model - Vector<2> gain = {50, 50} - MomentumObserver observer{ model, gain } -# each case below is a TEST_F(TestMomentumObserver, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -no_external_torque_gives_zero_residual: - Arrange: consistent M,C,g; tau equals internal dynamics torque; no contact - Act: Reset then Update over steps - Assert: residual ≈ 0 - -step_external_torque_is_tracked: - Arrange: inject constant τ_ext on joint 0 - Act: Update repeatedly - Assert: r[0] rises toward τ_ext (first-order); r[1] stays small - -higher_gain_converges_faster: - Arrange: two observers K_O = 20 vs 200, same τ_ext - Assert: larger gain reaches τ_ext in fewer steps - -reset_captures_initial_momentum: - Arrange: nonzero q̇ at reset - Assert: first residual after Reset ≈ 0 (p0 subtracted) - -collision_detected_above_threshold: - Arrange: residual driven past threshold - Assert: CollisionDetected(threshold) == true; false below - -residual_decays_after_contact_release: - Arrange: τ_ext applied then removed - Assert: r returns toward 0 with the observer time constant - -per_joint_localization: - Arrange: contact only on joint 1 - Assert: |r[1]| >> |r[0]| -``` - -## Reference vectors - -- Static arm (q̇ = 0), gravity-consistent τ ⇒ residual 0 at every step. -- First-order response: with gain `k`, `r(t) ≈ τ_ext·(1 − e^{−k·t})` for a constant external torque. - -## Edge cases - -- Large `dt` step ⇒ integration drift; assert bounded residual under model bias. -- Zero gain ⇒ residual stays 0 (observer disabled). -- Model bias in `g(q)` ⇒ constant residual offset (documents the Reset-to-rezero behavior). diff --git a/roadmap/filters/passive/MovingAverage/explanation.md b/roadmap/filters/passive/MovingAverage/explanation.md deleted file mode 100644 index fe9b98c4..00000000 --- a/roadmap/filters/passive/MovingAverage/explanation.md +++ /dev/null @@ -1,28 +0,0 @@ -# Moving Average — Overview - -## What it is -A length-`N` boxcar (unweighted) FIR filter computed recursively: keep a running sum of the last -`N` samples, update it as `sum += x[n] − x[n−N]`, then divide by `N`. - -## Why it matters (embedded) -For a given noise-reduction target it is the **optimal filter per unit of computation** — and the -recursive form costs `O(1)` per sample regardless of window length. Ideal for cleaning up white -noise on ADC/sensor streams when a flat passband and linear phase are wanted. - -## How it works (intuition) -Averaging `N` samples attenuates zero-mean noise by `√N` while passing DC untouched. Because every -tap has equal weight, the frequency response is a `sinc` — excellent in the time domain (sharp step -response, linear phase) but a mediocre frequency-selective filter (slow roll-off, sidelobes). Its -finite memory means an impulse fully leaves after exactly `N` samples. - -## Key parameters -- **N (window length)** — larger `N` ⇒ more smoothing and longer delay `(N−1)/2`. -- **initial state** — seed value to shorten the startup transient. - -## Reference -S. W. Smith, *The Scientist and Engineer's Guide to Digital Signal Processing*, Ch. 15 -(Moving Average Filters — recursive running-sum form). - -## See also -`ExponentialMovingAverage` (cheaper IIR smoother), `SavitzkyGolayFilter` (peak-preserving), -`BiquadCascade` (frequency-selective roll-off). diff --git a/roadmap/filters/passive/MovingAverage/implementation.md b/roadmap/filters/passive/MovingAverage/implementation.md deleted file mode 100644 index bf278c7f..00000000 --- a/roadmap/filters/passive/MovingAverage/implementation.md +++ /dev/null @@ -1,61 +0,0 @@ -# Moving Average (Running-Sum Boxcar) — Implementation Pseudocode - -> Roadmap ref: #2 (Tier 1) · Target: `numerical/filters/passive` · Namespace `filters::passive` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class MovingAverage: # N = window length - RecursiveBuffer window # bounded delay line, holds x[n-1..n-N] - T sum # running sum - T invN # precomputed 1/N - bool enabled = true -``` - -## Interface - -``` -MovingAverage(T initial = 0) -T Filter(T input) # hot path -void Reset(T value = 0) -``` - -## Algorithm (pseudocode) - -``` -function Filter(x): # OPTIMIZE_FOR_SPEED - if not enabled: return x - oldest = window.Oldest() # x[n-N] - sum = sum + x - oldest # incremental update, O(1) - window.Push(x) # drop oldest, append newest - return sum * invN # divide by N via multiply -``` - -## Complexity & memory - -- Time: `O(1)` per sample — one add, one sub, one multiply, independent of `N`. -- Memory: `O(N)` for the delay line plus a single accumulator word. - -## Numerical / embedded notes - -- Linear phase: constant group delay of `(N-1)/2` samples; DC gain is exactly 1. -- Recursive running-sum can accumulate rounding error over long runs; periodically rebuild the - sum from the window if drift matters. -- Optimal white-noise reducer per computation, but a poor frequency-selective filter - (`sinc`-shaped magnitude, slow stopband roll-off). -- 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/passive/MovingAverage.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Filter`, and - `extern template class MovingAverage;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/filters/passive/MovingAverage.cpp` → - `template class MovingAverage;` -- Test: `numerical/filters/passive/test/TestMovingAverage.cpp` -- Doc: `doc/filters/passive/MovingAverage.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestMovingAverage.cpp` → the `_test` target. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/filters/passive/MovingAverage/tests.md b/roadmap/filters/passive/MovingAverage/tests.md deleted file mode 100644 index 5ef15cd9..00000000 --- a/roadmap/filters/passive/MovingAverage/tests.md +++ /dev/null @@ -1,51 +0,0 @@ -# Moving Average — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestMovingAverage : public ::testing::Test: - MovingAverage ma{} # window length N = 4 -# each case below is a TEST_F(TestMovingAverage, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -warmup_fills_window: - Arrange: N = 4, input = [1, 0, 0, 0], zero-initialized window - Act: collect y per sample - Assert: y ≈ [0.25, 0.25, 0.25, 0.25] (impulse spread over N taps) - -constant_input_unity_gain: - Arrange: feed constant c = 0.5 for > N samples - Assert: steady-state output ≈ 0.5 - -step_response_is_linear_ramp: - Arrange: step 0 -> 1 with N = 4 - Assert: y climbs 0.25, 0.5, 0.75, 1.0 then holds - -running_sum_matches_direct_average: - Arrange: random bounded sequence - Act: compare Filter() against a brute-force mean of the last N - Assert: equal within tolerance (EXPECT_NEAR, tol 1e-6f) - -impulse_leaves_after_N: - Arrange: single impulse then zeros - Assert: output returns to 0 exactly after N samples (FIR, finite memory) - -reset_clears_state: - Arrange: run samples, Reset(0) - Assert: window and sum cleared; next output == input / N -``` - -## Reference vectors - -- `N = 4`, unit impulse ⇒ `y = [0.25, 0.25, 0.25, 0.25, 0, 0, ...]`. -- `N = 4`, unit step ⇒ `y = [0.25, 0.5, 0.75, 1, 1, ...]`. - -## Edge cases - -- `N = 1` ⇒ pass-through. -- Long constant run: assert no slow drift from running-sum rounding (`EXPECT_NEAR`, tol `1e-6f`). diff --git a/roadmap/solvers/JacobiEigenSolver/explanation.md b/roadmap/solvers/JacobiEigenSolver/explanation.md deleted file mode 100644 index 8988f97f..00000000 --- a/roadmap/solvers/JacobiEigenSolver/explanation.md +++ /dev/null @@ -1,28 +0,0 @@ -# Symmetric Eigenvalue Solver (Jacobi) — Overview - -## What it is -An iterative method that finds the eigenvalues and eigenvectors of a **symmetric** matrix by applying -a sequence of plane (Jacobi) rotations that progressively zero the off-diagonal entries until only a -diagonal of eigenvalues remains. - -## Why it matters (embedded) -Symmetric eigenproblems are the heart of **PCA / feature extraction**, modal (vibration) analysis, -and covariance conditioning. On-device these matrices are small — a few states, an inertia tensor, a -covariance block — exactly the regime where Jacobi is both simplest and most accurate. - -## How it works (intuition) -Each rotation targets one off-diagonal pair `(p,q)` and turns it into zero, at the cost of slightly -disturbing others — but the total off-diagonal "energy" always decreases. Sweeping over every pair -repeatedly drives the matrix to diagonal form; the accumulated rotations spell out the eigenvectors. -Convergence is quadratic, so a handful of sweeps suffices. - -## Key parameters -- **Tolerance** — the off-diagonal norm at which the matrix is declared diagonal. -- **Max sweeps** — an upper bound that guarantees bounded, real-time-friendly work. - -## Reference -Golub & Van Loan, *Matrix Computations*, Ch. 8 (symmetric eigenproblem / cyclic Jacobi). - -## See also -`SingularValueDecomposition` (built on the same rotations), `LyapunovSylvester`, -`QrDecomposition`. diff --git a/roadmap/solvers/JacobiEigenSolver/implementation.md b/roadmap/solvers/JacobiEigenSolver/implementation.md deleted file mode 100644 index 7ea23411..00000000 --- a/roadmap/solvers/JacobiEigenSolver/implementation.md +++ /dev/null @@ -1,77 +0,0 @@ -# Symmetric Eigenvalue Solver (Cyclic Jacobi) — Implementation Pseudocode - -> Roadmap ref: #42 (Tier 5) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class JacobiEigenSolver: - Vector eigenvalues - Matrix eigenvectors # columns - std::size_t maxSweeps - T tolerance # off-diagonal threshold - bool converged -``` - -## Interface - -``` -JacobiEigenSolver(std::size_t maxSweeps = 30, T tolerance = T(1e-9)) -bool Decompose(const Matrix& symmetricA) # hot path -const Vector& Eigenvalues() const -const Matrix& Eigenvectors() const -``` - -## Algorithm (pseudocode) - -``` -function Decompose(A): # OPTIMIZE_FOR_SPEED (cyclic Jacobi) - assert IsSymmetric(A) - D = A; V = Identity - for sweep in 1 .. maxSweeps: - if OffDiagonalNorm(D) < tolerance: converged = true; break - for p in 0 .. N-2: # cyclic order over all pairs - for q in p+1 .. N-1: - if |D[p,q]| ≈ 0: continue - theta = (D[q,q] − D[p,p]) / (2·D[p,q]) # angle that zeros D[p,q] - t = sign(theta) / (|theta| + sqrt(theta² + 1)) - c = 1 / sqrt(t² + 1); s = t·c - D = Jᵀ(p,q,c,s) · D · J(p,q,c,s) # updates two rows & two cols - V = V · J(p,q,c,s) # accumulate eigenvectors - eigenvalues = diag(D) - eigenvectors = V - SortDescending(eigenvalues, eigenvectors) - return converged -``` - -## Complexity & memory - -- `O(N³)` per sweep; convergence is quadratic, typically `6–10` sweeps ⇒ `O(N³)` in practice. -- Each rotation updates only two rows/columns — `O(N)` work per off-diagonal pair. -- Memory: `O(N²)` for `D` and `V` — static, no heap. - -## Numerical / embedded notes - -- **Symmetric input only** — assert symmetry; the algorithm guarantees **real** eigenvalues and an - **orthonormal** eigenvector set (`V` stays orthogonal because every `J` is a rotation). -- **Cyclic** sweeps visit pairs in fixed order, avoiding the `O(N²)` largest-off-diagonal search of - classical Jacobi while keeping the same accuracy — a good fit for small on-device matrices. -- Highly accurate even for clustered eigenvalues; preferred over QR-iteration for the small symmetric - matrices typical on-device (covariance, inertia tensors, Gramians). -- Reused by `SingularValueDecomposition` (the `AᵀA` route) and the symmetric `LyapunovSylvester` path. -- 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/solvers/JacobiEigenSolver.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Decompose`, and - `extern template class JacobiEigenSolver;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/solvers/JacobiEigenSolver.cpp` → - `template class JacobiEigenSolver;` and `` (the tested sizes). -- Test: `numerical/solvers/test/TestJacobiEigenSolver.cpp` -- Doc: `doc/solvers/JacobiEigenSolver.md` -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestJacobiEigenSolver.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/JacobiEigenSolver/tests.md b/roadmap/solvers/JacobiEigenSolver/tests.md deleted file mode 100644 index 88bf0246..00000000 --- a/roadmap/solvers/JacobiEigenSolver/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Symmetric Eigenvalue Solver (Jacobi) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestJacobiEigenSolver : public ::testing::Test: - JacobiEigenSolver solver - JacobiEigenSolver solver2 -# each case below is a TEST_F(TestJacobiEigenSolver, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -diagonal_matrix_returns_diagonal: - Arrange: A = diag(3, 1, 2) - Assert: eigenvalues ≈ {3, 2, 1} (sorted), eigenvectors ≈ axes - -reconstructs_A: - Assert: V · diag(λ) · Vᵀ ≈ A - -eigenvectors_orthonormal: - Assert: Vᵀ·V ≈ I - -known_symmetric_2x2: - Arrange: [[2,1],[1,2]] - Assert: λ ≈ {3, 1}, vectors ≈ (1,±1)/√2 - -eigenpairs_satisfy_definition: - Assert: A·v_i ≈ λ_i·v_i for every pair - -off_diagonal_norm_decreases: - Assert: OffDiagonalNorm shrinks monotonically across sweeps - -repeated_eigenvalues_handled: - Arrange: A with a double eigenvalue - Assert: reconstructs A, eigenvectors still orthonormal - -identity_has_unit_eigenvalues: - Assert: λ ≈ {1,1,1}, converges in zero sweeps - -converges_within_max_sweeps: - Assert: Decompose returns true (converged flag) -``` - -## Reference vectors - -- `[[2,1],[1,2]]` ⇒ `λ = {3,1}`, vectors `(1,1)/√2` and `(1,−1)/√2`. -- Diagonal input ⇒ eigenvalues equal the diagonal, eigenvectors the identity. - -## Edge cases - -- Already-diagonal input ⇒ no rotations performed. -- Repeated / clustered eigenvalues ⇒ eigenvectors non-unique but orthonormal. -- Non-symmetric input ⇒ assertion (documented precondition). -- `N == 1` trivial case. From c9423cfb572984e2f6ede73cbafcc7431ef68859 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 2 Aug 2026 06:23:01 +0000 Subject: [PATCH 3/5] fix documentation created --- doc/estimators/TotalLeastSquares.md | 36 +++++++++++++++++++++-------- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/doc/estimators/TotalLeastSquares.md b/doc/estimators/TotalLeastSquares.md index 76bd988d..68095926 100644 --- a/doc/estimators/TotalLeastSquares.md +++ b/doc/estimators/TotalLeastSquares.md @@ -81,12 +81,30 @@ Fit $b = 2a$ from four clean points $a = (1,2,3,4)$, $b = (2,4,6,8)$. - **OLS counterparts** — [Linear Regression](LinearRegression.md) and [Polynomial Fitting](PolynomialFitting.md) solve the noise-free-regressor case. -## Reference - -G. H. Golub, C. F. Van Loan, "An Analysis of the Total Least Squares Problem," -*SIAM J. Numer. Anal.*, 17(6), 1980. - -## See Also - -- [`SingularValueDecomposition`](../solvers/SingularValueDecomposition.md) — the numerical engine. -- [Linear Regression](LinearRegression.md), [Polynomial Fitting](PolynomialFitting.md) — OLS fits. +## Applications + +- **Sensor calibration** — fitting one measured quantity against another (two drifting sensors, + current vs. torque, strain vs. force) where *both* channels carry noise. +- **System identification** — parameter estimation when the regressors themselves are measured, + removing the OLS bias that would corrupt an identified plant model. +- **Line/plane fitting** — geometric fitting that minimizes perpendicular distance, e.g. estimating + a boundary or feature direction from noisy point clouds. +- **Model reduction** — the smallest-singular-direction analysis flags near-degenerate fits and + quantifies the orthogonal residual. + +## Connections to Other Algorithms + +- [`SingularValueDecomposition`](../solvers/SingularValueDecomposition.md) — the numerical engine; + TLS is the SVD of `[A | b]` read from the smallest-singular-value end. +- [Linear Regression](LinearRegression.md) / [Polynomial Fitting](PolynomialFitting.md) — the + ordinary least-squares counterparts (exact regressors, vertical residuals). +- Symmetric eigenvalue / [Jacobi eigen solver](../solvers/JacobiEigenSolver.md) — TLS on the + augmented normal matrix `[A | b]ᵀ[A | b]` reduces to its smallest-eigenvalue eigenvector. + +## References & Further Reading + +- G. H. Golub, C. F. Van Loan, "An Analysis of the Total Least Squares Problem," + *SIAM J. Numer. Anal.*, 17(6), 1980. +- S. Van Huffel, J. Vandewalle, *The Total Least Squares Problem: Computational Aspects and + Analysis*, SIAM, 1991. +- G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., Ch. 6 (least squares) & Ch. 8 (SVD). From 883938d0cb0bcc324235c788817ae244207a44eb Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 2 Aug 2026 06:23:47 +0000 Subject: [PATCH 4/5] fix roadmap --- ROADMAP.md | 1 - 1 file changed, 1 deletion(-) diff --git a/ROADMAP.md b/ROADMAP.md index beb3e9ba..7cf2cc51 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|------------------------------------------------------|---------------------------|------------| -| 44 | Total Least Squares | `estimators/offline` | ★★★★★ | | 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) | ★★★★★ | From ee6b4657003fbf661a4ba80410f3cafab3278613 Mon Sep 17 00:00:00 2001 From: gfs Date: Sun, 2 Aug 2026 09:09:15 +0200 Subject: [PATCH 5/5] Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- doc/estimators/TotalLeastSquares.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/estimators/TotalLeastSquares.md b/doc/estimators/TotalLeastSquares.md index 68095926..09994829 100644 --- a/doc/estimators/TotalLeastSquares.md +++ b/doc/estimators/TotalLeastSquares.md @@ -47,11 +47,11 @@ all-zero regressor column) and no finite coefficient vector satisfies the fit ## Complexity Analysis -| Case | Time | Space | Notes | -|---------|---------------------|------------------|---------------------------------------------------| -| Best | $O(m\,n^2)$ | $O(m\,n)$ | Golub–Kahan bidiagonalization dominates | -| Average | $O(m\,n^2)$ | $O(m\,n)$ | Implicit-QR sweeps converge in $O(n)$ per value | -| Worst | $O(m\,n^2)$ | $O(m\,n)$ | Bounded iteration cap in the SVD engine | +| Case | Time | Space | Notes | +|---------|-------------|-----------|-------------------------------------------------| +| Best | $O(m\,n^2)$ | $O(m\,n)$ | Golub–Kahan bidiagonalization dominates | +| Average | $O(m\,n^2)$ | $O(m\,n)$ | Implicit-QR sweeps converge in $O(n)$ per value | +| Worst | $O(m\,n^2)$ | $O(m\,n)$ | Bounded iteration cap in the SVD engine | All storage is stack-allocated (`std::array`-backed `math::Matrix`); no heap, no recursion.