diff --git a/README.md b/README.md index 724000e4..3f6b63a6 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,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 | -| [Robust Control](doc/robust_control/README.md) | Sliding Mode Control (SMC), Disturbance Observer (DOB) | +| [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 | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index b4b0b804..ac2435ad 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|------------------------------------------------------|---------------------------|------------| -| 36 | Active Disturbance Rejection Control (ADRC + ESO) | `robust_control` (new) | ★★★★☆ | | 37 | Hilbert transform / analytic signal / envelope | `analysis` | ★★★★☆ | | 38 | Discrete Wavelet Transform (Haar / Daubechies) | `analysis` | ★★★★☆ | | 39 | Square-root / Information Kalman filter | `filters/active` | ★★★★☆ | diff --git a/doc/robust_control/ActiveDisturbanceRejection.md b/doc/robust_control/ActiveDisturbanceRejection.md new file mode 100644 index 00000000..0b8812b0 --- /dev/null +++ b/doc/robust_control/ActiveDisturbanceRejection.md @@ -0,0 +1,123 @@ +# Active Disturbance Rejection Control (ADRC + ESO) + +## Overview & Motivation + +Active Disturbance Rejection Control addresses a fundamental tension in feedback design: high-performance control normally requires an accurate plant model, yet accurate models are expensive to identify and degrade with temperature, load, and wear. ADRC resolves this by treating everything beyond a known input gain — unmodeled dynamics, parameter variation, and external disturbances — as a single lumped signal called the *total disturbance*. An Extended State Observer (ESO) estimates this signal in real time, and the control law subtracts the estimate before issuing the command. What remains behaves like a clean chain of integrators that a simple PD law can regulate with textbook bandwidth. + +The practical payoff on embedded hardware is significant: you need only one plant number ($b_0$, the rough input gain) and two tuning dials. The controller then survives a bad model because any mismatch is absorbed into the disturbance estimate. + +## Mathematical Theory + +### Plant Representation + +An $n$-th order SISO plant is written as the canonical integrator chain plus a total-disturbance term $f$: + +$$y^{(n)} = f(t, y, \dot{y}, \ldots, d) + b_0 u$$ + +where $f$ captures unmodeled dynamics, nonlinearities, and external loads; $b_0$ is a nominal input-gain estimate; and $u$ is the control input. + +### Extended State Observer + +Augmenting the $n$ plant states with $x_{n+1} = f$ yields an $(n+1)$-dimensional system. The continuous ESO is a Luenberger-type observer driven by the output error: + +$$\dot{\hat{x}}_i = \hat{x}_{i+1} + \beta_i (y - \hat{x}_1), \quad i = 1, \ldots, n$$ +$$\dot{\hat{x}}_{n+1} = \beta_{n+1} (y - \hat{x}_1)$$ + +with the convention $\hat{x}_{n+1} = \hat{f}$ and $\hat{x}_2$ through $\hat{x}_n$ as derivative estimates. + +The forward-Euler discretization used here is: + +$$\hat{x}_i[k+1] = \hat{x}_i[k] + T_s \bigl(\beta_i \, e[k] + \hat{x}_{i+1}[k]\bigr), \quad e[k] = y[k] - \hat{x}_1[k]$$ + +with $b_0 u[k-1]$ injected into the $(n)$-th state to drive the highest derivative. + +### Bandwidth Parameterization (Gao) + +All observer poles are placed at $-\omega_o$ (Gao's bandwidth parameterization). The resulting gains follow the binomial expansion of $(\lambda + \omega_o)^{n+1}$: + +$$\beta_i = \binom{n+1}{i} \omega_o^i, \quad i = 1, \ldots, n+1$$ + +All control poles are placed at $-\omega_c$ via the expansion of $(\lambda + \omega_c)^n$: + +$$k_i = \binom{n}{i} \omega_c^i, \quad i = 1, \ldots, n$$ + +For a second-order plant ($n = 2$): + +$$\beta = [3\omega_o,\; 3\omega_o^2,\; \omega_o^3], \quad k = [\omega_c^2,\; 2\omega_c]$$ + +### Control Law + +After disturbance estimation the control is: + +$$u = \frac{u_0 - \hat{f}}{b_0}, \qquad u_0 = k_1(r - \hat{x}_1) - \sum_{i=2}^{n} k_i \hat{x}_i$$ + +Substituting into the plant equation and using $\hat{f} \approx f$ gives the closed-loop residual $y^{(n)} \approx u_0$, a pure integrator chain under a PD law — independent of the original plant dynamics. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|---------|--------|--------|-----------------------------------------| +| Best | $O(n)$ | $O(n)$ | Linear sweep over $n+1$ ESO states | +| Average | $O(n)$ | $O(n)$ | Same; gains precomputed at construction | +| Worst | $O(n)$ | $O(n)$ | No branching in the hot path | + +Gains are computed once at construction from closed-form binomial formulas in $O(n)$ time. The `Compute` hot path is a pair of $O(n)$ loops with no dynamic allocation. + +## Step-by-Step Walkthrough + +Second-order plant ($n=2$), $\omega_o = 30$, $\omega_c = 6$, $b_0 = 1$, $T_s = 0.001$ s. + +Observer gains: $\beta_1 = 90$, $\beta_2 = 2700$, $\beta_3 = 27000$. +Control gains: $k_p = 36$, $k_d = 12$. + +At sample $k$ with state $\hat{x} = [\hat{y}, \hat{\dot{y}}, \hat{f}]$, measurement $y[k]$, reference $r$: + +1. Output error: $e = y[k] - \hat{y}$. +2. Inject correction into all three states: $\hat{x}_i \mathrel{+}= T_s \beta_i e$. +3. Chain integration: $\hat{y} \mathrel{+}= T_s \hat{\dot{y}}$; then $\hat{\dot{y}} \mathrel{+}= T_s b_0 u[k-1]$. +4. PD law on integrator chain: $u_0 = k_p(r - \hat{y}) - k_d \hat{\dot{y}}$. +5. Disturbance cancellation: $u = (u_0 - \hat{f}) / b_0$. + +After a transient of roughly $5/\omega_o \approx 0.17$ s the observer converges; the output tracks $r$ with bandwidth $\omega_c$. + +## Pitfalls & Edge Cases + +**ESO peaking.** Large initial estimation errors drive high-magnitude corrections, temporarily saturating the actuator. Mitigation: initialize the observer near the first measurement, or schedule $\omega_o$ upward from a low value during the first few samples. + +**Observer bandwidth vs. noise.** Increasing $\omega_o$ speeds convergence but amplifies measurement noise because $\beta_3 = \omega_o^3$ grows cubically. A practical rule of thumb is $\omega_o \in [3\omega_c, 10\omega_c]$. + +**$b_0$ mismatch.** The ESO is robust to moderate mismatch (factor of 2–3), but large errors shrink the stability margin. If $b_0 \gg b_\text{true}$ the effective loop gain drops and response slows; if $b_0 \ll b_\text{true}$ the loop gain rises and may oscillate. + +**Euler discretization accuracy.** The forward-Euler ESO introduces phase lag proportional to $\omega_o T_s$. Keeping $\omega_o T_s \ll 1$ (e.g., $\omega_o T_s \leq 0.1$) maintains accuracy; at higher $\omega_o T_s$ a ZOH or bilinear discretization is preferred. + +**Integer overflow in gain computation.** Binomial coefficients are computed with integer arithmetic at compile time. For large orders or very high bandwidths the intermediate product may exceed `std::size_t` before the division; keep $n \leq 5$ in practice. + +## Variants & Generalizations + +**Nonlinear ESO (NESO).** Replace the linear correction $\beta_i e$ with Han's fal function to reduce peaking while preserving fast convergence. + +**Discrete ESO.** Exact discretization of the observer (ZOH or pole-matched) improves accuracy when $\omega_o T_s$ is not small. + +**Higher-order plants.** The template parameter `Order` generalizes the same bandwidth-parameterized structure to $n > 2$ — gains grow binomially and the `Compute` loop extends automatically. + +**Multi-input / multi-output (MIMO).** Each output channel runs an independent ADRC; cross-coupling is absorbed into the respective disturbance estimates. + +## Applications + +- Electric motor drives (rejects friction, load torque, and back-EMF variation with a single $b_0$ estimate). +- Attitude and position control of UAVs and satellites (absorbs aerodynamic and thruster uncertainty). +- Industrial process control where the plant model is poorly known or time-varying. +- Hard-disk drive servo (high-bandwidth disturbance rejection without a detailed head-media model). + +## Connections to Other Algorithms + +- **Luenberger Observer** — the ESO is a Luenberger observer augmented with one extra disturbance state. +- **Disturbance Observer (DOB)** — the transfer-function sibling; DOB works in the frequency domain while ESO works in the state-space domain. +- **PID** — ADRC generalizes PID: a first-order ADRC with proportional-plus-integral action recovers a PI with disturbance feed-forward. +- **LQR / LQI** — state-feedback alternatives that require a full model; ADRC trades optimality for model-independence. + +## References & Further Reading + +- J. Han, "From PID to Active Disturbance Rejection Control," *IEEE Transactions on Industrial Electronics*, vol. 56, no. 3, pp. 900–906, 2009. +- Z. Gao, "Scaling and Bandwidth-Parameterization Based Controller Tuning," *Proceedings of the American Control Conference*, 2003, pp. 4989–4996. +- R. Miklosovic, A. Radke, Z. Gao, "Discrete implementation and generalization of the extended state observer," *ACC*, 2006. diff --git a/doc/robust_control/README.md b/doc/robust_control/README.md index d0a67f5d..4962b6cd 100644 --- a/doc/robust_control/README.md +++ b/doc/robust_control/README.md @@ -4,7 +4,8 @@ Algorithms for robust control design: controllers that explicitly account for di ## Algorithms -| Algorithm | Description | -|-----------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Sliding Mode Control](SlidingModeControl.md) | Variable-structure controller driving the state onto a sliding surface with a boundary layer to suppress chattering — robust to matched disturbances and parameter uncertainty | -| [Disturbance Observer](DisturbanceObserver.md) | Estimates lumped disturbance and model mismatch via the nominal plant inverse and a Q-filter, cancelling the disturbance to make the real plant behave like the nominal model | +| Algorithm | Description | +|-----------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Active Disturbance Rejection Control](ActiveDisturbanceRejection.md) | Near model-free controller pairing an Extended State Observer with bandwidth-parameterized PD feedback to estimate and cancel total disturbance in real time | +| [Sliding Mode Control](SlidingModeControl.md) | Variable-structure controller driving the state onto a sliding surface with a boundary layer to suppress chattering — robust to matched disturbances and parameter uncertainty | +| [Disturbance Observer](DisturbanceObserver.md) | Estimates lumped disturbance and model mismatch via the nominal plant inverse and a Q-filter, cancelling the disturbance to make the real plant behave like the nominal model | diff --git a/numerical/robust_control/ActiveDisturbanceRejection.cpp b/numerical/robust_control/ActiveDisturbanceRejection.cpp new file mode 100644 index 00000000..0a007491 --- /dev/null +++ b/numerical/robust_control/ActiveDisturbanceRejection.cpp @@ -0,0 +1,6 @@ +#include "numerical/robust_control/ActiveDisturbanceRejection.hpp" + +namespace robust_control +{ + template class ActiveDisturbanceRejectionControl; +} diff --git a/numerical/robust_control/ActiveDisturbanceRejection.hpp b/numerical/robust_control/ActiveDisturbanceRejection.hpp new file mode 100644 index 00000000..5a892576 --- /dev/null +++ b/numerical/robust_control/ActiveDisturbanceRejection.hpp @@ -0,0 +1,150 @@ +// Copyright (c) 2024 Numerical Toolbox Contributors +// SPDX-License-Identifier: MIT + +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace robust_control +{ + template + class ActiveDisturbanceRejectionControl + { + static_assert(std::is_floating_point_v, "ActiveDisturbanceRejectionControl supports floating-point types"); + static_assert(Order > 0, "ActiveDisturbanceRejectionControl requires Order > 0"); + + public: + using StateVector = math::Vector; + using ControlVector = math::Vector; + + ActiveDisturbanceRejectionControl(T observerBandwidth, T controlBandwidth, T b0, T sampleTime); + + OPTIMIZE_FOR_SPEED T Compute(T reference, T measuredOutput); + void Reset(); + + [[nodiscard]] static StateVector ObserverGainFromBandwidth(T wo); + [[nodiscard]] static ControlVector ControlGainFromBandwidth(T wc); + + [[nodiscard]] const StateVector& EstimatedState() const; + [[nodiscard]] T AppliedPrev() const; + + private: + StateVector xhat{}; + StateVector observerGain{}; + ControlVector controlGain{}; + T b0; + T sampleTime; + T appliedPrev{ T{ 0 } }; + }; + + namespace detail + { + constexpr std::size_t BinomialCoeff(std::size_t n, std::size_t k) + { + if (k == 0 || k == n) + return 1; + if (k > n) + return 0; + std::size_t result{ 1 }; + for (std::size_t i = 0; i < k; ++i) + { + result *= (n - i); + result /= (i + 1); + } + return result; + } + } + + template + ActiveDisturbanceRejectionControl::ActiveDisturbanceRejectionControl( + T observerBandwidth, T controlBandwidth, T b0, T sampleTime) + : observerGain{ ObserverGainFromBandwidth(observerBandwidth) } + , controlGain{ ControlGainFromBandwidth(controlBandwidth) } + , b0{ b0 } + , sampleTime{ sampleTime } + {} + + template + OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl::Compute(T reference, T measuredOutput) + { + const T e = measuredOutput - xhat.at(0, 0); + + for (std::size_t i = 0; i <= Order; ++i) + xhat.at(i, 0) += sampleTime * observerGain.at(i, 0) * e; + + for (std::size_t i = 0; i < Order; ++i) + xhat.at(i, 0) += sampleTime * xhat.at(i + 1, 0); + + xhat.at(Order - 1, 0) += sampleTime * b0 * appliedPrev; + + T u0 = controlGain.at(0, 0) * (reference - xhat.at(0, 0)); + for (std::size_t i = 1; i < Order; ++i) + u0 -= controlGain.at(i, 0) * xhat.at(i, 0); + + const T u = (u0 - xhat.at(Order, 0)) / b0; + appliedPrev = u; + return u; + } + + template + void ActiveDisturbanceRejectionControl::Reset() + { + xhat = StateVector{}; + appliedPrev = T{ 0 }; + } + + template + typename ActiveDisturbanceRejectionControl::StateVector + ActiveDisturbanceRejectionControl::ObserverGainFromBandwidth(T wo) + { + StateVector gains{}; + const std::size_t n = Order + 1; + T woPow{ wo }; + for (std::size_t i = 0; i < n; ++i) + { + const T coeff = static_cast(detail::BinomialCoeff(n, i + 1)); + gains.at(i, 0) = coeff * woPow; + woPow *= wo; + } + return gains; + } + + template + typename ActiveDisturbanceRejectionControl::ControlVector + ActiveDisturbanceRejectionControl::ControlGainFromBandwidth(T wc) + { + ControlVector gains{}; + T wcPow{ wc }; + for (std::size_t i = 0; i < Order; ++i) + { + const T coeff = static_cast(detail::BinomialCoeff(Order, i + 1)); + gains.at(Order - 1 - i, 0) = coeff * wcPow; + wcPow *= wc; + } + return gains; + } + + template + const typename ActiveDisturbanceRejectionControl::StateVector& + ActiveDisturbanceRejectionControl::EstimatedState() const + { + return xhat; + } + + template + T ActiveDisturbanceRejectionControl::AppliedPrev() const + { + return appliedPrev; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ActiveDisturbanceRejectionControl; +#endif +} diff --git a/numerical/robust_control/CMakeLists.txt b/numerical/robust_control/CMakeLists.txt index 2148c2f9..e98a9704 100644 --- a/numerical/robust_control/CMakeLists.txt +++ b/numerical/robust_control/CMakeLists.txt @@ -13,11 +13,13 @@ target_link_libraries(numerical.robust_control ${NUMERICAL_VISIBILITY} ) target_sources(numerical.robust_control PRIVATE + ActiveDisturbanceRejection.hpp DisturbanceObserver.hpp SlidingModeControl.hpp ) numerical_add_coverage_sources(numerical.robust_control + ActiveDisturbanceRejection.cpp DisturbanceObserver.cpp SlidingModeControl.cpp ) diff --git a/numerical/robust_control/test/CMakeLists.txt b/numerical/robust_control/test/CMakeLists.txt index 7ad28aa7..7fb58925 100644 --- a/numerical/robust_control/test/CMakeLists.txt +++ b/numerical/robust_control/test/CMakeLists.txt @@ -8,6 +8,7 @@ target_link_libraries(numerical.robust_control_test PUBLIC ) target_sources(numerical.robust_control_test PRIVATE + TestActiveDisturbanceRejection.cpp TestDisturbanceObserver.cpp TestSlidingModeControl.cpp ) diff --git a/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp new file mode 100644 index 00000000..f681f3fb --- /dev/null +++ b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp @@ -0,0 +1,138 @@ +#include "numerical/math/Tolerance.hpp" +#include "numerical/robust_control/ActiveDisturbanceRejection.hpp" +#include +#include + +namespace +{ + static constexpr float kWo{ 30.0f }; + static constexpr float kWc{ 6.0f }; + static constexpr float kB0{ 1.0f }; + static constexpr float kTs{ 0.001f }; + + struct SecondOrderPlant + { + float y{ 0.0f }; + float ydot{ 0.0f }; + float bTrue; + + explicit SecondOrderPlant(float b) + : bTrue{ b } + {} + + float Step(float u, float disturbance = 0.0f) + { + const float yddot = bTrue * u + disturbance; + ydot += kTs * yddot; + y += kTs * ydot; + return y; + } + }; + + class TestActiveDisturbanceRejection : public ::testing::Test + { + protected: + robust_control::ActiveDisturbanceRejectionControl adrc{ kWo, kWc, kB0, kTs }; + SecondOrderPlant plant{ kB0 }; + }; +} + +TEST_F(TestActiveDisturbanceRejection, bandwidth_gain_mapping) +{ + const auto og = robust_control::ActiveDisturbanceRejectionControl::ObserverGainFromBandwidth(kWo); + const auto cg = robust_control::ActiveDisturbanceRejectionControl::ControlGainFromBandwidth(kWc); + + EXPECT_NEAR(og.at(0, 0), 3.0f * kWo, math::Tolerance()); + EXPECT_NEAR(og.at(1, 0), 3.0f * kWo * kWo, math::Tolerance()); + EXPECT_NEAR(og.at(2, 0), kWo * kWo * kWo, math::Tolerance()); + + EXPECT_NEAR(cg.at(0, 0), kWc * kWc, math::Tolerance()); + EXPECT_NEAR(cg.at(1, 0), 2.0f * kWc, math::Tolerance()); +} + +TEST_F(TestActiveDisturbanceRejection, reset_clears_observer) +{ + for (int i = 0; i < 50; ++i) + adrc.Compute(1.0f, plant.Step(0.1f)); + + adrc.Reset(); + + const auto& xhat = adrc.EstimatedState(); + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(xhat.at(i, 0), 0.0f, math::Tolerance()); + + EXPECT_NEAR(adrc.AppliedPrev(), 0.0f, math::Tolerance()); +} + +TEST_F(TestActiveDisturbanceRejection, eso_converges) +{ + for (int i = 0; i < 3000; ++i) + { + const float y = plant.Step(adrc.Compute(0.0f, plant.y)); + (void)y; + } + + const float estimationError = plant.y - adrc.EstimatedState().at(0, 0); + EXPECT_NEAR(estimationError, 0.0f, 1e-2f); +} + +TEST_F(TestActiveDisturbanceRejection, tracks_step_reference) +{ + const float reference{ 1.0f }; + + for (int i = 0; i < 5000; ++i) + plant.Step(adrc.Compute(reference, plant.y)); + + EXPECT_NEAR(plant.y, reference, 1e-2f); +} + +TEST_F(TestActiveDisturbanceRejection, estimates_total_disturbance) +{ + const float constantDisturbance{ 5.0f }; + + for (int i = 0; i < 5000; ++i) + plant.Step(adrc.Compute(0.0f, plant.y), constantDisturbance); + + EXPECT_NEAR(adrc.EstimatedState().at(2, 0), constantDisturbance, 1.0f); +} + +TEST_F(TestActiveDisturbanceRejection, rejects_step_disturbance) +{ + const float reference{ 1.0f }; + + for (int i = 0; i < 3000; ++i) + plant.Step(adrc.Compute(reference, plant.y)); + + const float disturbance{ 3.0f }; + for (int i = 0; i < 5000; ++i) + plant.Step(adrc.Compute(reference, plant.y), disturbance); + + EXPECT_NEAR(plant.y, reference, 5e-2f); +} + +TEST_F(TestActiveDisturbanceRejection, control_cancels_disturbance_term) +{ + const float reference{ 0.0f }; + const float injectedDisturbance{ 10.0f }; + + for (int i = 0; i < 4000; ++i) + plant.Step(adrc.Compute(reference, plant.y), injectedDisturbance); + + const float fhat = adrc.EstimatedState().at(2, 0); + const float u = adrc.Compute(reference, plant.y); + + EXPECT_LT(fhat * u, 0.0f); +} + +TEST_F(TestActiveDisturbanceRejection, near_model_free_robustness) +{ + const float bTrue{ 2.0f }; + SecondOrderPlant mismatchedPlant{ bTrue }; + robust_control::ActiveDisturbanceRejectionControl controller{ kWo, kWc, kB0, kTs }; + + const float reference{ 1.0f }; + for (int i = 0; i < 8000; ++i) + mismatchedPlant.Step(controller.Compute(reference, mismatchedPlant.y)); + + EXPECT_NEAR(mismatchedPlant.y, reference, 0.1f); +} diff --git a/roadmap/robust_control/ActiveDisturbanceRejection/explanation.md b/roadmap/robust_control/ActiveDisturbanceRejection/explanation.md deleted file mode 100644 index 63e53cb6..00000000 --- a/roadmap/robust_control/ActiveDisturbanceRejection/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Active Disturbance Rejection Control (ADRC + ESO) — Overview - -## What it is -A near model-free controller built around an **Extended State Observer (ESO)**. The ESO treats the -*total* disturbance — everything acting on the plant beyond a known input gain — as one extra state, -estimates it in real time, and a simple feedback law cancels it. What remains behaves like a clean -chain of integrators that a textbook PD loop can control. - -## Why it matters (embedded) -ADRC delivers strong, robust motion control **without an accurate model**. You do not identify the -plant; you estimate and cancel its dynamics online. That is a huge win on microcontrollers driving -motors and actuators whose parameters drift with temperature, load, and wear — it is increasingly -the default in industrial drives precisely because it survives a bad model. - -## How it works (intuition) -Write the plant as "a chain of integrators plus an unknown lump `f`, driven by `b0·u`." The ESO runs -this model, compares its predicted output to the measurement, and uses the error to correct **every** -state — including the extra state that stands in for `f`. Because `f` absorbs all the unmodeled -physics, once you subtract the estimate `f̂` in the control law the residual system is just the -integrator chain. **Bandwidth parameterization** then reduces all tuning to two intuitive dials: -how fast the observer watches (`ω_o`) and how fast the loop responds (`ω_c`). - -## Key parameters -- **`ω_o` (observer bandwidth)** — how quickly the ESO tracks the disturbance; higher = faster but noisier. -- **`ω_c` (control bandwidth)** — closed-loop response speed after cancellation. -- **`b0` (input-gain estimate)** — the one plant number you must roughly know; the ESO forgives the rest. - -## Reference -J. Han, "From PID to Active Disturbance Rejection Control," *IEEE Trans. Industrial Electronics*, -56(3), 2009; Z. Gao, "Scaling and Bandwidth-Parameterization Based Controller Tuning," *ACC*, 2003. - -## See also -`LuenbergerObserver` (the linear-observer building block); `DisturbanceObserver` (transfer-function -sibling); `Pid` (the classical loop ADRC generalizes). diff --git a/roadmap/robust_control/ActiveDisturbanceRejection/implementation.md b/roadmap/robust_control/ActiveDisturbanceRejection/implementation.md deleted file mode 100644 index 41916693..00000000 --- a/roadmap/robust_control/ActiveDisturbanceRejection/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# Active Disturbance Rejection Control (ADRC + ESO) — Implementation Pseudocode - -> Roadmap ref: #36 (Tier 4) · Target: `numerical/robust_control` · Namespace `robust_control` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class ActiveDisturbanceRejectionControl: - # Extended State Observer: Order plant states + 1 total-disturbance state - math::Vector xhat # [ ŷ, ẏ̂, ..., f̂ ] (f̂ = total disturbance) - math::Vector observerGain # β, bandwidth-parameterized - math::Vector controlGain # k (kp, kd, ...), bandwidth-parameterized - T b0 # input-gain estimate - T sampleTime # Ts - T appliedPrev # last u (drives highest derivative) -``` - -## Interface - -``` -ActiveDisturbanceRejectionControl(T observerBandwidth, # ω_o - T controlBandwidth, # ω_c - T b0, T Ts) - -T Compute(T reference, T measuredOutput) # hot path -void Reset() -static Vector ObserverGainFromBandwidth(T wo) # β_i = C(n+1,i)·ω_o^i -static Vector ControlGainFromBandwidth(T wc) # place at -ω_c -``` - -## Algorithm (pseudocode) - -``` -function Compute(r, y): # OPTIMIZE_FOR_SPEED - # --- ESO: correct on the output error, then predict (discrete Euler) --- - e = y - xhat[0] # output estimation error - for i in 0 .. Order: # inject correction β_i·e into every state - xhat[i] += Ts * observerGain[i] * e - for i in 0 .. Order - 1: # chain of integrators - xhat[i] += Ts * xhat[i + 1] - xhat[Order - 1] += Ts * b0 * appliedPrev # b0·u drives the highest derivative - # (the disturbance state xhat[Order] moves only via its correction term) - - # --- control law: reject total disturbance f̂ = xhat[Order] --- - u0 = controlGain[0] * (r - xhat[0]) # kp·(r − ŷ) - for i in 1 .. Order - 1: - u0 -= controlGain[i] * xhat[i] # − kd·(derivative estimates) - u = (u0 - xhat[Order]) / b0 # subtract estimated disturbance, scale by 1/b0 - appliedPrev = u - return u -``` - -## Complexity & memory - -- `Compute`: `O(Order)` — the ESO and control law are linear sweeps over `Order + 1` states. -- Design-time: `O(Order)` binomial/pole formulas for the gains. -- Memory: `O(Order)` for the estimate and gain vectors — all static, no heap. - -## Numerical / embedded notes - -- **Bandwidth parameterization (Gao):** collapse all tuning to two knobs — observer bandwidth `ω_o` - and control bandwidth `ω_c` — by placing all ESO poles at `−ω_o` and all control poles at `−ω_c`. -- **Near model-free:** only `b0` and the two bandwidths are needed; `f̂` absorbs unmodeled dynamics - and external disturbance, so a rough `b0` still works. -- Typically `ω_o ≈ 3–10·ω_c`: too high amplifies measurement noise and causes ESO peaking, too low - leaves the disturbance unrejected. -- The Euler discretization shown is simplest; a ZOH/discrete ESO improves accuracy at low sample rates. -- Reuse item 19 (observer) structure and `math::Vector`; guard against **ESO peaking** at startup - (large initial `e`) by limiting the initial estimate or scheduling `ω_o`. -- 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/robust_control/ActiveDisturbanceRejection.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ActiveDisturbanceRejectionControl;` - under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/robust_control/ActiveDisturbanceRejection.cpp` → - `template class ActiveDisturbanceRejectionControl;` -- Test: `numerical/robust_control/test/TestActiveDisturbanceRejection.cpp` -- Doc: `doc/robust_control/ActiveDisturbanceRejection.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestActiveDisturbanceRejection.cpp` → the `_test` target. -- New module: create `numerical/robust_control/CMakeLists.txt` via `numerical_add_header_library(...)`, - add `test/`, register in `numerical/CMakeLists.txt`, add `doc/robust_control/`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/robust_control/ActiveDisturbanceRejection/tests.md b/roadmap/robust_control/ActiveDisturbanceRejection/tests.md deleted file mode 100644 index a2e1c4ef..00000000 --- a/roadmap/robust_control/ActiveDisturbanceRejection/tests.md +++ /dev/null @@ -1,61 +0,0 @@ -# Active Disturbance Rejection Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestActiveDisturbanceRejection : public ::testing::Test: - # second-order plant (Order = 2): ÿ = f + b·u - ActiveDisturbanceRejectionControl adrc{ - /*ω_o*/ 30.0f, /*ω_c*/ 6.0f, /*b0*/ 1.0f, /*Ts*/ 0.001f } - SecondOrderPlant plant = MakePlant(bTrue = 1.0f) -# each case below is a TEST_F(TestActiveDisturbanceRejection, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -estimates_total_disturbance: - Arrange: apply a constant load f, run the ESO to steady state - Assert: xhat[Order] (f̂) -> f - -rejects_step_disturbance: - Arrange: closed loop, step load disturbance mid-run - Assert: output returns to the reference (zero steady-state error) - -tracks_step_reference: - Arrange: step reference r - Assert: output y -> r with no steady-state offset - -eso_converges: - Arrange: known input, no disturbance - Assert: estimation error y - xhat[0] -> 0 - -bandwidth_gain_mapping: - Arrange: ω_o = 30, Order = 2 - Assert: observerGain == [3ω_o, 3ω_o², ω_o³]; controlGain == [ω_c², 2ω_c] - -near_model_free_robustness: - Arrange: true plant gain bTrue != b0 within a tolerance band - Assert: loop stays stable and still rejects the disturbance - -reset_clears_observer: - Arrange: run, accumulate estimates, Reset() - Assert: xhat == 0 and appliedPrev == 0 - -control_cancels_disturbance_term: - Arrange: nonzero f̂ in the estimate - Assert: u contains the −f̂/b0 cancellation term (correct sign/scale) -``` - -## Reference vectors - -- `Order = 2`, `ω_o`: `β = [3ω_o, 3ω_o², ω_o³]` — golden observer gains. -- `ω_c`: `kp = ω_c²`, `kd = 2ω_c` — golden control gains (critically-damped target). - -## Edge cases - -- `b0` mismatch — sweep `bTrue/b0`; assert the stable/robust ratio band and document the limit. -- `ω_o` too high — noise amplification and ESO peaking; assert bounded but noisy estimate. -- Low sample rate — Euler ESO inaccuracy; assert degraded but stable behaviour is documented.