feat: add backstepping controller

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 |
-| [Nonlinear Control](doc/nonlinear_control/README.md)               | Feedback Linearization                                                                                    |
+| [Nonlinear Control](doc/nonlinear_control/README.md)               | Feedback Linearization, Backstepping Control |
 | [Robust Control](doc/robust_control/README.md)                     | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB) |
 | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD                                         |
 

ROADMAP.md

@@ -29,7 +29,6 @@ Difficulty legend:
 |----|------------------------------------------------------|---------------------------|------------|
 | 39 | Square-root / Information Kalman filter              | `filters/active`          | ★★★★☆      |
 | 40 | Feedback linearization                               | `nonlinear_control` (new) | ★★★★☆      |
-| 41 | Backstepping controller                              | `nonlinear_control` (new) | ★★★★☆      |
 | 42 | Symmetric eigenvalue solver (Jacobi)                 | `solvers`                 | ★★★★★      |
 | 43 | Singular Value Decomposition (Golub-Kahan)           | `solvers`                 | ★★★★★      |
 | 44 | Total Least Squares                                  | `estimators/offline`      | ★★★★★      |
@@ -227,12 +226,6 @@ the library does not yet expose. Detailed below under
 - **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.
 
-### 41. Backstepping controller  *(float-first)*
-- **What:** Recursive Lyapunov-based design for strict-feedback systems, stabilizing one integrator stage at a time.
-- **Embedded value:** Systematic, provably-stable control for cascaded nonlinear plants (electromechanical, flight).
-- **Algorithm / paper:** M. Krstić, I. Kanellakopoulos, P. Kokotović, *Nonlinear and Adaptive Control Design* (1995).
-- **Reuses:** `math::Matrix`, new `nonlinear_control/` module.
-
 ---
 
 ## Tier 5 — Hard / research-grade ★★★★★

doc/nonlinear_control/BacksteppingControl.md

@@ -0,0 +1,106 @@
+# Backstepping Control
+
+## Overview & Motivation
+
+Many embedded control plants are naturally cascaded: motor current drives torque, torque drives velocity, velocity drives position. Classical linear controllers treat the whole cascade as a single transfer function and tune gains empirically. Backstepping instead exploits the cascade structure directly, stabilising each integrator stage in sequence with a Lyapunov certificate attached at every step. The result is a constructive design recipe — not empirical tuning — that is provably stable by construction, which is essential for safety-critical electromechanical and flight control loops.
+
+## Mathematical Theory
+
+### Strict-Feedback Form
+
+The controller targets plants in strict-feedback form:
+
+$$\dot{x}_i = f_i(x_1,\ldots,x_i) + g_i(x_1,\ldots,x_i)\,x_{i+1}, \quad i = 1,\ldots,n-1$$
+
+$$\dot{x}_n = f_n(x) + g_n(x)\,u$$
+
+where $x_i \in \mathbb{R}$ are scalar states, $u \in \mathbb{R}$ is the real input, and $g_i \neq 0$ everywhere in the operating region (controllability condition).
+
+### Stage Errors and Virtual Controls
+
+Let $r(t)$ be the reference with known derivative $\dot{r}$. Define the stage-1 error:
+
+$$z_1 = x_1 - r$$
+
+To make $z_1$ converge, treat $x_2$ as a virtual control and choose the desired value:
+
+$$\alpha_1 = \frac{\dot{r} - f_1 - k_1 z_1}{g_1}$$
+
+so that $\dot{z}_1 = -k_1 z_1$ when $x_2 = \alpha_1$. Since $x_2$ cannot be instantaneously set, define the next error:
+
+$$z_2 = x_2 - \alpha_1$$
+
+Repeating down the chain, at stage $i$ the virtual control satisfies:
+
+$$\alpha_i = \frac{\dot{\alpha}_{i-1} - f_i - k_i z_i - g_{i-1} z_{i-1}}{g_i}$$
+
+The Lyapunov cross term $g_{i-1} z_{i-1}$ cancels the coupling between successive stages. At stage $n$, $\alpha_n$ is the actual input $u$.
+
+### Lyapunov Certificate
+
+The aggregate Lyapunov function:
+
+$$V = \frac{1}{2} \sum_{i=1}^{n} z_i^2$$
+
+has time derivative:
+
+$$\dot{V} = -\sum_{i=1}^{n} k_i z_i^2 < 0 \quad \forall\, z \neq 0$$
+
+This guarantees uniform asymptotic stability of the origin $z = 0$ (equivalently, $x \to r$) for all $k_i > 0$.
+
+### Virtual Derivative Propagation
+
+Each $\alpha_i$ depends on $x$, the gains, and the reference; its time derivative $\dot{\alpha}_i$ must be propagated analytically (chain rule) down the cascade. Numerical differentiation is excluded: noise amplification destroys the stability guarantee.
+
+## Complexity Analysis
+
+| Operation      | Time   | Space  | Notes                                      |
+|----------------|--------|--------|--------------------------------------------|
+| Construction   | $O(n)$ | $O(n)$ | Gain validation only                       |
+| ComputeControl | $O(n)$ | $O(n)$ | Single forward sweep; one divide per stage |
+
+All storage is in fixed-size arrays on the stack. No heap allocation, no recursion.
+
+## Step-by-Step Walkthrough
+
+**Order-2 chain of integrators** ($f_i = 0$, $g_i = 1$, $r = 0$, $\dot{r} = 0$):
+
+1. $z_1 = x_1$. Stage-1 virtual control: $\alpha_1 = -k_1 z_1$.
+2. $z_2 = x_2 - \alpha_1 = x_2 + k_1 x_1$.
+3. $\dot{\alpha}_1 = -k_1 \dot{x}_1 = -k_1 x_2$ (propagated analytically).
+4. Stage-2 actual input: $u = \dot{\alpha}_1 - k_2 z_2 - g_1 z_1 = -k_1 x_2 - k_2(x_2 + k_1 x_1) - x_1$.
+5. Closed-loop: $\dot{V} = -k_1 z_1^2 - k_2 z_2^2 < 0$ — both errors decay independently at rates $k_1$ and $k_2$.
+
+## Pitfalls & Edge Cases
+
+- **Loss of controllability**: if $g_i(x) \to 0$, the virtual control $\alpha_i$ is undefined (division by zero). A guard asserts $|g_i| \geq \varepsilon$ and the controller holds its output rather than outputting infinity.
+- **Derivative explosion**: at each stage $\dot{\alpha}_{i-1}$ must be the exact analytic derivative; finite-difference approximation introduces noise that worsens with stage depth and sampling rate.
+- **Large gains**: high $k_i$ accelerates convergence but amplifies noise and can saturate actuators. In practice the gains are limited by actuator bandwidth and measurement noise.
+- **Non-strict-feedback plants**: if $f_i$ depends on $x_j$ for $j > i$ (non-strict-feedback), the design does not directly apply; feedback linearisation or dynamic extension may be needed.
+
+## Variants & Generalizations
+
+- **Adaptive backstepping**: replaces known parameters in $f_i$ with online estimates (RLS or gradient update); bridges to Model Reference Adaptive Control.
+- **Robust backstepping**: adds a sliding-mode or dead-zone term to each stage to handle bounded uncertainty in $f_i$ without cancellation.
+- **Output-feedback backstepping**: combines with a high-gain observer to reconstruct unmeasured states before applying the control law.
+- **Command-filtered backstepping**: replaces analytic derivative propagation with a first-order command filter, trading exact Lyapunov guarantees for implementability when $\dot{\alpha}$ is expensive to compute.
+
+## Applications
+
+- Motor drives: current → torque → speed → position cascade with known drift terms.
+- UAV attitude and altitude control: thrust dynamics → angular rate → angle → position.
+- Marine vessel path following: surge force → surge speed → horizontal position.
+- Underactuated mechanical systems where the cascade structure is embedded in the Euler-Lagrange equations.
+
+## Connections to Other Algorithms
+
+- **Feedback Linearization**: cancels nonlinearities exactly via coordinate change; backstepping instead dominates them via stage-wise Lyapunov design — less sensitive to model error.
+- **Sliding Mode Control**: achieves robustness via discontinuous switching; backstepping achieves it via constructive Lyapunov design without chattering.
+- **Model Reference Adaptive Control (MRAC)**: adaptive backstepping extends this design to plants with unknown parameters, making the two approaches complementary.
+- **LQR**: optimal for linear plants; backstepping generalises stability-guaranteed design to nonlinear strict-feedback plants at the cost of requiring the analytic model.
+
+## References & Further Reading
+
+- M. Krstić, I. Kanellakopoulos, P. Kokotović, *Nonlinear and Adaptive Control Design*, Wiley, 1995.
+- H. K. Khalil, *Nonlinear Systems*, 3rd ed., Prentice-Hall, 2002, Chapter 14.
+- M. Krstić, P. Kokotović, "Control Lyapunov functions for adaptive nonlinear stabilization," *Systems & Control Letters*, 26(1), 1995.

doc/nonlinear_control/README.md

@@ -1,9 +1,10 @@
 # Nonlinear Control
 
-Algorithms for nonlinear control design: controllers that exploit a known plant model to cancel or structurally transform nonlinear dynamics.
+Algorithms for nonlinear control design: controllers that exploit or cancel plant nonlinearities with stability guarantees.
 
 ## Algorithms
 
-| Algorithm                                          | Description                                                                                                                                                    |
-|----------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| [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 |
+| 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                  |

numerical/nonlinear_control/BacksteppingControl.cpp

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

numerical/nonlinear_control/BacksteppingControl.hpp

@@ -0,0 +1,107 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "infra/util/ReallyAssert.hpp"
+#include "numerical/math/CompilerOptimizations.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <type_traits>
+
+namespace nonlinear_control
+{
+    template<typename T, std::size_t Order>
+    class StrictFeedbackModel
+    {
+        static_assert(std::is_floating_point_v<T>, "StrictFeedbackModel supports floating-point types");
+        static_assert(Order > 0, "StrictFeedbackModel requires Order > 0");
+
+    public:
+        using StateVector = std::array<T, Order>;
+
+        virtual ~StrictFeedbackModel() = default;
+
+        virtual T Drift(std::size_t i, const StateVector& x) const = 0;
+        virtual T Gain(std::size_t i, const StateVector& x) const = 0;
+        virtual T VirtualDerivative(std::size_t i, const StateVector& x, T alpha) const = 0;
+    };
+
+    template<typename T, std::size_t Order>
+    class BacksteppingControl
+    {
+        static_assert(std::is_floating_point_v<T>, "BacksteppingControl supports floating-point types");
+        static_assert(Order > 0, "BacksteppingControl requires Order > 0");
+
+    public:
+        using StateVector = std::array<T, Order>;
+
+        struct Reference
+        {
+            T value{};
+            T derivative{};
+        };
+
+        BacksteppingControl(const StrictFeedbackModel<T, Order>& model,
+            const std::array<T, Order>& gains);
+
+        OPTIMIZE_FOR_SPEED T ComputeControl(const StateVector& x, const Reference& ref);
+        void Reset();
+
+    private:
+        const StrictFeedbackModel<T, Order>& model;
+        std::array<T, Order> gains;
+        std::array<T, Order> z{};
+    };
+
+    template<typename T, std::size_t Order>
+    BacksteppingControl<T, Order>::BacksteppingControl(
+        const StrictFeedbackModel<T, Order>& model,
+        const std::array<T, Order>& gains)
+        : model{ model }
+        , gains{ gains }
+    {
+        for (std::size_t i = 0; i < Order; ++i)
+            really_assert(gains[i] > T{ 0 });
+    }
+
+    template<typename T, std::size_t Order>
+    OPTIMIZE_FOR_SPEED T BacksteppingControl<T, Order>::ComputeControl(
+        const StateVector& x, const Reference& ref)
+    {
+        z[0] = x[0] - ref.value;
+        T alphaDot{ ref.derivative };
+
+        T alpha{ T{ 0 } };
+        for (std::size_t i = 0; i < Order; ++i)
+        {
+            const T f = model.Drift(i, x);
+            const T g = model.Gain(i, x);
+            really_assert(std::abs(g) > T{ 0 });
+
+            const T cross = (i == 0) ? T{ 0 } : model.Gain(i - 1, x) * z[i - 1];
+            alpha = (alphaDot - f - gains[i] * z[i] - cross) / g;
+
+            if (i < Order - 1)
+            {
+                z[i + 1] = x[i + 1] - alpha;
+                alphaDot = model.VirtualDerivative(i, x, alpha);
+            }
+        }
+
+        return alpha;
+    }
+
+    template<typename T, std::size_t Order>
+    void BacksteppingControl<T, Order>::Reset()
+    {
+        z = {};
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class BacksteppingControl<float, 1>;
+    extern template class BacksteppingControl<float, 2>;
+#endif
+}

numerical/nonlinear_control/CMakeLists.txt

@@ -11,10 +11,12 @@ target_link_libraries(numerical.nonlinear_control ${NUMERICAL_VISIBILITY}
 )
 
 target_sources(numerical.nonlinear_control PRIVATE
+    BacksteppingControl.hpp
     FeedbackLinearization.hpp
 )
 
 numerical_add_coverage_sources(numerical.nonlinear_control
+    BacksteppingControl.cpp
     FeedbackLinearization.cpp
 )
 

numerical/nonlinear_control/test/CMakeLists.txt

@@ -8,5 +8,6 @@ target_link_libraries(numerical.nonlinear_control_test PUBLIC
 )
 
 target_sources(numerical.nonlinear_control_test PRIVATE
+    TestBacksteppingControl.cpp
     TestFeedbackLinearization.cpp
 )