From 68bea53d0892ac89a049302fe12e91b9cb725dd7 Mon Sep 17 00:00:00 2001 From: gfs Date: Fri, 14 Aug 2026 14:53:01 +0000 Subject: [PATCH] fix(adrc): add 3-arg Compute for actuator saturation anti-windup (#276) When the downstream actuator clips the commanded value, the ESO was integrating the wrong input, causing the disturbance estimate to drift and producing excessive overshoot. The new Compute(reference, measuredOutput, actualApplied) overload lets callers close the anti-windup loop by feeding the actually-applied signal back into the ESO. The two-argument overload delegates to it for zero-change backwards compatibility. Also fixes a pre-existing EXPECT_FLOAT_EQ in reset_mid_run_matches_fresh_instance that failed under fast-math due to register-allocation differences between differently-addressed objects; replaced with EXPECT_NEAR using math::Tolerance. Co-Authored-By: Claude Sonnet 4.6 --- .../ActiveDisturbanceRejection.md | 4 +++- .../ActiveDisturbanceRejection.hpp | 11 ++++++++-- .../test/TestActiveDisturbanceRejection.cpp | 22 +++++++++++++++++-- 3 files changed, 32 insertions(+), 5 deletions(-) diff --git a/doc/robust_control/ActiveDisturbanceRejection.md b/doc/robust_control/ActiveDisturbanceRejection.md index 4123c47..a2c496e 100644 --- a/doc/robust_control/ActiveDisturbanceRejection.md +++ b/doc/robust_control/ActiveDisturbanceRejection.md @@ -74,7 +74,7 @@ At sample $k$ with state $\hat{x} = [\hat{y}, \hat{\dot{y}}, \hat{f}]$, measurem 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]$. +3. Chain integration: $\hat{y} \mathrel{+}= T_s \hat{\dot{y}}$; then $\hat{\dot{y}} \mathrel{+}= T_s b_0 u_\text{applied}[k-1]$, where $u_\text{applied}$ is the value actually delivered by the actuator. When the actuator does not saturate this equals the commanded $u[k-1]$; when it does, pass the clipped value via the three-argument `Compute(reference, measuredOutput, actualApplied)` overload. 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$. @@ -82,6 +82,8 @@ After a transient of roughly $5/\omega_o \approx 0.17$ s the observer converges; ## Pitfalls & Edge Cases +**Actuator saturation (anti-windup).** When the downstream actuator clips the commanded value, the ESO integrates the wrong input and its disturbance estimate drifts, causing significant overshoot. Close the anti-windup loop by feeding the actually-applied signal back: `u_applied = clamp(Compute(r, y, u_applied_prev), lo, hi)`, then pass `u_applied` as the third argument on the next call. The two-argument `Compute(reference, measuredOutput)` is a convenience overload that assumes no saturation (commanded = applied) and is equivalent to passing the last commanded value. + **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]$. diff --git a/numerical/robust_control/ActiveDisturbanceRejection.hpp b/numerical/robust_control/ActiveDisturbanceRejection.hpp index 96a74b4..6bce237 100644 --- a/numerical/robust_control/ActiveDisturbanceRejection.hpp +++ b/numerical/robust_control/ActiveDisturbanceRejection.hpp @@ -27,6 +27,7 @@ namespace robust_control ActiveDisturbanceRejectionControl(T observerBandwidth, T controlBandwidth, T b0, T sampleTime); + OPTIMIZE_FOR_SPEED T Compute(T reference, T measuredOutput, T actualApplied); OPTIMIZE_FOR_SPEED T Compute(T reference, T measuredOutput); void Reset(); @@ -75,7 +76,7 @@ namespace robust_control } template - OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl::Compute(T reference, T measuredOutput) + OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl::Compute(T reference, T measuredOutput, T actualApplied) { const T e = measuredOutput - xhat.at(0, 0); @@ -85,7 +86,7 @@ namespace robust_control 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; + xhat.at(Order - 1, 0) += sampleTime * b0 * actualApplied; T u0 = controlGain.at(0, 0) * (reference - xhat.at(0, 0)); for (std::size_t i = 1; i < Order; ++i) @@ -96,6 +97,12 @@ namespace robust_control return u; } + template + OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl::Compute(T reference, T measuredOutput) + { + return Compute(reference, measuredOutput, appliedPrev); + } + template void ActiveDisturbanceRejectionControl::Reset() { diff --git a/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp index 64e6a3d..ab6b28b 100644 --- a/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp +++ b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp @@ -1,5 +1,6 @@ #include "numerical/math/Tolerance.hpp" #include "numerical/robust_control/ActiveDisturbanceRejection.hpp" +#include #include #include @@ -222,8 +223,8 @@ TEST_F(TestActiveDisturbanceRejection, reset_mid_run_matches_fresh_instance) freshPlant.Step(uFresh); } - EXPECT_FLOAT_EQ(plant.y, freshPlant.y); - EXPECT_FLOAT_EQ(adrc.AppliedPrev(), fresh.AppliedPrev()); + EXPECT_NEAR(plant.y, freshPlant.y, math::Tolerance()); + EXPECT_NEAR(adrc.AppliedPrev(), fresh.AppliedPrev(), math::Tolerance()); } TEST_F(TestActiveDisturbanceRejection, eso_stable_at_high_observer_bandwidth) @@ -266,3 +267,20 @@ TEST_F(TestBinomialCoeff, known_interior_values) EXPECT_EQ(robust_control::detail::BinomialCoeff(4, 2), 6u); EXPECT_EQ(robust_control::detail::BinomialCoeff(5, 3), 10u); } + +TEST_F(TestActiveDisturbanceRejection, tracks_step_reference_with_saturated_actuator) +{ + const float reference{ 1.0f }; + const float satLimit{ 5.0f }; + float actualApplied{ 0.0f }; + + for (int i = 0; i < 8000; ++i) + { + const float commanded = adrc.Compute(reference, plant.y, actualApplied); + actualApplied = std::clamp(commanded, -satLimit, satLimit); + plant.Step(actualApplied); + } + + EXPECT_NEAR(plant.y, reference, 1e-2f); + EXPECT_NEAR(adrc.EstimatedState().at(0, 0), reference, 1e-2f); +}