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

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