feat: add polynomial least-squares fitting (#175)

* Add Polynomial Least-Squares Fitting

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

Author: gabrielfrasantos

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                                        |
 | [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, PID, LQR, MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller |
 | [Dynamics](doc/dynamics/README.md)                                 | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler, ABA            |
-| [Estimators](doc/estimators/README.md)                             | Linear Regression, Yule-Walker (offline), Recursive Least Squares (online) |
+| [Estimators](doc/estimators/README.md)                             | Linear Regression, Polynomial Fitting, Yule-Walker (offline), Recursive Least Squares (online) |
 | [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 |
 | [Kinematics](doc/kinematics/README.md)                             | Forward Kinematics                                                   |
 | [Neural Network](doc/neural_network/README.md)                     | Layers, activations, losses, model                                   |

ROADMAP.md

@@ -27,8 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 10 | Gain-scheduled controller                            | `controllers`             | ★★☆☆☆      |
-| 12 | Polynomial least-squares curve fitting               | `estimators/offline`      | ★★☆☆☆      |
 | 13 | Goertzel algorithm                                   | `analysis`                | ★★☆☆☆      |
 | 14 | CIC (Cascaded Integrator-Comb) filter                | `filters/passive`         | ★★☆☆☆      |
 | 15 | Biquad / Second-Order-Section cascade                | `filters/passive`         | ★★★☆☆      |

doc/estimators/PolynomialFitting.md

@@ -0,0 +1,158 @@
+# Polynomial Least-Squares Fitting
+
+## Overview & Motivation
+
+Sensor calibration curves, ADC linearization, thermistor transfer functions, and drift trends
+all require fitting a smooth curve to a discrete set of measured points. A degree-$d$ polynomial
+captures these behaviors with only $d+1$ coefficients, making evaluation at runtime a handful of
+multiply-adds rather than a table lookup or expensive transcendental.
+
+The least-squares formulation finds the polynomial that minimizes the sum of squared residuals
+across all measurement samples. Unlike exact interpolation, it is robust to measurement noise:
+extra samples average out errors rather than being forced to pass through noisy points.
+
+## Mathematical Theory
+
+### The Model
+
+Given $n$ scalar observations $\{(x_i, y_i)\}_{i=0}^{n-1}$, the degree-$d$ polynomial model is
+
+$$p(x) = c_0 + c_1 x + c_2 x^2 + \cdots + c_d x^d$$
+
+The goal is to find the coefficient vector $\mathbf{c} \in \mathbb{R}^{d+1}$ that minimizes
+
+$$\min_{\mathbf{c}} \sum_{i=0}^{n-1} \bigl(y_i - p(x_i)\bigr)^2$$
+
+### Vandermonde Design Matrix
+
+Stacking the model evaluations at all sample abscissae gives the Vandermonde matrix
+
+$$\mathbf{V} \in \mathbb{R}^{n \times (d+1)}, \quad V_{i,j} = x_i^j$$
+
+The least-squares problem then becomes $\min_{\mathbf{c}} \|\mathbf{V}\mathbf{c} - \mathbf{y}\|^2$.
+
+### Normal Equations
+
+Setting the gradient of the squared residual with respect to $\mathbf{c}$ to zero yields
+
+$$(\mathbf{V}^\top \mathbf{V})\,\mathbf{c} = \mathbf{V}^\top \mathbf{y}$$
+
+The $(d+1)\times(d+1)$ matrix $\mathbf{V}^\top\mathbf{V}$ is symmetric and, when the abscissae are
+distinct and $n \geq d+1$, positive-definite. Its small size allows direct solution by Gaussian
+elimination or Cholesky factorization in bounded time on embedded hardware.
+
+### Horner Evaluation
+
+Once $\mathbf{c}$ is known, evaluating $p(x)$ at a new point uses Horner's method
+
+$$p(x) = c_0 + x\bigl(c_1 + x\bigl(c_2 + \cdots + x\,c_d\bigr)\cdots\bigr)$$
+
+This requires exactly $d$ multiplications and $d$ additions — optimal for a degree-$d$ polynomial.
+
+## Complexity Analysis
+
+| Phase                            | Time        | Space     | Notes                             |
+|----------------------------------|-------------|-----------|-----------------------------------|
+| Build $\mathbf{V}$               | $O(n\,d)$   | $O(n\,d)$ | Incremental powers, no `pow()`    |
+| Form $\mathbf{V}^\top\mathbf{V}$ | $O(n\,d^2)$ | $O(d^2)$  | Symmetric, only upper half needed |
+| Form $\mathbf{V}^\top\mathbf{y}$ | $O(n\,d)$   | $O(d)$    | Matrix-vector product             |
+| Solve $(d+1)\times(d+1)$ system  | $O(d^3)$    | $O(d^2)$  | Gaussian elimination              |
+| Predict (Horner)                 | $O(d)$      | $O(1)$    | One MAC per coefficient           |
+
+All dimensions are compile-time constants; no heap allocation is required.
+
+## Step-by-Step Walkthrough
+
+**Data:** $n = 4$ samples, $d = 2$ (quadratic fit).
+
+| $x_i$ | $y_i$ |
+|-------|-------|
+| 0     | 1     |
+| 1     | 0.75  |
+| 2     | 1     |
+| 3     | 1.75  |
+
+**Step 1 — Build $\mathbf{V}$:**
+
+$$\mathbf{V} = \begin{bmatrix} 1 & 0 & 0 \\ 1 & 1 & 1 \\ 1 & 2 & 4 \\ 1 & 3 & 9 \end{bmatrix}$$
+
+**Step 2 — Normal equations:**
+
+$$\mathbf{V}^\top\mathbf{V} = \begin{bmatrix} 4 & 6 & 14 \\ 6 & 14 & 36 \\ 14 & 36 & 98 \end{bmatrix}, \qquad \mathbf{V}^\top\mathbf{y} = \begin{bmatrix} 4.5 \\ 7.25 \\ 19.75 \end{bmatrix}$$
+
+**Step 3 — Solve:** Gaussian elimination → $\mathbf{c} \approx [1,\,-0.5,\,0.25]^\top$.
+
+**Result:** $p(x) = 1 - 0.5\,x + 0.25\,x^2$.
+
+**Prediction at $x = 1.5$:**
+
+$$p(1.5) = 0.25\cdot1.5^2 - 0.5\cdot1.5 + 1 = 0.5625 - 0.75 + 1 = 0.8125$$
+
+## Pitfalls & Edge Cases
+
+- **Ill-conditioning of the Vandermonde system.** The condition number of $\mathbf{V}^\top\mathbf{V}$
+  grows exponentially with $d$ and with the spread of abscissae. Center and scale the abscissa
+  $x \leftarrow (x - \bar{x})/\sigma_x$ before fitting to reduce condition numbers by orders of
+  magnitude. Recommended for $d \geq 3$ or when abscissae are far from the origin.
+
+- **Degree selection.** Over-fitting occurs when $d$ is too large relative to $n$ or to the
+  signal-to-noise ratio. Keep $d \leq 4$ for typical embedded calibration tasks.
+
+- **Exactly $n = d+1$ points.** The normal equation system has a unique solution equal to the
+  interpolating polynomial; the residual is zero. The system is well-posed only if all abscissae
+  are distinct.
+
+- **Repeated or nearly-coincident abscissae.** $\mathbf{V}^\top\mathbf{V}$ becomes singular or
+  nearly so. Partial-pivoting in the Gaussian solver will flag this via `really_assert`; avoid
+  duplicate $x$ values in practice.
+
+- **Large degree with `float` arithmetic.** Powers $x^d$ for $|x| \gg 1$ can exceed the `float`
+  dynamic range. Centering/scaling eliminates this risk.
+
+## Variants & Generalizations
+
+| Variant                     | Key Difference                                                               |
+|-----------------------------|------------------------------------------------------------------------------|
+| Orthogonal polynomial basis | Uses Legendre/Chebyshev basis instead of monomials; much better conditioning |
+| Weighted least squares      | Each sample weighted differently (e.g., by measurement precision)            |
+| Regularized (Ridge) fitting | Adds $\lambda\|\mathbf{c}\|^2$ to damp large coefficients                    |
+| Constrained fitting         | Enforces derivative constraints at endpoints                                 |
+| Savitzky-Golay smoothing    | Sliding-window polynomial fit for real-time derivative estimation            |
+
+## Applications
+
+- **Sensor linearization** — converting thermistor resistance or pressure-sensor ADC counts to
+  engineering units via a quadratic or cubic polynomial.
+- **Drift and aging compensation** — fitting a polynomial to sampled drift data and subtracting
+  the trend from future measurements.
+- **Compact lookup-table replacement** — replacing a 256-entry table with a degree-3 polynomial
+  evaluated in four MACs.
+- **Calibration curve storage** — a handful of coefficients in flash replace a bulky lookup table.
+
+## Connections to Other Algorithms
+
+```mermaid
+graph LR
+    PF["Polynomial Fitting"]
+    GE["Gaussian Elimination"]
+    LR["Linear Regression"]
+    SG["Savitzky-Golay (planned)"]
+    RLS["Recursive Least Squares"]
+
+    PF --> GE
+    PF -.->|"polynomial features = special case"| LR
+    SG -.->|"local polynomial fit per window"| PF
+    RLS -.->|"online counterpart"| PF
+```
+
+| Algorithm                                                 | Relationship                                                     |
+|-----------------------------------------------------------|------------------------------------------------------------------|
+| [Gaussian Elimination](../solvers/GaussianElimination.md) | Solves the normal equations                                      |
+| [Linear Regression](LinearRegression.md)                  | Polynomial fitting is linear regression with polynomial features |
+| [Recursive Least Squares](RecursiveLeastSquares.md)       | Online / streaming counterpart for time-varying models           |
+
+## References & Further Reading
+
+- Press, W. H., Teukolsky, S. A., Vetterling, W. T. and Flannery, B. P., *Numerical Recipes in C*, 3rd ed., Cambridge University Press, 2007 — Chapter 15 (Modeling of Data).
+- Golub, G. H. and Van Loan, C. F., *Matrix Computations*, 4th ed., Johns Hopkins University Press, 2013 — Chapter 5 (orthogonal factorizations and least squares).
+- Hildebrand, F. B., *Introduction to Numerical Analysis*, 2nd ed., Dover, 1987 — Chapter 7 (least-squares approximation).

doc/estimators/README.md

@@ -4,11 +4,12 @@ Statistical estimation algorithms for fitting models to observed data and making
 
 ## Offline Estimators (Batch)
 
-| Algorithm                                                         | Description                                                             |
-|-------------------------------------------------------------------|-------------------------------------------------------------------------|
-| [Linear Regression](LinearRegression.md)                         | Ordinary least-squares regression using the normal equation             |
-| [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) |
+| Algorithm                                              | Description                                                               |
+|--------------------------------------------------------|---------------------------------------------------------------------------|
+| [Linear Regression](LinearRegression.md)               | Ordinary least-squares regression using the normal equation               |
+| [Polynomial Fitting](PolynomialFitting.md)             | Degree-d polynomial fit via Vandermonde normal equations                  |
+| [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) |
 
 ## Online Estimators (Streaming)
 

numerical/estimators/offline/CMakeLists.txt

@@ -15,12 +15,14 @@ target_link_libraries(numerical.estimators.offline ${NUMERICAL_VISIBILITY}
 target_sources(numerical.estimators.offline PRIVATE
     ExpectationMaximization.hpp
     LinearRegression.hpp
+    PolynomialFitting.hpp
     YuleWalker.hpp
 )
 
 numerical_add_coverage_sources(numerical.estimators.offline
     ExpectationMaximization.cpp
     LinearRegression.cpp
+    PolynomialFitting.cpp
     YuleWalker.cpp
 )
 

numerical/estimators/offline/PolynomialFitting.cpp

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

numerical/estimators/offline/PolynomialFitting.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/Matrix.hpp"
+#include "numerical/solvers/GaussianElimination.hpp"
+#include <type_traits>
+
+namespace estimators
+{
+    template<typename T, std::size_t Samples, std::size_t Degree>
+    class PolynomialFitting
+    {
+        static_assert(std::is_floating_point_v<T>, "PolynomialFitting supports floating-point types");
+        static_assert(Samples >= Degree + 1, "Samples must be >= Degree + 1");
+
+    public:
+        using CoefficientsVector = math::Matrix<T, Degree + 1, 1>;
+        using SamplesVector = math::Matrix<T, Samples, 1>;
+
+        PolynomialFitting() = default;
+
+        OPTIMIZE_FOR_SPEED void Fit(const SamplesVector& x, const SamplesVector& y);
+        T Predict(T xVal) const;
+        const CoefficientsVector& Coefficients() const;
+
+    private:
+        CoefficientsVector coefficients;
+    };
+
+    template<typename T, std::size_t Samples, std::size_t Degree>
+    OPTIMIZE_FOR_SPEED void PolynomialFitting<T, Samples, Degree>::Fit(const SamplesVector& x, const SamplesVector& y)
+    {
+        math::Matrix<T, Samples, Degree + 1> v;
+
+        for (std::size_t i = 0; i < Samples; ++i)
+        {
+            v.at(i, 0) = T{ 1 };
+            for (std::size_t j = 1; j <= Degree; ++j)
+                v.at(i, j) = v.at(i, j - 1) * x.at(i, 0);
+        }
+
+        auto vt = v.Transpose();
+        auto normalMatrix = vt * v;
+        auto rhs = vt * y;
+
+        coefficients = solvers::SolveSystem<T, Degree + 1, 1>(normalMatrix, rhs);
+    }
+
+    template<typename T, std::size_t Samples, std::size_t Degree>
+    T PolynomialFitting<T, Samples, Degree>::Predict(T xVal) const
+    {
+        T acc = coefficients.at(Degree, 0);
+        for (std::size_t j = Degree; j > 0; --j)
+            acc = acc * xVal + coefficients.at(j - 1, 0);
+        return acc;
+    }
+
+    template<typename T, std::size_t Samples, std::size_t Degree>
+    const typename PolynomialFitting<T, Samples, Degree>::CoefficientsVector&
+    PolynomialFitting<T, Samples, Degree>::Coefficients() const
+    {
+        return coefficients;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class PolynomialFitting<float, 8, 2>;
+#endif
+}

numerical/estimators/offline/test/CMakeLists.txt

@@ -10,5 +10,6 @@ target_link_libraries(numerical.estimators.offline_test PUBLIC
 target_sources(numerical.estimators.offline_test PRIVATE
     TestExpectationMaximization.cpp
     TestLinearRegression.cpp
+    TestPolynomialFitting.cpp
     TestYuleWalker.cpp
 )