feat(estimators): add recursive instrumental variables

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, Mel Filterbank / MFCC |
 | [Control Analysis](doc/control_analysis/README.md)                 | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space |
 | [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer |
-| [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 |
+| [Estimators](doc/estimators/README.md)                             | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS |
 | [Filters](doc/filters/README.md)                                   | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS |
 | [Optimization](doc/optimization/README.md)                         | Gradient Descent, SGD / Adam Step Optimisers                         |
 | [Regularization](doc/regularization/README.md)                     | L1 (Lasso), L2 (Ridge)                                              |

doc/estimators/README.md

@@ -14,6 +14,7 @@ Statistical estimation algorithms for fitting models to observed data and making
 
 ## Online Estimators (Streaming)
 
-| Algorithm                                           | Description                                                              |
-|-----------------------------------------------------|--------------------------------------------------------------------------|
-| [Recursive Least Squares](RecursiveLeastSquares.md) | Sample-by-sample parameter estimation with exponential forgetting factor |
+| Algorithm                                                             | Description                                                                           |
+|-----------------------------------------------------------------------|---------------------------------------------------------------------------------------|
+| [Recursive Least Squares](RecursiveLeastSquares.md)                   | Sample-by-sample parameter estimation with exponential forgetting factor              |
+| [Recursive Instrumental Variables](RecursiveInstrumentalVariables.md) | Recursive least squares that stays unbiased when the regressor is measured with noise |

doc/estimators/RecursiveInstrumentalVariables.md

@@ -0,0 +1,135 @@
+# Recursive Instrumental Variables (RIV)
+
+## Overview & Motivation
+
+Recursive least squares assumes the regressor is known exactly and only the output is noisy. When
+the regressor is itself measured — a derivative taken from two noisy samples, a current read
+through a noisy sensor — least squares reads the noise as a weaker dependence on that column and
+biases its coefficient toward zero. The effect is regression dilution, and it does not shrink with
+more data: the estimate converges, but to the wrong value.
+
+The instrumental-variable method removes that bias without modelling the noise. It needs a second
+signal per observation, the *instrument*, that moves with the true regressor but not with its
+noise. A known excitation is the usual choice: the reference a controller injects, a command, or a
+low-noise measurement of the same quantity. The recursive form updates the estimate sample by
+sample at the cost of recursive least squares.
+
+## Mathematical Theory
+
+### Problem Setting
+
+The model is $y = \mathbf{x}^T \boldsymbol{\theta} + \epsilon$, but only $\tilde{\mathbf{x}} =
+\mathbf{x} + \mathbf{v}$ is observed, with $\mathbf{v}$ zero-mean noise. Least squares solves
+$\sum \tilde{\mathbf{x}}\,(y - \tilde{\mathbf{x}}^T \boldsymbol{\theta}) = 0$. Because
+$E[\tilde{\mathbf{x}} \tilde{\mathbf{x}}^T] = R_x + R_v$, its limit is
+
+$$\hat{\boldsymbol{\theta}}_{LS} \to (R_x + R_v)^{-1} R_x\, \boldsymbol{\theta}$$
+
+For a single slope this is the attenuation $\sigma_x^2 / (\sigma_x^2 + \sigma_v^2)$.
+
+### Instrumental-Variable Estimate
+
+Given an instrument $\mathbf{z}$ correlated with $\mathbf{x}$ and uncorrelated with $\mathbf{v}$ and
+$\epsilon$, the estimate solves
+
+$$\sum_{k} \lambda^{n-k}\, \mathbf{z}_k \left(y_k - \tilde{\mathbf{x}}_k^T \boldsymbol{\theta}\right) = 0
+\quad\Rightarrow\quad
+\hat{\boldsymbol{\theta}} = \Big(\sum_k \lambda^{n-k} \mathbf{z}_k \tilde{\mathbf{x}}_k^T\Big)^{-1} \sum_k \lambda^{n-k} \mathbf{z}_k y_k$$
+
+The noise enters only through $E[\mathbf{z}\,\mathbf{v}^T] = 0$, so the estimate is consistent.
+
+### Update Equations
+
+With $P$ the inverse of the weighted cross-moment $\sum \lambda^{n-k} \mathbf{z}_k
+\tilde{\mathbf{x}}_k^T$, a new observation $(\tilde{\mathbf{x}}, \mathbf{z}, y)$ gives:
+
+1. **Gain vector**: $\mathbf{g} = P \mathbf{z}$
+2. **Denominator**: $d = \lambda + \tilde{\mathbf{x}}^T P \mathbf{z}$
+3. **Innovation**: $e = y - \tilde{\mathbf{x}}^T \hat{\boldsymbol{\theta}}$
+4. **Parameter update**: $\hat{\boldsymbol{\theta}} \leftarrow \hat{\boldsymbol{\theta}} + \mathbf{g}\, e / d$
+5. **Matrix update**: $P \leftarrow \frac{1}{\lambda}\left(P - \mathbf{g}\, \tilde{\mathbf{x}}^T P / d\right)$
+
+Steps 2 and 5 are the Sherman–Morrison update of a rank-one, non-symmetric term
+$\mathbf{z}\tilde{\mathbf{x}}^T$. With $\mathbf{z} = \tilde{\mathbf{x}}$ every step reduces to
+recursive least squares.
+
+### Diagnostics
+
+The innovation, the post-update residual and the trace of $P$ are reported as for recursive least
+squares. $P$ is no longer a scaled covariance and need not be symmetric; its trace still falls as
+the instrument excites the parameters.
+
+## Complexity Analysis
+
+| Case    | Time     | Space    | Notes                                            |
+|---------|----------|----------|--------------------------------------------------|
+| Average | $O(p^2)$ | $O(p^2)$ | Two matrix-vector products and one outer product |
+
+The cost per update matches recursive least squares: one more matrix-vector product, for
+$\tilde{\mathbf{x}}^T P$, replaces the symmetric outer product.
+
+## Step-by-Step Walkthrough
+
+A single slope, $y = 2x$, observed through $\tilde{x} = x + v$ with the instrument $z = x$. The noise
+alternates so that it is uncorrelated with $x$ over the four samples. $\lambda = 1$, $P_0 = 1000$,
+$\hat{\theta}_0 = 0$:
+
+| Step | $x$ | $v$  | $\tilde{x}$ | $y$ | Least squares $\hat{\theta}$ | Instrumental $\hat{\theta}$ |
+|------|-----|------|-------------|-----|------------------------------|-----------------------------|
+| 1    | 1   | 0.5  | 1.5         | 2   | 1.333                        | 1.332                       |
+| 2    | 1   | -0.5 | 0.5         | 2   | 1.599                        | 1.999                       |
+| 3    | 2   | 0.5  | 2.5         | 4   | 1.600                        | 1.714                       |
+| 4    | 2   | -0.5 | 1.5         | 4   | 1.818                        | 2.000                       |
+
+After the four samples least squares holds $2\,\sum x^2 / (\sum x^2 + \sum v^2) = 20/11$, the
+attenuated slope, while the instrumental estimate is back at 2: each time the noise has averaged out
+against the instrument, it is exact.
+
+## Pitfalls & Edge Cases
+
+- **Weak instrument**: if $\mathbf{z}$ is barely correlated with $\mathbf{x}$, the cross-moment is
+  near-singular and the estimate's variance grows without bound. The instrument must excite every
+  parameter.
+- **Instrument shares the noise**: any correlation between $\mathbf{z}$ and $\mathbf{v}$, or
+  between $\mathbf{z}$ and $\epsilon$ (a closed loop that feeds measured noise back into the
+  instrument), brings the bias back in proportion.
+- **Zero instrument**: an observation whose instrument is zero leaves $\hat{\boldsymbol{\theta}}$
+  untouched but still divides $P$ by $\lambda$; with forgetting, a long run of such observations
+  grows $P$ as in under-excited least squares. Skip those observations.
+- **Variance**: the method trades bias for variance. For the same data its estimates scatter more
+  than least squares, most of all when the instrument explains only part of the regressor.
+
+## Variants & Generalizations
+
+- **Batch IV**: the closed-form estimate above, solved once.
+- **Two-stage least squares**: regress the regressor on the instruments first, then regress the
+  output on the fitted regressor; with as many instruments as regressors it equals IV.
+- **Bootstrap / refined IV**: build the instrument from a model simulated with the current
+  estimate, as in Young's refined instrumental-variable method for transfer functions.
+- **Columns as their own instruments**: columns known without noise can serve as their own
+  instruments, so only the noisy columns need an external one.
+
+## Applications
+
+- Online identification of a winding's resistance and inductance from a derivative of measured
+  current, with the injected current reference as the instrument
+- Closed-loop system identification, where the reference is independent of the measurement noise
+- Calibration of one sensor against another, both noisy
+- Econometrics and errors-in-variables regression in general
+
+## Connections to Other Algorithms
+
+- **Recursive Least Squares**: the special case $\mathbf{z} = \tilde{\mathbf{x}}$; same cost, same
+  forgetting factor and diagnostics.
+- **Total Least Squares**: also removes errors-in-variables bias, but needs the noise ratio between
+  channels and a batch SVD; IV needs an instrument instead and runs recursively.
+- **Kalman filter**: a Kalman filter with the regressor noise in its model is the optimal
+  alternative when that noise's covariance is known.
+
+## References & Further Reading
+
+- Söderström, T. and Stoica, P., *Instrumental Variable Methods for System Identification*,
+  Springer, 1983.
+- Ljung, L., *System Identification: Theory for the User*, 2nd ed., Prentice Hall, 1999 — §7.6 and
+  §11.3.
+- Young, P.C., *Recursive Estimation and Time-Series Analysis*, 2nd ed., Springer, 2011 — Chapter 6.

numerical/estimators/online/CMakeLists.txt

@@ -11,11 +11,13 @@ target_link_libraries(numerical.estimators.online ${NUMERICAL_VISIBILITY}
 )
 
 target_sources(numerical.estimators.online PRIVATE
+    RecursiveInstrumentalVariables.hpp
     RecursiveLeastSquares.hpp
     LmsAdaptiveFilter.hpp
 )
 
 numerical_add_coverage_sources(numerical.estimators.online
+    RecursiveInstrumentalVariables.cpp
     RecursiveLeastSquares.cpp
     LmsAdaptiveFilter.cpp
 )

numerical/estimators/online/RecursiveInstrumentalVariables.cpp

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

numerical/estimators/online/RecursiveInstrumentalVariables.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include "infra/util/ReallyAssert.hpp"
+#include "numerical/estimators/Estimator.hpp"
+#include "numerical/estimators/online/RecursiveLeastSquares.hpp"
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/Math.hpp"
+#include <type_traits>
+
+namespace estimators
+{
+    template<typename T, std::size_t Features>
+    class RecursiveInstrumentalVariables
+    {
+        static_assert(std::is_floating_point_v<T>, "RecursiveInstrumentalVariables supports floating-point types");
+
+    public:
+        using CoefficientsMatrix = typename OnlineEstimator<T, Features>::CoefficientsMatrix;
+        using DesignMatrix = typename OnlineEstimator<T, Features>::DesignMatrix;
+        using InputMatrix = typename OnlineEstimator<T, Features>::InputMatrix;
+        using EstimationMetrics = typename OnlineEstimator<T, Features>::EstimationMetrics;
+
+        RecursiveInstrumentalVariables(T initialCovariance, T forgettingFactor);
+
+        EstimationMetrics Update(const InputMatrix& regressor, const InputMatrix& instrument, const math::Matrix<T, 1, 1>& y);
+        const CoefficientsMatrix& Coefficients() const;
+        void SetCoefficients(const CoefficientsMatrix& initial);
+
+    private:
+        CoefficientsMatrix theta{};
+        DesignMatrix covariance;
+        T lambda;
+        T lambdaInverse;
+        EstimationMetrics metrics{};
+    };
+
+    template<typename T, std::size_t Features>
+    RecursiveInstrumentalVariables<T, Features>::RecursiveInstrumentalVariables(T initialCovariance, T forgettingFactor)
+        : covariance{ DesignMatrix::Identity() * initialCovariance }
+        , lambda{ forgettingFactor }
+        , lambdaInverse{ T(1) / forgettingFactor }
+    {
+        really_assert((RecursiveLeastSquares<T, Features>::IsForgettingFactorValid(forgettingFactor)));
+        really_assert(math::IsFinite(initialCovariance) && initialCovariance > T(0));
+    }
+
+    template<typename T, std::size_t Features>
+    OPTIMIZE_FOR_SPEED
+        typename RecursiveInstrumentalVariables<T, Features>::EstimationMetrics
+        RecursiveInstrumentalVariables<T, Features>::Update(const InputMatrix& regressor, const InputMatrix& instrument, const math::Matrix<T, 1, 1>& y)
+    {
+        const auto gain = covariance * instrument;
+        const auto denominatorInverse = T(1) / (lambda + (regressor.Transpose() * gain).at(0, 0));
+        const auto error = y.at(0, 0) - (regressor.Transpose() * theta).at(0, 0);
+        theta += gain * (error * denominatorInverse);
+        covariance = (covariance - gain * (regressor.Transpose() * covariance) * denominatorInverse) * lambdaInverse;
+
+        metrics.innovation = error;
+        metrics.residual = y.at(0, 0) - (regressor.Transpose() * theta).at(0, 0);
+        metrics.uncertainty = covariance.Trace();
+
+        return metrics;
+    }
+
+    template<typename T, std::size_t Features>
+    const typename RecursiveInstrumentalVariables<T, Features>::CoefficientsMatrix& RecursiveInstrumentalVariables<T, Features>::Coefficients() const
+    {
+        return theta;
+    }
+
+    template<typename T, std::size_t Features>
+    void RecursiveInstrumentalVariables<T, Features>::SetCoefficients(const CoefficientsMatrix& initial)
+    {
+        theta = initial;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class RecursiveInstrumentalVariables<float, 2>;
+#endif
+}

numerical/estimators/online/test/CMakeLists.txt

@@ -10,6 +10,7 @@ target_link_libraries(numerical.estimators.online_test PUBLIC
 numerical_link_qemu_runtime(numerical.estimators.online_test)
 
 target_sources(numerical.estimators.online_test PRIVATE
+    TestRecursiveInstrumentalVariables.cpp
     TestRecursiveLeastSquares.cpp
     TestLmsAdaptiveFilter.cpp
 )

numerical/estimators/online/test/TestRecursiveInstrumentalVariables.cpp

@@ -0,0 +1,83 @@
+#include "numerical/estimators/online/RecursiveInstrumentalVariables.hpp"
+#include "numerical/estimators/online/RecursiveLeastSquares.hpp"
+#include "numerical/math/Tolerance.hpp"
+#include <cmath>
+#include <gtest/gtest.h>
+#include <random>
+
+namespace
+{
+    class TestRecursiveInstrumentalVariables
+        : public ::testing::Test
+    {
+    protected:
+        static constexpr std::size_t Features = 2;
+
+        using Riv = estimators::RecursiveInstrumentalVariables<float, Features>;
+        using Rls = estimators::RecursiveLeastSquares<float, Features>;
+        using Input = Riv::InputMatrix;
+        using Output = math::Matrix<float, 1, 1>;
+
+        static constexpr float trueIntercept = 0.5f;
+        static constexpr float trueSlope = 2.0f;
+
+        Input MakeInput(float bias, float x)
+        {
+            Input result;
+            result.at(0, 0) = bias;
+            result.at(1, 0) = x;
+            return result;
+        }
+
+        Output MakeOutput(float y)
+        {
+            Output result;
+            result.at(0, 0) = y;
+            return result;
+        }
+
+        std::mt19937 engine{ 5u };
+        std::normal_distribution<float> unit{ 0.0f, 1.0f };
+    };
+}
+
+TEST_F(TestRecursiveInstrumentalVariables, MatchesRecursiveLeastSquaresWhenTheRegressorIsItsOwnInstrument)
+{
+    Riv riv{ 1000.0f, 0.99f };
+    Rls rls{ 1000.0f, 0.99f };
+    const auto seed = MakeInput(1.0f, 1.0f);
+    riv.SetCoefficients(seed);
+    rls.SetCoefficients(seed);
+    Riv::EstimationMetrics rivMetrics{};
+    Rls::EstimationMetrics rlsMetrics{};
+
+    for (int i{ 0 }; i != 200; ++i)
+    {
+        const float x = std::sin(0.1f * static_cast<float>(i));
+        const auto regressor = MakeInput(1.0f, x);
+        const auto output = MakeOutput(trueIntercept + trueSlope * x + 0.1f * unit(engine));
+        rivMetrics = riv.Update(regressor, regressor, output);
+        rlsMetrics = rls.Update(regressor, output);
+    }
+
+    EXPECT_NEAR(riv.Coefficients().at(0, 0), rls.Coefficients().at(0, 0), math::Tolerance<float>());
+    EXPECT_NEAR(riv.Coefficients().at(1, 0), rls.Coefficients().at(1, 0), math::Tolerance<float>());
+    EXPECT_NEAR(rivMetrics.innovation, rlsMetrics.innovation, math::Tolerance<float>());
+    EXPECT_NEAR(rivMetrics.residual, rlsMetrics.residual, math::Tolerance<float>());
+    EXPECT_NEAR(rivMetrics.uncertainty, rlsMetrics.uncertainty, math::Tolerance<float>());
+}
+
+TEST_F(TestRecursiveInstrumentalVariables, RecoversTheSlopeWhenOnlyTheRegressorIsNoisy)
+{
+    Riv riv{ 1000.0f, 1.0f };
+
+    for (int i{ 0 }; i != 4000; ++i)
+    {
+        const float x = unit(engine);
+        const float observed = x + 0.5f * unit(engine);
+        riv.Update(MakeInput(1.0f, observed), MakeInput(1.0f, x), MakeOutput(trueIntercept + trueSlope * x));
+    }
+
+    EXPECT_NEAR(riv.Coefficients().at(0, 0), trueIntercept, 0.05f);
+    EXPECT_NEAR(riv.Coefficients().at(1, 0), trueSlope, 0.1f);
+}