From 0c6304da4c365b50f334eec6f721b5fadddb3b58 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 2 Aug 2026 10:35:05 +0000 Subject: [PATCH 1/2] add mrac --- README.md | 2 +- ROADMAP.md | 291 ------------------ .../ModelReferenceAdaptiveControl.md | 130 ++++++++ doc/nonlinear_control/README.md | 5 +- numerical/nonlinear_control/CMakeLists.txt | 2 + .../ModelReferenceAdaptiveControl.cpp | 6 + .../ModelReferenceAdaptiveControl.hpp | 102 ++++++ .../nonlinear_control/test/CMakeLists.txt | 1 + .../TestModelReferenceAdaptiveControl.cpp | 251 +++++++++++++++ .../explanation.md | 38 --- .../implementation.md | 91 ------ .../ModelReferenceAdaptiveControl/tests.md | 71 ----- 12 files changed, 496 insertions(+), 494 deletions(-) create mode 100644 doc/nonlinear_control/ModelReferenceAdaptiveControl.md create mode 100644 numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp create mode 100644 numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp create mode 100644 numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp delete mode 100644 roadmap/nonlinear_control/ModelReferenceAdaptiveControl/explanation.md delete mode 100644 roadmap/nonlinear_control/ModelReferenceAdaptiveControl/implementation.md delete mode 100644 roadmap/nonlinear_control/ModelReferenceAdaptiveControl/tests.md diff --git a/README.md b/README.md index 85ad43b8..b2bf3371 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD | diff --git a/ROADMAP.md b/ROADMAP.md index 643fccef..a5d3f6ec 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,273 +26,12 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|------------------------------------------------------|---------------------------|------------| -| 47 | Model Reference Adaptive Control (MRAC) | `nonlinear_control` (new) | ★★★★★ | Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family unit-test reference [`TESTING.md`](TESTING.md) depends on but which the library does not yet expose. Detailed below under [Evaluation & metrics primitives](#evaluation--metrics-primitives). ---- - -## Tier 1 — Trivial primitives ★☆☆☆☆ - -### 1. Exponential Moving Average (one-pole / leaky integrator) -- **What:** Single-pole recursive smoother `y[n] = α·x[n] + (1−α)·y[n−1]`. -- **Embedded value:** The cheapest low-pass filter — one multiply-add, one state word. Ubiquitous for sensor smoothing and DC tracking. -- **Algorithm / paper:** S. W. Smith, *The Scientist and Engineer's Guide to DSP*, Ch. 19 (Recursive/single-pole filters). -- **Reuses:** Trivial; templated scalar state. - -### 2. Moving Average (running-sum boxcar) -- **What:** Length-`N` boxcar via incremental running sum: `sum += x[n] − x[n−N]`. -- **Embedded value:** Optimal white-noise reducer per computation; O(1) per sample independent of window length. -- **Algorithm / paper:** S. W. Smith, *DSP Guide*, Ch. 15 (Moving Average Filters, recursive form). -- **Reuses:** `math::RecursiveBuffer` for the delay line. - -### 3. Saturation / rate-limiter / slew-rate blocks -- **What:** Composable actuator-constraint primitives: `clamp(u, lo, hi)` and `slew = clamp(Δu, −rate·Ts, +rate·Ts)`. -- **Embedded value:** Reusable safety wrappers around any controller output; a prerequisite for correct anti-windup. -- **Algorithm / paper:** K. J. Åström, R. M. Murray, *Feedback Systems* (2008), actuator saturation & windup. -- **Reuses:** Scalar/`Vector` templates. - -### 5. Peak / zero-crossing / RMS-envelope detectors -- **What:** Lightweight feature extractors: rising/falling peak hold, sign-change (zero-crossing) counter, and RMS envelope via one-pole on `x²`. -- **Embedded value:** Cheap building blocks for frequency estimation, VU metering, activity detection, and event triggers. -- **Algorithm / paper:** R. G. Lyons, *Understanding Digital Signal Processing*, 3rd ed. -- **Reuses:** Item 1 (one-pole), `math::Statistics`. - ---- - -## Tier 2 — Easy ★★☆☆☆ - -### 8. Alpha-beta / alpha-beta-gamma filter -- **What:** Fixed-gain steady-state tracker for position/velocity(/acceleration) states. -- **Embedded value:** Delivers most of the benefit of a Kalman filter at a fraction of the cost — no online covariance propagation. -- **Algorithm / paper:** P. Kalata, "The tracking index: A generalized parameter for α-β and α-β-γ target trackers," *IEEE Trans. AES*, 20(2), 1984. -- **Reuses:** `math::LinearTimeInvariant`, `filters/active` patterns. - -### 10. Gain-scheduled controller -- **What:** Interpolates a set of precomputed controller gains across a scheduling variable (speed, load, operating point). -- **Embedded value:** Extends linear controllers to mildly nonlinear plants without online redesign. -- **Algorithm / paper:** W. J. Rugh, J. S. Shamma, "Research on gain scheduling," *Automatica*, 36(10), 2000. -- **Reuses:** LUT + linear/bilinear interpolation; existing controllers. - -### 11. Convolution & correlation utilities -- **What:** Linear/circular convolution and auto/cross-correlation over bounded vectors. -- **Embedded value:** Matched filtering, template matching, delay/lag estimation, system-response measurement. -- **Algorithm / paper:** A. V. Oppenheim, R. W. Schafer, *Discrete-Time Signal Processing*, Ch. 2 & 8. -- **Reuses:** `math::Vector`, `math::Toeplitz`, FFT for fast convolution. - -### 12. Polynomial least-squares curve fitting -- **What:** Fit a degree-`d` polynomial via the Vandermonde normal equations. -- **Embedded value:** Sensor calibration curves, drift/trend modeling, lightweight interpolation tables. -- **Algorithm / paper:** Press et al., *Numerical Recipes*, Ch. 15 (Modeling of Data / general linear least squares). -- **Reuses:** `solvers::GaussianElimination` or `solvers::CholeskyDecomposition`. - -### 13. Goertzel algorithm -- **What:** Single-bin DFT via a second-order recurrence — detects one target frequency without a full FFT or input buffer. -- **Embedded value:** DTMF/tone detection, notch monitoring, sync-tone recovery; O(N) with O(1) memory. -- **Algorithm / paper:** G. Goertzel, "An Algorithm for the Evaluation of Finite Trigonometric Series," *American Mathematical Monthly*, 65(1), 1958. -- **Reuses:** `math::ComplexNumber`, `TrigonometricFunctions`. - -### 14. CIC (Cascaded Integrator-Comb) filter -- **What:** Multiplier-free decimator/interpolator built from integrator and comb stages. -- **Embedded value:** The canonical front-end for sigma-delta ADC/DAC rate conversion — no multipliers, integer-only. -- **Algorithm / paper:** E. B. Hogenauer, "An Economical Class of Digital Filters for Decimation and Interpolation," *IEEE Trans. ASSP*, 29(2), 1981. -- **Reuses:** Integer accumulators; `math::RecursiveBuffer`. - ---- - -## Tier 3 — Moderate ★★★☆☆ - -### 16. Notch / comb filter -- **What:** Narrow-band rejection (notch biquad) and periodic comb rejection. -- **Embedded value:** Removes 50/60 Hz mains hum and harmonic interference from bio-signals and instrumentation. -- **Algorithm / paper:** Bristow-Johnson EQ cookbook (notch); Lyons, *Understanding DSP* (comb filters). -- **Reuses:** Item 15 (biquad), `math::RecursiveBuffer`. - -### 17. Lead-lag compensator -- **What:** Classic frequency-domain compensator `C(s) = K·(s+z)/(s+p)` discretized (Tustin) to a first-order section. -- **Embedded value:** Phase-margin shaping and bandwidth extension where a full state-space design is overkill. -- **Algorithm / paper:** G. F. Franklin, J. D. Powell, A. Emami-Naeini, *Feedback Control of Dynamic Systems*. -- **Reuses:** Item 15 realization; `controllers`. - -### 18. Quaternion type -- **What:** Unit-quaternion class: Hamilton product, conjugate/inverse, normalization, rotation of vectors, SLERP, ↔ rotation-matrix/Euler conversions. -- **Embedded value:** Singularity-free attitude representation; **prerequisite for AHRS (item 33)** and 3D robotics. -- **Algorithm / paper:** J. B. Kuipers, *Quaternions and Rotation Sequences* (1999); K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH*, 1985 (SLERP). -- **Reuses:** [Geometry3D.hpp](numerical/math/Geometry3D.hpp) (`Vector3`, `Matrix3`, Rodrigues). - -### 19. Luenberger observer + pole placement (Ackermann) -- **What:** Deterministic full/reduced-order state observer with gains placed by Ackermann's formula. -- **Embedded value:** Reconstructs unmeasured states for state-feedback control; the deterministic counterpart to the Kalman filter. Currently only referenced in [Lqg.md](doc/controllers/Lqg.md), not implemented. -- **Algorithm / paper:** D. Luenberger, "An Introduction to Observers," *IEEE Trans. Automatic Control*, 16(6), 1971; Ackermann's formula (Franklin et al.). -- **Reuses:** `math::Matrix`, `StateFeedbackController`, `math::LinearTimeInvariant`. - -### 20. Integral / servo state feedback (LQI) -- **What:** Augments the plant with integral-of-error states before LQR design for zero steady-state tracking error. -- **Embedded value:** Removes steady-state offset under constant references/disturbances — the practical version of LQR. -- **Algorithm / paper:** B. D. O. Anderson, J. B. Moore, *Optimal Control: Linear Quadratic Methods* (1990). -- **Reuses:** [Lqr.hpp](numerical/controllers/implementations/Lqr.hpp), [DARE](numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp). - -### 22. Savitzky-Golay filter -- **What:** Convolution smoother that fits a local polynomial, preserving peak height/width and giving smoothed derivatives. -- **Embedded value:** Spectroscopy, ECG/PPG, and any signal where peaks matter and moving-average distortion is unacceptable. -- **Algorithm / paper:** A. Savitzky, M. J. E. Golay, "Smoothing and Differentiation of Data by Simplified Least Squares Procedures," *Analytical Chemistry*, 36(8), 1964. -- **Reuses:** `constexpr` coefficient tables; FIR convolution (item 11). - -### 23. CORDIC -- **What:** Iterative shift-add engine for `sin`/`cos`, `atan2`, magnitude, and vector rotation — no multiplier required. -- **Embedded value:** Trig and Cartesian↔polar on FPU-less MCUs; naturally fixed-point, deterministic cycle count. -- **Algorithm / paper:** J. Volder, "The CORDIC Trigonometric Computing Technique," *IRE Trans. Electronic Computers*, EC-8(3), 1959; R. Andraka survey, 1998. -- **Reuses:** `math::QNumber`; complements `TrigonometricFunctions`. - -### 25. Real-input FFT (RFFT) -- **What:** FFT specialized for real signals using the complex-pack (N/2-point) trick. -- **Embedded value:** ~2× throughput and half the memory versus a complex FFT on real ADC data. -- **Algorithm / paper:** H. Sorensen, D. Jones, M. Heideman, C. Burrus, "Real-valued fast Fourier transform algorithms," *IEEE Trans. ASSP*, 35(6), 1987. -- **Reuses:** [FastFourierTransform.hpp](numerical/analysis/FastFourierTransform.hpp), `math::ComplexNumber`. - -### 26. Controllability / Observability matrices & Gramians -- **What:** Build controllability/observability matrices (rank test) and Gramians (via Lyapunov) for a state-space model. -- **Embedded value:** Design-time verification that a plant is controllable/observable before deploying an observer or LQR. -- **Algorithm / paper:** R. E. Kalman canonical structure (1960); P. Antsaklis, A. Michel, *A Linear Systems Primer*. -- **Reuses:** `math::LinearTimeInvariant`, `math::Matrix`; Gramians need item 31. - ---- - -## Tier 4 — Advanced ★★★★☆ - -### 27. QR decomposition (Householder / Givens) *(float-first)* -- **What:** `A = QR` via Householder reflections (or Givens rotations for sparse/streaming updates). -- **Embedded value:** Numerically robust least-squares and the workhorse behind eigen/SVD and square-root filtering. -- **Algorithm / paper:** A. Householder, "Unitary Triangularization of a Nonsymmetric Matrix," *JACM*, 5(4), 1958; Golub & Van Loan, *Matrix Computations*, Ch. 5. -- **Reuses:** `math::Matrix`; foundational for items 39, 42, 43, 44. - -### 28. LU decomposition with partial pivoting *(float-first)* -- **What:** `PA = LU` for general linear solves, determinant, and matrix inverse. -- **Embedded value:** General-purpose dense solver where Cholesky (SPD-only) does not apply. -- **Algorithm / paper:** Golub & Van Loan, *Matrix Computations*, Ch. 3 (GEPP). -- **Reuses:** [GaussianElimination.hpp](numerical/solvers/GaussianElimination.hpp) (factored form), `math::Matrix`. - -### 29. Matrix exponential (scaling & squaring + Padé) *(float-first)* -- **What:** `expm(A)` via scaling-and-squaring with a Padé approximant. -- **Embedded value:** Core building block for exact discretization, continuous Gramians, and linear-system simulation. -- **Algorithm / paper:** C. Moler, C. Van Loan, "Nineteen Dubious Ways to Compute the Exponential of a Matrix, Twenty-Five Years Later," *SIAM Review*, 45(1), 2003; N. Higham, scaling-and-squaring, 2005. -- **Reuses:** `math::Matrix`; **unblocks items 30, 31, 26.** - -### 31. Lyapunov / Sylvester equation solvers *(float-first)* -- **What:** Solve `AX + XB = C` (Sylvester) and discrete/continuous Lyapunov equations. -- **Embedded value:** Stability certificates, controllability/observability Gramians, robust-control synthesis. -- **Algorithm / paper:** R. Bartels, G. Stewart, "Solution of the Matrix Equation AX + XB = C," *Comm. ACM*, 15(9), 1972. -- **Reuses:** `math::Matrix`, item 27 (Schur/QR building blocks). - -### 33. Madgwick / Mahony AHRS *(float-first)* -- **What:** Quaternion-based orientation filter fusing gyro + accel (+ mag): Madgwick's gradient-descent correction or Mahony's passive complementary filter on SO(3). -- **Embedded value:** The de-facto attitude estimator for drones, robots, and wearables — cheaper and more robust than a full quaternion EKF. -- **Algorithm / paper:** S. Madgwick, "An efficient orientation filter for inertial and inertial/magnetic sensor arrays," 2010; R. Mahony, T. Hamel, J.-M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," *IEEE Trans. AC*, 53(5), 2008. -- **Reuses:** **Item 18 (Quaternion)**, `math::Geometry3D`. - - -### 36. Active Disturbance Rejection Control (ADRC + ESO) *(float-first)* -- **What:** Extended State Observer estimates total disturbance as an augmented state; a feedback law cancels it in real time. -- **Embedded value:** Near model-free, strongly robust motion control; increasingly standard in industrial drives. -- **Algorithm / paper:** J. Han, "From PID to Active Disturbance Rejection Control," *IEEE Trans. Ind. Electron.*, 56(3), 2009; Z. Gao, bandwidth parameterization, *ACC*, 2003. -- **Reuses:** Item 19 (observer), `math::Matrix`, new `robust_control/` module. - -### 37. Hilbert transform / analytic signal / envelope *(float-first)* -- **What:** Compute the analytic signal (via FFT or a Type-III/IV FIR) for instantaneous amplitude, phase, and frequency. -- **Embedded value:** AM demodulation, envelope detection, vibration/bearing analysis, single-sideband processing. -- **Algorithm / paper:** S. L. Marple, "Computing the Discrete-Time Analytic Signal via FFT," *IEEE Trans. Signal Processing*, 47(9), 1999. -- **Reuses:** Items 25/`FastFourierTransform`, FIR. - -### 38. Discrete Wavelet Transform (Haar / Daubechies) *(float-first)* -- **What:** Multiresolution analysis via a quadrature-mirror analysis/synthesis filter bank. -- **Embedded value:** Denoising, compression, and time-frequency features for condition monitoring and edge-ML pipelines. -- **Algorithm / paper:** S. Mallat, "A Theory for Multiresolution Signal Decomposition," *IEEE TPAMI*, 11(7), 1989; I. Daubechies, *Ten Lectures on Wavelets* (1992). -- **Reuses:** FIR filter banks, `math::RecursiveBuffer`. - -### 39. Square-root / Information Kalman filter *(float-first)* -- **What:** Propagate a Cholesky/QR factor of the covariance (square-root form) or its inverse (information form). -- **Embedded value:** Guaranteed positive-definite covariance and better conditioning — critical for reduced-precision hardware and multi-sensor fusion. -- **Algorithm / paper:** P. Kaminski, A. Bryson, S. Schmidt, "Discrete Square Root Filtering: A Survey of Current Techniques," *IEEE Trans. AC*, 16(6), 1971. -- **Reuses:** [KalmanFilterBase.hpp](numerical/filters/active/KalmanFilterBase.hpp), [Cholesky](numerical/solvers/CholeskyDecomposition.hpp), item 27. - ---- - -## Tier 5 — Hard / research-grade ★★★★★ - -### ~~42. Symmetric eigenvalue solver (Jacobi)~~ *(float-first)* ✓ Done -- **What:** Cyclic Jacobi rotations for the eigenvalues/vectors of a symmetric matrix. -- **Embedded value:** PCA/feature extraction, modal analysis, covariance conditioning, Gramian analysis. -- **Algorithm / paper:** Golub & Van Loan, *Matrix Computations*, Ch. 8 (symmetric eigenproblem / cyclic Jacobi). -- **Reuses:** `math::Matrix`, item 27. - -### 43. Singular Value Decomposition (Golub-Kahan) *(float-first)* -- **What:** `A = UΣVᵀ` via Golub-Kahan bidiagonalization + implicit QR sweeps. -- **Embedded value:** Pseudo-inverse, rank/condition estimation, model reduction, robust least squares — foundational. -- **Algorithm / paper:** G. Golub, W. Kahan, "Calculating the Singular Values and Pseudo-Inverse of a Matrix," *SIAM J. Numer. Anal.*, 2(2), 1965; Golub & Reinsch, 1970. -- **Reuses:** Items 27 & 42. - -### 44. Total Least Squares *(float-first)* -- **What:** Errors-in-variables fitting where both inputs and outputs are noisy (SVD-based solution). -- **Embedded value:** Accurate calibration and system identification when the regressors themselves are measured with noise. -- **Algorithm / paper:** G. Golub, C. Van Loan, "An Analysis of the Total Least Squares Problem," *SIAM J. Numer. Anal.*, 17(6), 1980. -- **Reuses:** **Item 43 (SVD)**, `estimators/offline`. - -### 45. IIR filter design (Butterworth / Chebyshev + bilinear) *(float-first)* -- **What:** Generate SOS/biquad coefficients on-device from an analog prototype via the bilinear transform. -- **Embedded value:** Runtime-reconfigurable filters (adjustable cutoff/order) without a host toolchain or hardcoded tables. -- **Algorithm / paper:** T. W. Parks, C. S. Burrus, *Digital Filter Design* (1987); bilinear transform — Oppenheim & Schafer, *DTSP*. -- **Reuses:** Item 15 (biquad target), `math::ComplexNumber`, item 23 (root placement). - -### ~~46. H∞ state-feedback control~~ *(float-first)* ✓ Done -- **What:** Robust optimal control minimizing the worst-case disturbance-to-error gain via a Riccati/LMI solution. -- **Embedded value:** Guaranteed performance under bounded model uncertainty for safety-critical loops. -- **Algorithm / paper:** J. Doyle, K. Glover, P. Khargonekar, B. Francis, "State-Space Solutions to Standard H₂ and H∞ Control Problems," *IEEE Trans. AC*, 34(8), 1989. -- **Reuses:** [DARE](numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp), items 29 & 31, new `robust_control/` module. - -### 47. Model Reference Adaptive Control (MRAC) *(float-first)* -- **What:** Online parameter adaptation (MIT rule / Lyapunov redesign) so the plant tracks a reference model. -- **Embedded value:** Self-tuning control for plants with slowly-varying or unknown parameters. -- **Algorithm / paper:** K. J. Åström, B. Wittenmark, *Adaptive Control* (1995); K. Narendra, A. Annaswamy, *Stable Adaptive Systems* (1989). -- **Reuses:** `estimators/online` (RLS), `math::LinearTimeInvariant`, new `nonlinear_control/` module. - ---- - -## Dependency notes - -Implement prerequisites first to avoid rework: - -- **18 Quaternion** → 33 Madgwick/Mahony AHRS -- **29 Matrix exponential** → 30 `c2d`, 31 Lyapunov (continuous), 26 continuous Gramians -- **27 QR** → 39 square-root KF, 42 Jacobi eigen, 43 SVD -- **42 eigen + 27 QR** → 43 SVD → 44 Total Least Squares -- **19 Luenberger observer** → 35 DOB, 36 ADRC -- **15 Biquad** → 16 notch/comb, 17 lead-lag, 45 IIR design -- **DARE (exists) + 29 + 31** → 46 H∞ - -```mermaid -graph LR - Q18[18 Quaternion] --> A33[33 AHRS] - E29[29 expm] --> C30[30 c2d] - E29 --> L31[31 Lyapunov] - E29 --> G26[26 Gramians] - QR27[27 QR] --> SR39[39 sqrt-KF] - QR27 --> EIG42[42 Eigen] - QR27 --> SVD43[43 SVD] - EIG42 --> SVD43 - SVD43 --> TLS44[44 TLS] - OBS19[19 Observer] --> DOB35[35 DOB] - OBS19 --> ADRC36[36 ADRC] - BIQ15[15 Biquad] --> NC16[16 Notch] - BIQ15 --> LL17[17 Lead-lag] - BIQ15 --> DES45[45 IIR design] - DARE[(DARE exists)] --> H46[46 H-inf] - E29 --> H46 - L31 --> H46 -``` - ## Evaluation & metrics primitives Reusable quantities that quantify *how well* an algorithm behaves — the properties the per-family @@ -303,41 +42,11 @@ and [`control_analysis/FrequencyResponse`](numerical/control_analysis/FrequencyR magnitude/phase. The items below are the missing pieces. All are **float-only**, no-heap, and operate on bounded `math::Vector`/`math::Matrix` inputs; tests are `TEST_F` on `float`. -### 51. Spectral radius / discrete stability margin ★★★☆☆ — `math` -- **What:** Dominant `|eigenvalue|` of a square (state/companion) matrix; `IsSchurStable` (all - `|λ| < 1`) and the stability margin `1 − ρ(A)`. -- **Metric value:** M4 (stability) — the general test for discrete-time controllers, observers, - IIR/Biquad (via the companion matrix of the denominator), and closed-loop `A − BK`. -- **Algorithm:** power iteration for the dominant eigenvalue, or characteristic-polynomial roots via - the existing `solvers::DurandKerner` for the full spectrum. -- **Reuses:** `math::Matrix`, `solvers::DurandKerner`. - -### ~~52. Estimator consistency metrics (NEES / NIS) ★★★☆☆ — `estimators`~~ ✓ Done -- **What:** Normalised Estimation Error Squared and Normalised Innovation Squared, with χ² - confidence-gate helpers. -- **Metric value:** M8 (statistical consistency) — the only rigorous correctness test for the Kalman - family (`filters/active/`: KF/EKF/UKF/smoother) beyond raw RMSE. -- **Algorithm:** `εᵀ·P⁻¹·ε` against χ² bounds for the state/measurement dimension. -- **Reuses:** `math::Matrix`, `solvers::GaussianElimination` (for `P⁻¹·ε`), existing - `estimators::EstimationMetrics`. - > **Test-only helpers (not production components).** A ULP/relative-error comparator and a > finite-difference **gradient check** (for `neural_network/` and `optimization/`) are pure test > utilities — add them to the `numerical.math_test_helper` INTERFACE library > ([`numerical/math/test_doubles/`](numerical/math/test_doubles/)), not to `numerical/` production code. -## New modules to introduce - -New top-level domains under `numerical/` are proposed to house the additions, -each mirroring the existing header-library + `test/` + `doc/` layout: - -- `numerical/robust_control/` → items 34, 35, 36, 46 (namespace `robust_control`) -- `numerical/nonlinear_control/` → items 40, 41, 47 (namespace `nonlinear_control`) - -Robot-manipulator kinematics, dynamics, trajectory generation, and manipulator control now live in -[robotics-toolbox-cpp](https://github.com/embedded-pro/robotics-toolbox-cpp), which consumes this -library via `FetchContent`. - ## Per-component implementation checklist Every new component should follow the established repository conventions: diff --git a/doc/nonlinear_control/ModelReferenceAdaptiveControl.md b/doc/nonlinear_control/ModelReferenceAdaptiveControl.md new file mode 100644 index 00000000..2b70ca0e --- /dev/null +++ b/doc/nonlinear_control/ModelReferenceAdaptiveControl.md @@ -0,0 +1,130 @@ +# Model Reference Adaptive Control + +## Overview & Motivation + +Real hardware changes over time: motor resistance drifts with temperature, payload mass varies, actuators age. A fixed controller tuned at the factory cannot maintain performance across this variability. Model Reference Adaptive Control (MRAC) addresses this by continuously adjusting its own gains online, using only the tracking error between the real plant and a designer-specified reference model, until the plant's response matches the desired ideal. No knowledge of the exact plant parameters is needed — only the structure (the equations of motion) and the sign of the input gain. + +This makes MRAC the canonical direct-adaptive scheme for the "known structure, unknown numbers" class of problems that appears throughout embedded control: a family of units sharing the same firmware but differing in parameter values, or a single unit whose parameters change slowly during operation. + +## Mathematical Theory + +### Problem Setup + +Given a plant of the form + +$$\dot{x} = a\,x + b\,u$$ + +where $a$ and $b$ are unknown constants with $b \neq 0$ and $\text{sgn}(b)$ known, the goal is to choose the control $u$ so that $x(t) \to x_m(t)$ as $t \to \infty$, where $x_m$ satisfies the reference model + +$$\dot{x}_m = -a_m\,x_m + b_m\,r, \quad a_m > 0.$$ + +### Matching Conditions + +The ideal control law that would make the plant identical to the reference model is + +$$u^* = \theta_x^*\,x + \theta_r^*\,r$$ + +where + +$$\theta_x^* = \frac{-(a_m + a)}{b}, \qquad \theta_r^* = \frac{b_m}{b}.$$ + +Since $a$ and $b$ are unknown, $\theta_x^*$ and $\theta_r^*$ cannot be computed directly; instead they are estimated online. + +### Tracking Error Dynamics + +Let $\hat{\theta}_x$ and $\hat{\theta}_r$ denote the current parameter estimates and define the parameter errors $\tilde{\theta} = \hat{\theta} - \theta^*$. The tracking error $e = x - x_m$ satisfies + +$$\dot{e} = -a_m\,e + b\,(\tilde{\theta}_x\,x + \tilde{\theta}_r\,r).$$ + +### MIT Rule + +The MIT rule minimises $J = \tfrac{1}{2}e^2$ by gradient descent on the parameter space: + +$$\dot{\hat{\theta}}_x = -\gamma\,\text{sgn}(b)\,e\,x, \qquad \dot{\hat{\theta}}_r = -\gamma\,\text{sgn}(b)\,e\,r.$$ + +This is a first-order gradient update. It converges when $\gamma$ is small relative to the signal levels and the reference is persistently exciting. It has no global stability proof. + +### Lyapunov Redesign + +The Lyapunov method chooses the same update law but derives it from the Lyapunov function candidate + +$$V(e, \tilde{\theta}) = \frac{e^2}{2} + \frac{|b|}{2\gamma}\left(\tilde{\theta}_x^2 + \tilde{\theta}_r^2\right).$$ + +Computing $\dot{V}$ and choosing the parameter update laws to make $\dot{V} \leq -a_m\,e^2 \leq 0$ yields precisely the same gradient form. The Lyapunov construction guarantees that $e$ and $\tilde{\theta}$ remain bounded for all $t \geq 0$ and that $e(t) \to 0$, even under large initial errors — a global stability guarantee that the pure MIT rule lacks. + +### Discrete-Time Implementation + +The continuous-time update is approximated by forward Euler with step $\Delta t$: + +$$x_m[k+1] = x_m[k] + (A_m\,x_m[k] + B_m\,r[k])\,\Delta t$$ + +$$e[k] = x[k] - x_m[k+1]$$ + +$$u[k] = \hat{\theta}_x[k]\,x[k] + \hat{\theta}_r[k]\,r[k]$$ + +$$\hat{\theta}_x[k+1] = \hat{\theta}_x[k] - \gamma\,\text{sgn}(b)\,e[k]\,x[k]\,\Delta t$$ + +$$\hat{\theta}_r[k+1] = \hat{\theta}_r[k] - \gamma\,\text{sgn}(b)\,e[k]\,r[k]\,\Delta t$$ + +The same structure extends to the multi-input multi-output case using outer products of the error and regressor vectors, yielding matrix parameter estimates. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------|--------------------------------|-------------------------------------------|-----------------------------------------| +| ComputeControl | $O(n^2 + nm)$ | $O(1)$ working registers | Reference model step + two outer products + two matrix-vector products | +| Memory | $O(n^2 + nm)$ static | Parameter matrices and reference state | All fixed-size; no heap allocation | + +Here $n$ = StateSize and $m$ = InputSize. The dominant cost is the outer product update of the parameter matrices at each step. + +## Step-by-Step Walkthrough + +**First-order scalar example** ($n = m = 1$, $a = -1.5$, $b = 2$, $a_m = 1$, $b_m = 1$, $\gamma = 1$, $\Delta t = 0.01$): + +1. At step $k=0$: $x = 2$, $r = 1$, $x_m = 0$, $\hat{\theta}_x = 0$, $\hat{\theta}_r = 0$. +2. Advance reference model: $x_m \leftarrow 0 + (-1 \cdot 0 + 1 \cdot 1) \cdot 0.01 = 0.01$. +3. Tracking error: $e = 2 - 0.01 = 1.99$. +4. Control output: $u = 0 \cdot 2 + 0 \cdot 1 = 0$ (initial parameters are zero). +5. Update $\hat{\theta}_x$: $0 - 1 \cdot (+1) \cdot 1.99 \cdot 2 \cdot 0.01 = -0.0398$. +6. Update $\hat{\theta}_r$: $0 - 1 \cdot (+1) \cdot 1.99 \cdot 1 \cdot 0.01 = -0.0199$. +7. At the next step the control becomes $u = -0.0398 \cdot x - 0.0199 \cdot r$, already driving the plant toward the reference model. After many steps, $\hat{\theta}_x \to \theta_x^* = 0.25$ and $\hat{\theta}_r \to \theta_r^* = 0.5$. + +## Pitfalls & Edge Cases + +- **Adaptation gain too large**: with the MIT rule, $\gamma$ large relative to signal power causes $\hat{\theta}$ to overshoot and the closed-loop to go unstable. The Lyapunov law is more forgiving but still requires reasonable $\gamma$. +- **Wrong sign of $b$**: if $\text{sgn}(b)$ is set incorrectly, adaptation drives parameters in the wrong direction and the error grows. This is a hard fault — the algorithm is designed around knowing the plant's sign. +- **No persistent excitation**: if $r$ is constant or bandlimited, $\hat{\theta}$ converges to some values that achieve tracking but not necessarily to $\theta^*$. The plant still tracks the reference model; only parameter identification fails. +- **Parameter drift**: in the presence of noise or unmodelled disturbances, $\hat{\theta}$ drifts even when $e$ is small. Standard remedies are $\sigma$-modification ($\dot{\hat{\theta}} = -\gamma\,e\,\phi - \sigma\,\hat{\theta}$) and parameter projection onto a compact set. +- **Euler discretisation error**: the Euler step introduces $O(\Delta t)$ error in the reference model trajectory and the parameter update. Small $\Delta t$ is needed for accuracy; a higher-order integrator (RK4) can be used for the reference model when $\Delta t$ is large. + +## Variants & Generalizations + +- **Indirect MRAC**: first identifies the plant parameters (via Recursive Least Squares) and then recomputes the matching gains; separates identification from control but requires a plant model structure assumption. +- **$\sigma$-modification**: adds a leakage term $-\sigma\,\hat{\theta}$ to the parameter update, bounding parameter drift at the cost of a small steady-state bias. +- **e-modification**: replaces $\sigma$ with $\sigma\,|e|\,\hat{\theta}$, so leakage is active only when the error is large and vanishes at steady state. +- **Parameter projection**: constrains $\hat{\theta}$ to a known compact set, preventing unbounded drift without degrading tracking. +- **Adaptive backstepping**: embeds MRAC-style parameter adaptation inside the Backstepping recursive design for strict-feedback plants with unknown parameters. +- **MRAC with reference model order > 1**: the same gradient law extends to any LTI reference model by tracking the full state vector $x_m \in \mathbb{R}^n$. + +## Applications + +- Motor drives with varying load inertia or resistance: MRAC maintains speed/position bandwidth despite parameter changes. +- Aerospace: autopilot adaptation to changing mass, fuel consumption, or aerodynamic coefficients. +- Robotic manipulators: payload-varying adaptive torque controllers. +- Power electronics: adaptive current controllers for converters with uncertain filter inductance. +- Embedded firmware for a product family: a single binary adapts to unit-to-unit hardware variation at startup. + +## Connections to Other Algorithms + +- **Backstepping Control**: MRAC handles unknown parameters in a fixed-structure plant; adaptive backstepping merges both, providing recursive stability proofs for uncertain strict-feedback systems. +- **Feedback Linearization**: cancels nonlinearities via an explicit model; MRAC is model-free in the parameter sense and is more robust when the model is uncertain. +- **Recursive Least Squares (RLS)**: the indirect-MRAC identification step; direct MRAC avoids explicit parameter estimation by updating control gains rather than plant parameters. +- **LQR**: optimal fixed-gain control for known linear plants; MRAC extends the design to plants with unknown parameters at the cost of the adaptation transient. +- **math::LinearTimeInvariant**: provides the reference model state-space container; the MRAC controller holds a reference to an LTI instance to step the reference trajectory. + +## References & Further Reading + +- K. J. Åström, B. Wittenmark, *Adaptive Control*, 2nd ed., Addison-Wesley, 1995. +- K. S. Narendra, A. M. Annaswamy, *Stable Adaptive Systems*, Prentice-Hall, 1989; Dover reprint 2005. +- S. Sastry, M. Bodson, *Adaptive Control: Stability, Convergence and Robustness*, Prentice-Hall, 1989. +- P. A. Ioannou, J. Sun, *Robust Adaptive Control*, Prentice-Hall, 1996 (free PDF available from the authors). diff --git a/doc/nonlinear_control/README.md b/doc/nonlinear_control/README.md index 63b70ad0..7d9db2ab 100644 --- a/doc/nonlinear_control/README.md +++ b/doc/nonlinear_control/README.md @@ -6,5 +6,6 @@ Algorithms for nonlinear control design: controllers that exploit or cancel plan | Algorithm | Description | |----------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Backstepping Control](BacksteppingControl.md) | Lyapunov-based recursive design for strict-feedback nonlinear cascades that stabilises each integrator stage in sequence, yielding a provably stable controller by construction | -| [Feedback Linearization](FeedbackLinearization.md) | Cancels a control-affine plant's known nonlinear dynamics via an inner control law, leaving decoupled integrator chains that a simple outer PD/LQR loop drives | +| [Backstepping Control](BacksteppingControl.md) | Lyapunov-based recursive design for strict-feedback nonlinear cascades that stabilises each integrator stage in sequence, yielding a provably stable controller by construction | +| [Feedback Linearization](FeedbackLinearization.md) | Cancels a control-affine plant's known nonlinear dynamics via an inner control law, leaving decoupled integrator chains that a simple outer PD/LQR loop drives | +| [Model Reference Adaptive Control](ModelReferenceAdaptiveControl.md) | Online gradient-descent adaptation of control gains to match plant response to a stable reference model, with Lyapunov stability guarantee for bounded error under unknown plant parameters | diff --git a/numerical/nonlinear_control/CMakeLists.txt b/numerical/nonlinear_control/CMakeLists.txt index 7b1ddf17..66e2bf7c 100644 --- a/numerical/nonlinear_control/CMakeLists.txt +++ b/numerical/nonlinear_control/CMakeLists.txt @@ -13,11 +13,13 @@ target_link_libraries(numerical.nonlinear_control ${NUMERICAL_VISIBILITY} target_sources(numerical.nonlinear_control PRIVATE BacksteppingControl.hpp FeedbackLinearization.hpp + ModelReferenceAdaptiveControl.hpp ) numerical_add_coverage_sources(numerical.nonlinear_control BacksteppingControl.cpp FeedbackLinearization.cpp + ModelReferenceAdaptiveControl.cpp ) add_subdirectory(test) diff --git a/numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp new file mode 100644 index 00000000..95cd5780 --- /dev/null +++ b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp @@ -0,0 +1,6 @@ +#include "numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp" + +namespace nonlinear_control +{ + template class ModelReferenceAdaptiveControl; +} diff --git a/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp new file mode 100644 index 00000000..c4c7606d --- /dev/null +++ b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp @@ -0,0 +1,102 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/LinearTimeInvariant.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace nonlinear_control +{ + enum class AdaptationLaw + { + MitRule, + Lyapunov + }; + + template + class ModelReferenceAdaptiveControl + { + static_assert(std::is_floating_point_v, + "ModelReferenceAdaptiveControl supports floating-point types"); + static_assert(StateSize > 0, "ModelReferenceAdaptiveControl requires StateSize > 0"); + static_assert(InputSize > 0, "ModelReferenceAdaptiveControl requires InputSize > 0"); + + public: + using StateVector = math::Vector; + using InputVector = math::Vector; + using FeedbackMatrix = math::Matrix; + using FeedforwardMatrix = math::Matrix; + using ReferenceModel = math::LinearTimeInvariant; + + ModelReferenceAdaptiveControl(const ReferenceModel& referenceModel, + T gamma, T signB, AdaptationLaw law); + + OPTIMIZE_FOR_SPEED InputVector ComputeControl( + const StateVector& x, const InputVector& r, T dt); + + void Reset(); + + [[nodiscard]] const StateVector& GetReferenceState() const { return xm; } + [[nodiscard]] const FeedbackMatrix& GetThetaX() const { return thetaX; } + [[nodiscard]] const FeedforwardMatrix& GetThetaR() const { return thetaR; } + + private: + const ReferenceModel& reference; + StateVector xm{}; + FeedbackMatrix thetaX{}; + FeedforwardMatrix thetaR{}; + T gamma; + T signB; + AdaptationLaw law; + }; + + template + ModelReferenceAdaptiveControl::ModelReferenceAdaptiveControl( + const ReferenceModel& referenceModel, T gamma, T signB, AdaptationLaw law) + : reference{ referenceModel } + , gamma{ gamma } + , signB{ signB } + , law{ law } + {} + + template + OPTIMIZE_FOR_SPEED typename ModelReferenceAdaptiveControl::InputVector + ModelReferenceAdaptiveControl::ComputeControl( + const StateVector& x, const InputVector& r, T dt) + { + xm = xm + (reference.A * xm + reference.B * r) * dt; + + const StateVector e{ x - xm }; + + const InputVector u{ thetaX * x + thetaR * r }; + + const T scale{ gamma * signB * dt }; + for (std::size_t i = 0; i < InputSize; ++i) + { + for (std::size_t j = 0; j < StateSize; ++j) + thetaX.at(i, j) -= scale * e.at(i, 0) * x.at(j, 0); + + for (std::size_t j = 0; j < InputSize; ++j) + thetaR.at(i, j) -= scale * e.at(i, 0) * r.at(j, 0); + } + + return u; + } + + template + void ModelReferenceAdaptiveControl::Reset() + { + xm = StateVector{}; + thetaX = FeedbackMatrix{}; + thetaR = FeedforwardMatrix{}; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ModelReferenceAdaptiveControl; +#endif +} diff --git a/numerical/nonlinear_control/test/CMakeLists.txt b/numerical/nonlinear_control/test/CMakeLists.txt index d666a472..7227ed9e 100644 --- a/numerical/nonlinear_control/test/CMakeLists.txt +++ b/numerical/nonlinear_control/test/CMakeLists.txt @@ -10,4 +10,5 @@ target_link_libraries(numerical.nonlinear_control_test PUBLIC target_sources(numerical.nonlinear_control_test PRIVATE TestBacksteppingControl.cpp TestFeedbackLinearization.cpp + TestModelReferenceAdaptiveControl.cpp ) diff --git a/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp b/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp new file mode 100644 index 00000000..40503af9 --- /dev/null +++ b/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp @@ -0,0 +1,251 @@ +#include "numerical/math/LinearTimeInvariant.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp" +#include +#include +#include + +namespace +{ + static math::LinearTimeInvariant MakeFirstOrderReference() + { + math::LinearTimeInvariant lti{}; + lti.A.at(0, 0) = -1.0f; + lti.B.at(0, 0) = 1.0f; + return lti; + } + + class TestModelReferenceAdaptiveControl : public ::testing::Test + { + protected: + math::LinearTimeInvariant refModel{ MakeFirstOrderReference() }; + nonlinear_control::ModelReferenceAdaptiveControl mrac{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + }; +} + +TEST_F(TestModelReferenceAdaptiveControl, reference_model_advances) +{ + math::Vector x{}; + x.at(0, 0) = 0.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + const float dt{ 0.1f }; + + mrac.ComputeControl(x, r, dt); + + const float expectedXm{ refModel.B.at(0, 0) * r.at(0, 0) * dt }; + EXPECT_NEAR(mrac.GetReferenceState().at(0, 0), expectedXm, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, zero_error_freezes_parameters) +{ + math::Vector x{}; + x.at(0, 0) = 0.0f; + math::Vector r{}; + r.at(0, 0) = 0.0f; + + mrac.ComputeControl(x, r, 0.1f); + + const float txBefore{ mrac.GetThetaX().at(0, 0) }; + const float trBefore{ mrac.GetThetaR().at(0, 0) }; + + mrac.ComputeControl(x, r, 0.1f); + + EXPECT_NEAR(mrac.GetThetaX().at(0, 0), txBefore, math::Tolerance()); + EXPECT_NEAR(mrac.GetThetaR().at(0, 0), trBefore, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, positive_error_adapts_feedback) +{ + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 0.0f; + const float dt{ 0.1f }; + + const float txBefore{ mrac.GetThetaX().at(0, 0) }; + + mrac.ComputeControl(x, r, dt); + + const float xmAfter{ mrac.GetReferenceState().at(0, 0) }; + const float e{ x.at(0, 0) - xmAfter }; + const float expectedDelta{ -1.0f * 1.0f * e * x.at(0, 0) * dt }; + + EXPECT_NEAR(mrac.GetThetaX().at(0, 0), txBefore + expectedDelta, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, feedforward_param_tracks_command) +{ + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + const float dt{ 0.1f }; + + const float trBefore{ mrac.GetThetaR().at(0, 0) }; + + mrac.ComputeControl(x, r, dt); + + const float xmAfter{ mrac.GetReferenceState().at(0, 0) }; + const float e{ x.at(0, 0) - xmAfter }; + const float expectedDelta{ -1.0f * 1.0f * e * r.at(0, 0) * dt }; + + EXPECT_NEAR(mrac.GetThetaR().at(0, 0), trBefore + expectedDelta, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, signB_flips_adaptation_direction) +{ + nonlinear_control::ModelReferenceAdaptiveControl mracNeg{ + refModel, 1.0f, -1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 0.0f; + const float dt{ 0.1f }; + + mrac.ComputeControl(x, r, dt); + mracNeg.ComputeControl(x, r, dt); + + const float deltaPos{ mrac.GetThetaX().at(0, 0) }; + const float deltaNeg{ mracNeg.GetThetaX().at(0, 0) }; + + EXPECT_NEAR(deltaPos, -deltaNeg, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, control_law_combines_terms) +{ + float xPlant{ 0.5f }; + const float aPlant{ -1.5f }; + const float bPlant{ 2.0f }; + const float dt{ 0.01f }; + + math::Vector r{}; + r.at(0, 0) = 1.0f; + + for (int k = 0; k < 500; ++k) + { + math::Vector xVec{}; + xVec.at(0, 0) = xPlant; + const auto u = mrac.ComputeControl(xVec, r, dt); + xPlant += dt * (aPlant * xPlant + bPlant * u.at(0, 0)); + } + + math::Vector xNow{}; + xNow.at(0, 0) = xPlant; + const float txCurrent{ mrac.GetThetaX().at(0, 0) }; + const float trCurrent{ mrac.GetThetaR().at(0, 0) }; + const auto u = mrac.ComputeControl(xNow, r, dt); + + const float expectedU{ txCurrent * xPlant + trCurrent * r.at(0, 0) }; + EXPECT_NEAR(u.at(0, 0), expectedU, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, tracks_reference_over_time) +{ + float xPlant{ 2.0f }; + const float aPlant{ -1.5f }; + const float bPlant{ 2.0f }; + const float dt{ 0.01f }; + + math::Vector r{}; + r.at(0, 0) = 1.0f; + + for (int k = 0; k < 2000; ++k) + { + math::Vector xVec{}; + xVec.at(0, 0) = xPlant; + const auto u = mrac.ComputeControl(xVec, r, dt); + xPlant += dt * (aPlant * xPlant + bPlant * u.at(0, 0)); + } + + const float xm{ mrac.GetReferenceState().at(0, 0) }; + EXPECT_LT(std::abs(xPlant - xm), 0.1f); +} + +TEST_F(TestModelReferenceAdaptiveControl, gamma_scales_adaptation_speed) +{ + nonlinear_control::ModelReferenceAdaptiveControl mracSlow{ + refModel, 0.5f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + nonlinear_control::ModelReferenceAdaptiveControl mracFast{ + refModel, 5.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + float xSlow{ 2.0f }; + float xFast{ 2.0f }; + const float aPlant{ -1.5f }; + const float bPlant{ 2.0f }; + const float dt{ 0.005f }; + + math::Vector r{}; + r.at(0, 0) = 1.0f; + + for (int k = 0; k < 400; ++k) + { + math::Vector xsVec{}; + xsVec.at(0, 0) = xSlow; + const auto us = mracSlow.ComputeControl(xsVec, r, dt); + xSlow += dt * (aPlant * xSlow + bPlant * us.at(0, 0)); + + math::Vector xfVec{}; + xfVec.at(0, 0) = xFast; + const auto uf = mracFast.ComputeControl(xfVec, r, dt); + xFast += dt * (aPlant * xFast + bPlant * uf.at(0, 0)); + } + + const float errSlow{ std::abs(xSlow - mracSlow.GetReferenceState().at(0, 0)) }; + const float errFast{ std::abs(xFast - mracFast.GetReferenceState().at(0, 0)) }; + + EXPECT_LT(errFast, errSlow); +} + +TEST_F(TestModelReferenceAdaptiveControl, parameters_converge_under_excitation) +{ + const float aPlant{ -1.5f }; + const float bPlant{ 2.0f }; + const float amRef{ 1.0f }; + const float bmRef{ 1.0f }; + const float thetaXStar{ -(amRef - std::abs(aPlant)) / bPlant }; + const float thetaRStar{ bmRef / bPlant }; + + nonlinear_control::ModelReferenceAdaptiveControl mracPe{ + refModel, 2.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + float xPlant{ 0.0f }; + const float dt{ 0.005f }; + + for (int k = 0; k < 4000; ++k) + { + const float t{ static_cast(k) * dt }; + math::Vector r{}; + r.at(0, 0) = std::sin(t) + std::sin(3.0f * t); + + math::Vector xVec{}; + xVec.at(0, 0) = xPlant; + const auto u = mracPe.ComputeControl(xVec, r, dt); + xPlant += dt * (aPlant * xPlant + bPlant * u.at(0, 0)); + } + + EXPECT_NEAR(mracPe.GetThetaX().at(0, 0), thetaXStar, 0.5f); + EXPECT_NEAR(mracPe.GetThetaR().at(0, 0), thetaRStar, 0.5f); +} + +TEST_F(TestModelReferenceAdaptiveControl, reset_clears_state_and_params) +{ + math::Vector x{}; + x.at(0, 0) = 3.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + + mrac.ComputeControl(x, r, 0.1f); + mrac.Reset(); + + EXPECT_NEAR(mrac.GetReferenceState().at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(mrac.GetThetaX().at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(mrac.GetThetaR().at(0, 0), 0.0f, math::Tolerance()); +} diff --git a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/explanation.md b/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/explanation.md deleted file mode 100644 index 5886c5f8..00000000 --- a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/explanation.md +++ /dev/null @@ -1,38 +0,0 @@ -# Model Reference Adaptive Control (MRAC) — Overview - -## What it is -A self-tuning controller that makes an **uncertain** plant behave like a chosen **reference -model**. A designer picks the ideal response (a stable `A_m`, `B_m`); MRAC then adjusts its own -gains *online*, from the tracking error alone, until the real plant's output matches that ideal — -without ever measuring the unknown plant parameters directly. - -## Why it matters (embedded) -Real hardware drifts: motor resistance rises with temperature, payload mass changes, actuators -age. A fixed controller tuned at the factory degrades. MRAC keeps performance constant by adapting -in the field — the same firmware handles a family of units and slowly-varying plants without -re-tuning. It is the classic direct-adaptive scheme for these "known structure, unknown numbers" -problems. - -## How it works (intuition) -Run the reference model alongside the plant and watch the gap `e = x − x_m`. If the plant lags, the -adaptation law nudges the control gains in the direction that shrinks `e` — an online gradient -descent on the tracking error. Two flavours: the **MIT rule** follows the raw error gradient -(simple, but can go unstable if pushed hard), while the **Lyapunov redesign** derives the *same -shape* of update from a stability certificate, guaranteeing the error stays bounded. Feed it a -rich enough command and the gains also converge to their true ideal values. - -## Key parameters -- **reference model (A_m, B_m)** — the injected "gold standard" behaviour to imitate. -- **adaptation gain γ** — how aggressively parameters move; the central speed-vs-stability knob. -- **sign of the input gain** — the adaptation must know which way the plant responds. -- **robustness modification** (σ / e-mod / projection) — optional, bounds parameter drift under - noise and disturbance. - -## Reference -K. J. Åström, B. Wittenmark, *Adaptive Control*, 2nd ed. (1995); -K. S. Narendra, A. M. Annaswamy, *Stable Adaptive Systems* (1989). - -## See also -`RecursiveLeastSquares` (indirect-adaptive alternative: identify then control); -`FeedbackLinearization` / `BacksteppingControl` (fixed-parameter nonlinear designs MRAC augments); -`math::LinearTimeInvariant` (the reference-model container). diff --git a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/implementation.md b/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/implementation.md deleted file mode 100644 index fc527b15..00000000 --- a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/implementation.md +++ /dev/null @@ -1,91 +0,0 @@ -# Model Reference Adaptive Control (MRAC) — Implementation Pseudocode - -> Roadmap ref: #47 (Tier 5) · Target: `numerical/nonlinear_control` · Namespace `nonlinear_control` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -enum class AdaptationLaw { MitRule, Lyapunov } - -template # static_assert(std::is_floating_point_v); instantiated for float -class ModelReferenceAdaptiveControl: - const math::LinearTimeInvariant& reference # A_m, B_m - math::Vector xm # reference-model state - math::Matrix thetaX # adapted feedback params - math::Matrix thetaR # adapted feedforward params - T gamma # adaptation rate γ > 0 - T signB # sign of plant input gain (±1) - AdaptationLaw law -``` - -## Interface - -``` -# Reference model injected (DIP) — it defines the *desired* closed-loop behaviour: -ModelReferenceAdaptiveControl(const LinearTimeInvariant& referenceModel, - T gamma, T signB, AdaptationLaw law) - -InputVector ComputeControl(const StateVector& x, const InputVector& r, T dt) # hot path -void Reset() -``` - -## Algorithm (pseudocode) - -``` -function ComputeControl(x, r, dt): # OPTIMIZE_FOR_SPEED - # 1. advance the reference model — the trajectory we WANT the plant to follow - xm = xm + (reference.A * xm + reference.B * r) * dt - - # 2. tracking error between real plant and reference model - e = x - xm - - # 3. adaptive control law: u = θ_x·x + θ_r·r (matches plant to reference when converged) - u = thetaX * x + thetaR * r - - # 4. update parameters so e -> 0 (θ̇ = -γ·signB·e·regressorᵀ) - # MIT rule and Lyapunov redesign share this gradient form here; - # Lyapunov additionally guarantees boundedness of θ. - thetaX = thetaX - (gamma * signB) * outer(e, x) * dt - thetaR = thetaR - (gamma * signB) * outer(e, r) * dt - - return u - -function Reset(): - xm = 0; thetaX = 0; thetaR = 0 -``` - -## Complexity & memory - -- `ComputeControl`: `O(StateSize²)` — reference-model step, two outer products, two matvecs. -- Memory: `O(StateSize² + StateSize·InputSize)` for the parameter matrices; static, no heap. - -## Numerical / embedded notes - -- **γ (adaptation gain)** trades tracking speed against stability: too large destabilizes, - especially the pure **MIT rule**, which has *no* global stability proof. The **Lyapunov** law is - derived to keep the error system's Lyapunov function non-increasing. -- **Parameter drift:** with noise or unmodelled disturbance, `θ` can wander even while `e` stays - small. Add **σ-modification / e-modification** or **parameter projection** to bound `θ`. -- **Persistent excitation:** the command `r` must be rich enough for `θ` to converge to the true - values; without it the plant still *tracks* but the parameters are not identified. -- `signB` must match the sign of the plant's high-frequency gain, or adaptation runs the wrong way. -- Reuse `math::LinearTimeInvariant` for the reference model; for an *indirect* adaptive variant, - estimate the plant with `RecursiveLeastSquares` (`estimators/online`) and recompute the gains. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeControl`, and - `extern template class ModelReferenceAdaptiveControl;` - under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp` → - `template class ModelReferenceAdaptiveControl;` -- Test: `numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp` -- Doc: `doc/nonlinear_control/ModelReferenceAdaptiveControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestModelReferenceAdaptiveControl.cpp` → the `_test` target. -- New module: create `numerical/nonlinear_control/CMakeLists.txt` via `numerical_add_header_library(...)`, - add `test/`, register in `numerical/CMakeLists.txt`, add `doc/nonlinear_control/`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/tests.md b/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/tests.md deleted file mode 100644 index 86a9a14f..00000000 --- a/roadmap/nonlinear_control/ModelReferenceAdaptiveControl/tests.md +++ /dev/null @@ -1,71 +0,0 @@ -# Model Reference Adaptive Control (MRAC) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestModelReferenceAdaptiveControl : public ::testing::Test: - # First-order reference model ẋ_m = -a_m x_m + b_m r (stable, a_m > 0): - math::LinearTimeInvariant refModel = MakeFirstOrderReference() - ModelReferenceAdaptiveControl mrac{ refModel, /*gamma*/1.0, - /*signB*/+1.0, AdaptationLaw::Lyapunov } -# each case below is a TEST_F(TestModelReferenceAdaptiveControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -reference_model_advances: - Arrange: xm=0, step command r=1 - Act: ComputeControl(x, r, dt) once - Assert: xm == (refModel.B · r) · dt (Euler step of the reference model) - -zero_error_freezes_parameters: - Arrange: x == xm so e = 0 - Assert: thetaX and thetaR unchanged after the update - -positive_error_adapts_feedback: - Arrange: e = x - xm > 0, x > 0, signB = +1 - Assert: thetaX decreases by gamma·e·x·dt - -feedforward_param_tracks_command: - Arrange: e != 0, r != 0 - Assert: thetaR changes by -gamma·signB·e·r·dt - -signB_flips_adaptation_direction: - Arrange: identical error/state, signB = -1 - Assert: parameter update has opposite sign vs signB = +1 - -control_law_combines_terms: - Arrange: known thetaX, thetaR, x, r - Assert: u == thetaX·x + thetaR·r - -tracks_reference_over_time: - Arrange: wrap an unknown first-order plant; run K steps with a step command - Assert: |x - xm| -> 0 (below tol); Lyapunov law keeps signals bounded - -gamma_scales_adaptation_speed: - Arrange: two runs, gamma = 0.5 vs 5.0 - Assert: larger gamma reduces tracking error faster (until it destabilizes — documented) - -parameters_converge_under_excitation: - Arrange: persistently-exciting r (sum of sinusoids), known plant params - Assert: thetaX, thetaR approach the ideal matching gains - -reset_clears_state_and_params: - Arrange: adapt, then Reset() - Assert: xm == 0, thetaX == 0, thetaR == 0 -``` - -## Reference vectors - -- Ideal matching gains: for plant `ẋ = a·x + b·u` and reference `ẋ_m = -a_m·x_m + b_m·r`, - the perfect params are `θ_x* = -(a_m + a)/b`, `θ_r* = b_m/b` — the convergence targets. -- Zero error ⇒ `θ̇ = 0`; the golden invariant for the freeze test. - -## Edge cases - -- Large `gamma` with MIT rule: expect (and assert) growth — demonstrates the stability caveat. -- Disturbance with no σ-modification: parameters drift; enabling modification bounds them. -- No excitation (constant `r`): tracking succeeds but parameters need not reach `θ*`. From 69f6b543476d11b79b5a4632925ecd34cdc1bb94 Mon Sep 17 00:00:00 2001 From: gfs Date: Sun, 2 Aug 2026 12:51:26 +0200 Subject: [PATCH 2/2] Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- .../ModelReferenceAdaptiveControl.md | 8 ++++---- .../ModelReferenceAdaptiveControl.hpp | 17 ++++++++++++++--- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/doc/nonlinear_control/ModelReferenceAdaptiveControl.md b/doc/nonlinear_control/ModelReferenceAdaptiveControl.md index 2b70ca0e..21cb6e96 100644 --- a/doc/nonlinear_control/ModelReferenceAdaptiveControl.md +++ b/doc/nonlinear_control/ModelReferenceAdaptiveControl.md @@ -70,10 +70,10 @@ The same structure extends to the multi-input multi-output case using outer prod ## Complexity Analysis -| Operation | Time | Space | Notes | -|----------------|--------------------------------|-------------------------------------------|-----------------------------------------| -| ComputeControl | $O(n^2 + nm)$ | $O(1)$ working registers | Reference model step + two outer products + two matrix-vector products | -| Memory | $O(n^2 + nm)$ static | Parameter matrices and reference state | All fixed-size; no heap allocation | +| Operation | Time | Space | Notes | +|----------------|----------------------|----------------------------------------|------------------------------------------------------------------------| +| ComputeControl | $O(n^2 + nm)$ | $O(1)$ working registers | Reference model step + two outer products + two matrix-vector products | +| Memory | $O(n^2 + nm)$ static | Parameter matrices and reference state | All fixed-size; no heap allocation | Here $n$ = StateSize and $m$ = InputSize. The dominant cost is the outer product update of the parameter matrices at each step. diff --git a/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp index c4c7606d..94a6a379 100644 --- a/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp +++ b/numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp @@ -41,9 +41,20 @@ namespace nonlinear_control void Reset(); - [[nodiscard]] const StateVector& GetReferenceState() const { return xm; } - [[nodiscard]] const FeedbackMatrix& GetThetaX() const { return thetaX; } - [[nodiscard]] const FeedforwardMatrix& GetThetaR() const { return thetaR; } + [[nodiscard]] const StateVector& GetReferenceState() const + { + return xm; + } + + [[nodiscard]] const FeedbackMatrix& GetThetaX() const + { + return thetaX; + } + + [[nodiscard]] const FeedforwardMatrix& GetThetaR() const + { + return thetaR; + } private: const ReferenceModel& reference;