diff --git a/README.md b/README.md index fdddbfe8..ca402b57 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 | -| [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 | diff --git a/ROADMAP.md b/ROADMAP.md index 5786f348..ceebf6f5 100644 --- a/ROADMAP.md +++ b/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 ★★★★★ diff --git a/doc/nonlinear_control/BacksteppingControl.md b/doc/nonlinear_control/BacksteppingControl.md new file mode 100644 index 00000000..c6cc3574 --- /dev/null +++ b/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. diff --git a/doc/nonlinear_control/README.md b/doc/nonlinear_control/README.md index 3e55cae6..63b70ad0 100644 --- a/doc/nonlinear_control/README.md +++ b/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 | diff --git a/numerical/nonlinear_control/BacksteppingControl.cpp b/numerical/nonlinear_control/BacksteppingControl.cpp new file mode 100644 index 00000000..76ecd724 --- /dev/null +++ b/numerical/nonlinear_control/BacksteppingControl.cpp @@ -0,0 +1,7 @@ +#include "numerical/nonlinear_control/BacksteppingControl.hpp" + +namespace nonlinear_control +{ + template class BacksteppingControl; + template class BacksteppingControl; +} diff --git a/numerical/nonlinear_control/BacksteppingControl.hpp b/numerical/nonlinear_control/BacksteppingControl.hpp new file mode 100644 index 00000000..c2649cc7 --- /dev/null +++ b/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 +#include +#include +#include + +namespace nonlinear_control +{ + template + class StrictFeedbackModel + { + static_assert(std::is_floating_point_v, "StrictFeedbackModel supports floating-point types"); + static_assert(Order > 0, "StrictFeedbackModel requires Order > 0"); + + public: + using StateVector = std::array; + + 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 + class BacksteppingControl + { + static_assert(std::is_floating_point_v, "BacksteppingControl supports floating-point types"); + static_assert(Order > 0, "BacksteppingControl requires Order > 0"); + + public: + using StateVector = std::array; + + struct Reference + { + T value{}; + T derivative{}; + }; + + BacksteppingControl(const StrictFeedbackModel& model, + const std::array& gains); + + OPTIMIZE_FOR_SPEED T ComputeControl(const StateVector& x, const Reference& ref); + void Reset(); + + private: + const StrictFeedbackModel& model; + std::array gains; + std::array z{}; + }; + + template + BacksteppingControl::BacksteppingControl( + const StrictFeedbackModel& model, + const std::array& gains) + : model{ model } + , gains{ gains } + { + for (std::size_t i = 0; i < Order; ++i) + really_assert(gains[i] > T{ 0 }); + } + + template + OPTIMIZE_FOR_SPEED T BacksteppingControl::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 + void BacksteppingControl::Reset() + { + z = {}; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class BacksteppingControl; + extern template class BacksteppingControl; +#endif +} diff --git a/numerical/nonlinear_control/CMakeLists.txt b/numerical/nonlinear_control/CMakeLists.txt index 46403be2..7b1ddf17 100644 --- a/numerical/nonlinear_control/CMakeLists.txt +++ b/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 ) diff --git a/numerical/nonlinear_control/test/CMakeLists.txt b/numerical/nonlinear_control/test/CMakeLists.txt index c3530222..d666a472 100644 --- a/numerical/nonlinear_control/test/CMakeLists.txt +++ b/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 ) diff --git a/numerical/nonlinear_control/test/TestBacksteppingControl.cpp b/numerical/nonlinear_control/test/TestBacksteppingControl.cpp new file mode 100644 index 00000000..63376a9d --- /dev/null +++ b/numerical/nonlinear_control/test/TestBacksteppingControl.cpp @@ -0,0 +1,364 @@ +#include "numerical/math/Tolerance.hpp" +#include "numerical/nonlinear_control/BacksteppingControl.hpp" +#include +#include +#include +#include + +namespace +{ + template + class MockStrictFeedbackModel + : public nonlinear_control::StrictFeedbackModel + { + public: + using StateVector = std::array; + MOCK_METHOD(T, Drift, (std::size_t, const StateVector&), (const, override)); + MOCK_METHOD(T, Gain, (std::size_t, const StateVector&), (const, override)); + MOCK_METHOD(T, VirtualDerivative, (std::size_t, const StateVector&, T), (const, override)); + }; + + class TestBacksteppingControlOrder1 : public ::testing::Test + { + protected: + ::testing::StrictMock> model; + std::array gains{ { 2.0f } }; + nonlinear_control::BacksteppingControl controller{ model, gains }; + }; + + class TestBacksteppingControlOrder2 : public ::testing::Test + { + protected: + ::testing::StrictMock> model; + std::array gains{ { 2.0f, 3.0f } }; + nonlinear_control::BacksteppingControl controller{ model, gains }; + }; + + template + class PendulumPlant + : public nonlinear_control::StrictFeedbackModel + { + public: + using StateVector = std::array; + + T Drift(std::size_t, const StateVector& x) const override + { + return std::sin(x[0]); + } + + T Gain(std::size_t, const StateVector&) const override + { + return T{ 1 }; + } + + T VirtualDerivative(std::size_t, const StateVector&, T) const override + { + return T{ 0 }; + } + }; + + template + class DoubleIntegratorPlant + : public nonlinear_control::StrictFeedbackModel + { + public: + using StateVector = std::array; + + explicit DoubleIntegratorPlant(const std::array& gains) + : gains{ gains } + {} + + T Drift(std::size_t, const StateVector&) const override + { + return T{ 0 }; + } + + T Gain(std::size_t, const StateVector&) const override + { + return T{ 1 }; + } + + T VirtualDerivative(std::size_t, const StateVector& x, T) const override + { + return -gains[0] * x[1]; + } + + private: + std::array gains; + }; + + class TestBacksteppingRealPlantOrder1 : public ::testing::Test + { + protected: + PendulumPlant plant; + std::array gains{ { 3.0f } }; + nonlinear_control::BacksteppingControl controller{ plant, gains }; + }; + + class TestBacksteppingRealPlantOrder2 : public ::testing::Test + { + protected: + std::array gains{ { 3.0f, 4.0f } }; + DoubleIntegratorPlant plant{ gains }; + nonlinear_control::BacksteppingControl controller{ plant, gains }; + }; +} + +TEST_F(TestBacksteppingControlOrder1, single_stage_reduces_to_proportional) +{ + const float e{ 0.5f }; + std::array x{ { e } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, -gains[0] * e, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, cancels_stage_drift) +{ + const float d{ 1.5f }; + std::array x{ { 0.0f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(d)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, -d, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, divides_by_control_gain) +{ + const float e{ 0.4f }; + const float g{ 2.0f }; + std::array x{ { e } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, (-gains[0] * e) / g, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder2, two_stage_includes_cross_term) +{ + const float z1{ 0.3f }; + const float z2{ 0.2f }; + const float g0{ 1.5f }; + const float g1{ 2.0f }; + const float f1{ 0.5f }; + const float alphaDot1{ 0.1f }; + + std::array x{ { z1, z1 + z2 } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + ::testing::InSequence seq; + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g0)); + EXPECT_CALL(model, VirtualDerivative(0, x, ::testing::_)).WillOnce(::testing::Return(alphaDot1)); + EXPECT_CALL(model, Drift(1, x)).WillOnce(::testing::Return(f1)); + EXPECT_CALL(model, Gain(1, x)).WillOnce(::testing::Return(g1)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g0)); + + const float u = controller.ComputeControl(x, ref); + + const float alpha0 = (-gains[0] * z1) / g0; + const float z2Actual = x[1] - alpha0; + const float expectedU = (alphaDot1 - f1 - gains[1] * z2Actual - g0 * z1) / g1; + + EXPECT_NEAR(u, expectedU, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder2, virtual_control_feeds_next_stage) +{ + const float x1{ 1.0f }; + const float x2{ 0.5f }; + const float g0{ 1.0f }; + + std::array x{ { x1, x2 } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + ::testing::InSequence seq; + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g0)); + EXPECT_CALL(model, VirtualDerivative(0, x, ::testing::_)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Drift(1, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(1, x)).WillOnce(::testing::Return(1.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g0)); + + controller.ComputeControl(x, ref); + + const float alpha0 = (-gains[0] * x1) / g0; + const float z2Expected = x2 - alpha0; + + EXPECT_NEAR(x2 - alpha0, z2Expected, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, reference_derivative_feedforward) +{ + const float rdot{ 0.8f }; + std::array x{ { 0.0f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, rdot }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, rdot, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder2, lyapunov_decreases_closed_loop) +{ + std::array x{ { 1.0f, 0.0f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + const float dt{ 0.01f }; + const float alpha0Init = -gains[0] * x[0]; + const float z1Init = x[0]; + const float z2Init = x[1] - alpha0Init; + float vPrev{ 0.5f * (z1Init * z1Init + z2Init * z2Init) }; + + for (int step = 0; step < 50; ++step) + { + const float alphaDotExpected = -gains[0] * x[1]; + { + ::testing::InSequence seq; + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + EXPECT_CALL(model, VirtualDerivative(0, x, ::testing::_)).WillOnce(::testing::Return(alphaDotExpected)); + EXPECT_CALL(model, Drift(1, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(1, x)).WillOnce(::testing::Return(1.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + } + + const float u = controller.ComputeControl(x, ref); + + x[0] += dt * x[1]; + x[1] += dt * u; + + const float alpha0New = -gains[0] * x[0]; + const float z1New = x[0]; + const float z2New = x[1] - alpha0New; + const float vNew = 0.5f * (z1New * z1New + z2New * z2New); + + EXPECT_LT(vNew, vPrev + math::Tolerance()); + vPrev = vNew; + (void)u; + } +} + +TEST_F(TestBacksteppingControlOrder1, gains_set_convergence_rate) +{ + std::array gainsLow{ { 1.0f } }; + std::array gainsHigh{ { 5.0f } }; + + ::testing::StrictMock> modelLow; + ::testing::StrictMock> modelHigh; + + nonlinear_control::BacksteppingControl ctrlLow{ modelLow, gainsLow }; + nonlinear_control::BacksteppingControl ctrlHigh{ modelHigh, gainsHigh }; + + const float dt{ 0.01f }; + const float tol{ 0.05f }; + float xLow{ 1.0f }; + float xHigh{ 1.0f }; + int stepsLow{ 0 }; + int stepsHigh{ 0 }; + + for (int i = 0; i < 500 && xLow > tol; ++i) + { + std::array state{ { xLow } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + EXPECT_CALL(modelLow, Drift(0, state)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(modelLow, Gain(0, state)).WillOnce(::testing::Return(1.0f)); + const float u = ctrlLow.ComputeControl(state, ref); + xLow += dt * u; + ++stepsLow; + } + + for (int i = 0; i < 500 && xHigh > tol; ++i) + { + std::array state{ { xHigh } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + EXPECT_CALL(modelHigh, Drift(0, state)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(modelHigh, Gain(0, state)).WillOnce(::testing::Return(1.0f)); + const float u = ctrlHigh.ComputeControl(state, ref); + xHigh += dt * u; + ++stepsHigh; + } + + EXPECT_LT(stepsHigh, stepsLow); +} + +TEST_F(TestBacksteppingRealPlantOrder1, cancels_nonlinearity_and_regulates) +{ + float x{ 1.0f }; + const float dt{ 0.001f }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + for (int i = 0; i < 5000; ++i) + { + std::array state{ { x } }; + const float u = controller.ComputeControl(state, ref); + x += dt * (std::sin(x) + u); + } + + EXPECT_NEAR(x, 0.0f, 1.0e-2f); +} + +TEST_F(TestBacksteppingRealPlantOrder1, tracks_constant_reference) +{ + float x{ 0.0f }; + const float dt{ 0.001f }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.7f, 0.0f }; + + for (int i = 0; i < 5000; ++i) + { + std::array state{ { x } }; + const float u = controller.ComputeControl(state, ref); + x += dt * (std::sin(x) + u); + } + + EXPECT_NEAR(x, ref.value, 1.0e-2f); +} + +TEST_F(TestBacksteppingRealPlantOrder2, reset_restores_initial_response) +{ + std::array x{ { 0.4f, -0.2f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + const float first = controller.ComputeControl(x, ref); + + std::array drift{ { 1.0f, 0.6f } }; + controller.ComputeControl(drift, ref); + controller.Reset(); + + const float afterReset = controller.ComputeControl(x, ref); + + EXPECT_NEAR(afterReset, first, math::Tolerance()); +} + +TEST_F(TestBacksteppingRealPlantOrder2, stabilizes_double_integrator) +{ + std::array x{ { 1.0f, -0.5f } }; + const float dt{ 0.001f }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + for (int i = 0; i < 20000; ++i) + { + const float u = controller.ComputeControl(x, ref); + x[0] += dt * x[1]; + x[1] += dt * u; + } + + EXPECT_NEAR(x[0], 0.0f, 1.0e-2f); + EXPECT_NEAR(x[1], 0.0f, 1.0e-2f); +} diff --git a/roadmap/nonlinear_control/BacksteppingControl/explanation.md b/roadmap/nonlinear_control/BacksteppingControl/explanation.md deleted file mode 100644 index 0bbe0c9b..00000000 --- a/roadmap/nonlinear_control/BacksteppingControl/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Backstepping Controller — Overview - -## What it is -A systematic, Lyapunov-based design method for **strict-feedback** nonlinear systems — cascades -where each state is driven by the next (`ẋ_i = f_i + g_i·x_{i+1}`) and only the last stage sees the -real input. Backstepping stabilizes the chain **one integrator at a time**, treating each -downstream state as a *virtual control* for the stage above it. - -## Why it matters (embedded) -Many real plants are naturally cascaded: motor current → torque → velocity → position; thrust → -attitude → position. Backstepping gives a *constructive recipe* — no guesswork — that yields a -controller **with a stability proof attached**. For safety-critical electromechanical and flight -loops, "provably stable by construction" is worth far more than an empirically tuned PID. - -## How it works (intuition) -Start at the output. Define the tracking error `z_1` and pretend the next state `x_2` is your knob: -pick the value `α_1` it *should* take to make `z_1` shrink. Of course `x_2` can't jump there -instantly, so define a new error `z_2 = x_2 − α_1` and repeat — now `x_3` is the virtual control. -March down the chain until you reach the real input `u` at the last stage. A single Lyapunov -function `V = ½·Σz_i²` accumulates every stage; making `V̇ < 0` at each step guarantees the whole -cascade converges. The catch: each virtual control's *derivative* `α̇` must be carried forward -analytically. - -## Key parameters -- **strict-feedback model** — injected per-stage drift `f_i`, gain `g_i`, and virtual-control - derivatives; the design assumes this structure. -- **stage gains k_i > 0** — one per integrator; each sets that stage's error-decay rate. -- **reference and its derivatives** — feedforward that the first stage tracks. - -## Reference -M. Krstić, I. Kanellakopoulos, P. Kokotović, *Nonlinear and Adaptive Control Design* (Wiley, 1995). - -## See also -`FeedbackLinearization` (cancels rather than dominates nonlinearities); -`ModelReferenceAdaptiveControl` (adaptive backstepping handles unknown parameters); -`SlidingModeControl` (robust alternative for matched uncertainty). diff --git a/roadmap/nonlinear_control/BacksteppingControl/implementation.md b/roadmap/nonlinear_control/BacksteppingControl/implementation.md deleted file mode 100644 index 9bd876c1..00000000 --- a/roadmap/nonlinear_control/BacksteppingControl/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# Backstepping Controller — Implementation Pseudocode - -> Roadmap ref: #41 (Tier 4) · Target: `numerical/nonlinear_control` · Namespace `nonlinear_control` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -# Injected strict-feedback model exposes per-stage drift/gain (DIP): -template # pure interface -interface StrictFeedbackModel: - T Drift(int i, const StateVector& x) const # f_i(x_1..x_{i+1}) - T Gain (int i, const StateVector& x) const # g_i(x_1..x_{i+1}), ≠ 0 - T VirtualDerivative(int i, const StateVector& x, T alpha) const # α̇_{i-1} - -template # static_assert(std::is_floating_point_v); instantiated for float -class BacksteppingControl: - const StrictFeedbackModel& model - std::array gains # k_1..k_n > 0 -``` - -## Interface - -``` -BacksteppingControl(const StrictFeedbackModel& model, - const std::array& gains) - -T ComputeControl(const StateVector& x, const Reference& ref) # hot path -void Reset() -``` - -## Algorithm (pseudocode) - -``` -function ComputeControl(x, ref): # OPTIMIZE_FOR_SPEED - # Strict-feedback chain: ẋ_i = f_i + g_i · x_{i+1}, ẋ_n = f_n + g_n · u - z[0] = x[0] - ref.value # tracking error, stage 1 - alphaDot = ref.derivative # α̇_0 = ṙ (feedforward) - for i in 0 .. Order-1: - f = model.Drift(i, x) - g = model.Gain (i, x) # controllability: g ≠ 0 - cross = (i == 0) ? 0 : model.Gain(i-1, x) * z[i-1] # Lyapunov cross term - - # virtual control that makes V̇_i = -Σ k·z² along the chain - alpha_i = ( alphaDot - f - gains[i]*z[i] - cross ) / g - - if i < Order-1: - z[i+1] = x[i+1] - alpha_i # next-stage error - alphaDot = model.VirtualDerivative(i, x, alpha_i) # ∂α_i/∂x·ẋ + ∂α_i/∂t - else: - return alpha_i # α_n is the actual input u -``` - -## Complexity & memory - -- `ComputeControl`: `O(Order)` — a single forward sweep through the integrator chain. -- Memory: `O(Order)` for the error stack `z` and the gains; static, no heap, no recursion. - -## Numerical / embedded notes - -- **Controllability guard:** each `g_i(x)` divides the virtual control; assert `|g_i| ≥ ε` and keep - the state in the region where the chain is invertible. -- The **hard part** is `α̇_{i-1}`: the virtual control's time derivative must be propagated - analytically (chain rule) or the design loses its stability guarantee — do *not* differentiate it - numerically on-target (noise amplification). Inject it via `VirtualDerivative`. -- Lyapunov certificate `V = ½·Σ z_i²` decreases monotonically ⇒ provable stability; the gains - `k_i > 0` set each stage's decay rate independently. -- Combine with online estimation for unknown parameters ⇒ *adaptive backstepping* (bridges to MRAC). -- 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/BacksteppingControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeControl`, and - `extern template class BacksteppingControl;` - under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/nonlinear_control/BacksteppingControl.cpp` → - `template class BacksteppingControl;` -- Test: `numerical/nonlinear_control/test/TestBacksteppingControl.cpp` -- Doc: `doc/nonlinear_control/BacksteppingControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestBacksteppingControl.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/BacksteppingControl/tests.md b/roadmap/nonlinear_control/BacksteppingControl/tests.md deleted file mode 100644 index 32be3162..00000000 --- a/roadmap/nonlinear_control/BacksteppingControl/tests.md +++ /dev/null @@ -1,62 +0,0 @@ -# Backstepping Controller — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestBacksteppingControl : public ::testing::Test: - # Strict-feedback model injected as a StrictMock so control law is verified in isolation: - StrictMock> model - std::array gains = { 2.0, 3.0 } # k1, k2 > 0 - BacksteppingControl controller{ model, gains } -# each case below is a TEST_F(TestBacksteppingControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -single_stage_reduces_to_proportional: - Arrange: Order=1, f=0, g=1, ref=0, x=[e] - Act: u = ComputeControl(x, ref) - Assert: u == -k1 · e (first-stage virtual control = actual input) - -cancels_stage_drift: - Arrange: Order=1, f=d, g=1, z=0 - Assert: u == -d (drift cancelled exactly) - -divides_by_control_gain: - Arrange: Order=1, f=0, g=2, z=e - Assert: u == (-k1 · e) / 2 (gain g scales the virtual control) - -two_stage_includes_cross_term: - Arrange: Order=2; z1, z2 nonzero; g0, g1 given - Assert: u == ( α̇1 − f1 − k2·z2 − g0·z1 ) / g1 (Lyapunov cross term present) - -virtual_control_feeds_next_stage: - Arrange: Order=2, capture α1 for stage 0 - Assert: z2 computed as x2 − α1 (error propagated down the chain) - -reference_derivative_feedforward: - Arrange: ref.derivative = ṙ != 0, everything else 0 - Assert: ṙ appears in stage-0 virtual control (feedforward path) - -lyapunov_decreases_closed_loop: - Arrange: wrap an integrator-chain plant, run K steps from x0 != ref - Assert: V = ½Σz_i² strictly decreases each step; z -> 0 - -gains_set_convergence_rate: - Arrange: two runs with k1 = 1 vs k1 = 5 - Assert: larger gain reaches tol in fewer steps -``` - -## Reference vectors - -- Order-1, `f=0`, `g=1`: closed loop is `ż1 = −k1·z1` ⇒ `z1[k] = z1[0]·e^(−k1·t)`, hand-computable. -- Order-2 chain of integrators: `V̇ = −k1·z1² − k2·z2² < 0` for any nonzero error. - -## Edge cases - -- `g_i(x) -> 0` (loss of controllability): division guard triggers documented failure/hold. -- Deadbeat-like large gains: verify no overshoot artefact from the discrete step. -- Reference with zero derivative vs ramp: feedforward term correctly absent/present.