feat: total least squares

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                                                     |

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

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

doc/estimators/TotalLeastSquares.md

@@ -0,0 +1,110 @@
+# 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.
+
+## 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).

numerical/estimators/offline/CMakeLists.txt

@@ -16,13 +16,15 @@ target_sources(numerical.estimators.offline PRIVATE
     ExpectationMaximization.hpp
     LinearRegression.hpp
     PolynomialFitting.hpp
+    TotalLeastSquares.hpp
     YuleWalker.hpp
 )
 
 numerical_add_coverage_sources(numerical.estimators.offline
     ExpectationMaximization.cpp
     LinearRegression.cpp
     PolynomialFitting.cpp
+    TotalLeastSquares.cpp
     YuleWalker.cpp
 )
 

numerical/estimators/offline/TotalLeastSquares.cpp

@@ -0,0 +1,7 @@
+#include "numerical/estimators/offline/TotalLeastSquares.hpp"
+
+namespace estimators
+{
+    template class TotalLeastSquares<float, 8, 1>;
+    template class TotalLeastSquares<float, 10, 2>;
+}

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 <cmath>
+#include <cstddef>
+#include <type_traits>
+
+namespace estimators
+{
+    template<typename T, std::size_t Samples, std::size_t Features>
+    class TotalLeastSquares
+    {
+        static_assert(std::is_floating_point_v<T>, "TotalLeastSquares supports floating-point types only");
+        static_assert(Samples >= Features + 1, "Samples must be >= Features + 1");
+
+    public:
+        using CoefficientsVector = math::Vector<T, Features>;
+        using DesignMatrix = math::Matrix<T, Samples, Features>;
+        using SamplesVector = math::Vector<T, Samples>;
+        using FeaturesVector = math::Vector<T, Features>;
+
+        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<typename T, std::size_t Samples, std::size_t Features>
+    OPTIMIZE_FOR_SPEED bool TotalLeastSquares<T, Samples, Features>::Fit(const DesignMatrix& a, const SamplesVector& b)
+    {
+        math::Matrix<T, Samples, Augmented> 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<T, Samples, Augmented> 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<typename T, std::size_t Samples, std::size_t Features>
+    T TotalLeastSquares<T, Samples, Features>::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<typename T, std::size_t Samples, std::size_t Features>
+    const typename TotalLeastSquares<T, Samples, Features>::CoefficientsVector&
+    TotalLeastSquares<T, Samples, Features>::Coefficients() const
+    {
+        return coefficients;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class TotalLeastSquares<float, 8, 1>;
+    extern template class TotalLeastSquares<float, 10, 2>;
+#endif
+}

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
 )

numerical/estimators/offline/test/TestTotalLeastSquares.cpp

@@ -0,0 +1,143 @@
+#include "numerical/estimators/offline/TotalLeastSquares.hpp"
+#include "numerical/math/Tolerance.hpp"
+#include <cmath>
+#include <gtest/gtest.h>
+
+namespace
+{
+    class TestTotalLeastSquares : public ::testing::Test
+    {
+    protected:
+        estimators::TotalLeastSquares<float, 8, 1> tls;
+        estimators::TotalLeastSquares<float, 10, 2> tls2;
+    };
+}
+
+TEST_F(TestTotalLeastSquares, recovers_exact_line_no_noise)
+{
+    math::Matrix<float, 8, 1> a;
+    math::Vector<float, 8> b;
+
+    for (std::size_t i = 0; i < 8; ++i)
+    {
+        float ai = 1.0f + static_cast<float>(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<float>());
+}
+
+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<float, 8, 1> a;
+    math::Vector<float, 8> b;
+
+    float saa = 0.0f;
+    float sab = 0.0f;
+    for (std::size_t i = 0; i < 8; ++i)
+    {
+        float ai = (1.0f + static_cast<float>(i)) + na[i];
+        float bi = 2.0f * (1.0f + static_cast<float>(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<float>());
+}
+
+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<float, 8, 1> a;
+    math::Vector<float, 8> b;
+
+    float saa = 0.0f;
+    float sab = 0.0f;
+    for (std::size_t i = 0; i < 8; ++i)
+    {
+        float ai = 1.0f + static_cast<float>(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<float, 10, 2> a;
+    math::Vector<float, 10> b;
+
+    for (std::size_t i = 0; i < 10; ++i)
+    {
+        float a1 = 1.0f + static_cast<float>(i);
+        float a2 = 3.0f - 0.5f * static_cast<float>(i) + static_cast<float>(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<float>());
+    EXPECT_NEAR(tls2.Coefficients().at(1, 0), -0.5f, math::Tolerance<float>());
+}
+
+TEST_F(TestTotalLeastSquares, degenerate_returns_false)
+{
+    math::Matrix<float, 8, 1> a;
+    math::Vector<float, 8> b;
+
+    for (std::size_t i = 0; i < 8; ++i)
+    {
+        a.at(i, 0) = 0.0f;
+        b.at(i, 0) = 1.0f + static_cast<float>(i);
+    }
+
+    EXPECT_FALSE(tls.Fit(a, b));
+}
+
+TEST_F(TestTotalLeastSquares, predict_matches_dot_product)
+{
+    math::Matrix<float, 10, 2> a;
+    math::Vector<float, 10> b;
+
+    for (std::size_t i = 0; i < 10; ++i)
+    {
+        float a1 = 1.0f + static_cast<float>(i);
+        float a2 = 3.0f - 0.5f * static_cast<float>(i) + static_cast<float>(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<float, 2> 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<float>());
+}