fix(adrc): add 3-arg Compute for actuator saturation anti-windup (#276) (#282)

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 <noreply@anthropic.com>

Author: gabrielfrasantos

doc/robust_control/ActiveDisturbanceRejection.md

@@ -74,14 +74,16 @@ 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$.
 
 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
 
+**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]$.

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<typename T, std::size_t Order>
-    OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl<T, Order>::Compute(T reference, T measuredOutput)
+    OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl<T, Order>::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<typename T, std::size_t Order>
+    OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl<T, Order>::Compute(T reference, T measuredOutput)
+    {
+        return Compute(reference, measuredOutput, appliedPrev);
+    }
+
     template<typename T, std::size_t Order>
     void ActiveDisturbanceRejectionControl<T, Order>::Reset()
     {

numerical/robust_control/test/TestActiveDisturbanceRejection.cpp

@@ -1,5 +1,6 @@
 #include "numerical/math/Tolerance.hpp"
 #include "numerical/robust_control/ActiveDisturbanceRejection.hpp"
+#include <algorithm>
 #include <cmath>
 #include <gtest/gtest.h>
 
@@ -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<float>());
+    EXPECT_NEAR(adrc.AppliedPrev(), fresh.AppliedPrev(), math::Tolerance<float>());
 }
 
 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);
+}