feat: add model reference adaptive control

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                                         |
 

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).

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 |

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)

numerical/nonlinear_control/ModelReferenceAdaptiveControl.cpp

@@ -0,0 +1,6 @@
+#include "numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp"
+
+namespace nonlinear_control
+{
+    template class ModelReferenceAdaptiveControl<float, 1, 1>;
+}

numerical/nonlinear_control/ModelReferenceAdaptiveControl.hpp

@@ -0,0 +1,113 @@
+#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 <cstddef>
+#include <type_traits>
+
+namespace nonlinear_control
+{
+    enum class AdaptationLaw
+    {
+        MitRule,
+        Lyapunov
+    };
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize>
+    class ModelReferenceAdaptiveControl
+    {
+        static_assert(std::is_floating_point_v<T>,
+            "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<T, StateSize>;
+        using InputVector = math::Vector<T, InputSize>;
+        using FeedbackMatrix = math::Matrix<T, InputSize, StateSize>;
+        using FeedforwardMatrix = math::Matrix<T, InputSize, InputSize>;
+        using ReferenceModel = math::LinearTimeInvariant<T, StateSize, InputSize, StateSize>;
+
+        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<typename T, std::size_t StateSize, std::size_t InputSize>
+    ModelReferenceAdaptiveControl<T, StateSize, InputSize>::ModelReferenceAdaptiveControl(
+        const ReferenceModel& referenceModel, T gamma, T signB, AdaptationLaw law)
+        : reference{ referenceModel }
+        , gamma{ gamma }
+        , signB{ signB }
+        , law{ law }
+    {}
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize>
+    OPTIMIZE_FOR_SPEED typename ModelReferenceAdaptiveControl<T, StateSize, InputSize>::InputVector
+    ModelReferenceAdaptiveControl<T, StateSize, InputSize>::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<typename T, std::size_t StateSize, std::size_t InputSize>
+    void ModelReferenceAdaptiveControl<T, StateSize, InputSize>::Reset()
+    {
+        xm = StateVector{};
+        thetaX = FeedbackMatrix{};
+        thetaR = FeedforwardMatrix{};
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class ModelReferenceAdaptiveControl<float, 1, 1>;
+#endif
+}

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
 )