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); +}