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); +}