From eb8346d1cec49a97bac6ea0b844f82e64f21627e Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Tue, 20 Oct 2026 18:38:49 +0000 Subject: [PATCH] feat(estimators): add recursive instrumental variables Recursive least squares with its gain taken along an instrument instead of the regressor. When a regressor is itself measured, least squares reads its noise as a weaker dependence on that column and converges to an attenuated coefficient. An instrument that moves with the true regressor but not with its noise, such as an injected excitation, removes that bias. With the regressor as its own instrument the recursion is the recursive least-squares one, at the same cost. e-foc uses it to identify a winding's inductance online from a derivative of measured current, with the injected d-axis current reference as the instrument: on a bench motor the least-squares estimate read the inductance 55 % low and this one reads it within 8 %. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_011XTZKRSD8pDL7QvwFrVnoK --- README.md | 2 +- doc/estimators/README.md | 7 +- .../RecursiveInstrumentalVariables.md | 135 ++++++++++++++++++ numerical/estimators/online/CMakeLists.txt | 2 + .../online/RecursiveInstrumentalVariables.cpp | 6 + .../online/RecursiveInstrumentalVariables.hpp | 80 +++++++++++ .../estimators/online/test/CMakeLists.txt | 1 + .../TestRecursiveInstrumentalVariables.cpp | 83 +++++++++++ 8 files changed, 312 insertions(+), 4 deletions(-) create mode 100644 doc/estimators/RecursiveInstrumentalVariables.md create mode 100644 numerical/estimators/online/RecursiveInstrumentalVariables.cpp create mode 100644 numerical/estimators/online/RecursiveInstrumentalVariables.hpp create mode 100644 numerical/estimators/online/test/TestRecursiveInstrumentalVariables.cpp diff --git a/README.md b/README.md index 05524e6a..06bc7a39 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, 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) | diff --git a/doc/estimators/README.md b/doc/estimators/README.md index 98d548d5..58901867 100644 --- a/doc/estimators/README.md +++ b/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 | diff --git a/doc/estimators/RecursiveInstrumentalVariables.md b/doc/estimators/RecursiveInstrumentalVariables.md new file mode 100644 index 00000000..31910088 --- /dev/null +++ b/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. diff --git a/numerical/estimators/online/CMakeLists.txt b/numerical/estimators/online/CMakeLists.txt index 3423b0fa..1bbdca79 100644 --- a/numerical/estimators/online/CMakeLists.txt +++ b/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 ) diff --git a/numerical/estimators/online/RecursiveInstrumentalVariables.cpp b/numerical/estimators/online/RecursiveInstrumentalVariables.cpp new file mode 100644 index 00000000..74d7630f --- /dev/null +++ b/numerical/estimators/online/RecursiveInstrumentalVariables.cpp @@ -0,0 +1,6 @@ +#include "numerical/estimators/online/RecursiveInstrumentalVariables.hpp" + +namespace estimators +{ + template class RecursiveInstrumentalVariables; +} diff --git a/numerical/estimators/online/RecursiveInstrumentalVariables.hpp b/numerical/estimators/online/RecursiveInstrumentalVariables.hpp new file mode 100644 index 00000000..3d95d14a --- /dev/null +++ b/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 + +namespace estimators +{ + template + class RecursiveInstrumentalVariables + { + static_assert(std::is_floating_point_v, "RecursiveInstrumentalVariables supports floating-point types"); + + public: + using CoefficientsMatrix = typename OnlineEstimator::CoefficientsMatrix; + using DesignMatrix = typename OnlineEstimator::DesignMatrix; + using InputMatrix = typename OnlineEstimator::InputMatrix; + using EstimationMetrics = typename OnlineEstimator::EstimationMetrics; + + RecursiveInstrumentalVariables(T initialCovariance, T forgettingFactor); + + EstimationMetrics Update(const InputMatrix& regressor, const InputMatrix& instrument, const math::Matrix& y); + const CoefficientsMatrix& Coefficients() const; + void SetCoefficients(const CoefficientsMatrix& initial); + + private: + CoefficientsMatrix theta{}; + DesignMatrix covariance; + T lambda; + T lambdaInverse; + EstimationMetrics metrics{}; + }; + + template + RecursiveInstrumentalVariables::RecursiveInstrumentalVariables(T initialCovariance, T forgettingFactor) + : covariance{ DesignMatrix::Identity() * initialCovariance } + , lambda{ forgettingFactor } + , lambdaInverse{ T(1) / forgettingFactor } + { + really_assert((RecursiveLeastSquares::IsForgettingFactorValid(forgettingFactor))); + really_assert(math::IsFinite(initialCovariance) && initialCovariance > T(0)); + } + + template + OPTIMIZE_FOR_SPEED + typename RecursiveInstrumentalVariables::EstimationMetrics + RecursiveInstrumentalVariables::Update(const InputMatrix& regressor, const InputMatrix& instrument, const math::Matrix& 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 + const typename RecursiveInstrumentalVariables::CoefficientsMatrix& RecursiveInstrumentalVariables::Coefficients() const + { + return theta; + } + + template + void RecursiveInstrumentalVariables::SetCoefficients(const CoefficientsMatrix& initial) + { + theta = initial; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class RecursiveInstrumentalVariables; +#endif +} diff --git a/numerical/estimators/online/test/CMakeLists.txt b/numerical/estimators/online/test/CMakeLists.txt index bf0178a0..9f0e271f 100644 --- a/numerical/estimators/online/test/CMakeLists.txt +++ b/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 ) diff --git a/numerical/estimators/online/test/TestRecursiveInstrumentalVariables.cpp b/numerical/estimators/online/test/TestRecursiveInstrumentalVariables.cpp new file mode 100644 index 00000000..bb00a3cd --- /dev/null +++ b/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 +#include +#include + +namespace +{ + class TestRecursiveInstrumentalVariables + : public ::testing::Test + { + protected: + static constexpr std::size_t Features = 2; + + using Riv = estimators::RecursiveInstrumentalVariables; + using Rls = estimators::RecursiveLeastSquares; + using Input = Riv::InputMatrix; + using Output = math::Matrix; + + 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 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(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()); + EXPECT_NEAR(riv.Coefficients().at(1, 0), rls.Coefficients().at(1, 0), math::Tolerance()); + EXPECT_NEAR(rivMetrics.innovation, rlsMetrics.innovation, math::Tolerance()); + EXPECT_NEAR(rivMetrics.residual, rlsMetrics.residual, math::Tolerance()); + EXPECT_NEAR(rivMetrics.uncertainty, rlsMetrics.uncertainty, math::Tolerance()); +} + +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); +}