diff --git a/doc/robust_control/ActiveDisturbanceRejection.md b/doc/robust_control/ActiveDisturbanceRejection.md index 0b8812b0..4123c478 100644 --- a/doc/robust_control/ActiveDisturbanceRejection.md +++ b/doc/robust_control/ActiveDisturbanceRejection.md @@ -88,7 +88,7 @@ After a transient of roughly $5/\omega_o \approx 0.17$ s the observer converges; **$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. +**Euler discretization stability and accuracy.** The forward-Euler ESO is unconditionally unstable once $\omega_o T_s$ exceeds the stability boundary of the $(n+1)$-th order observer polynomial. For a first-order plant ($n=1$) this boundary is $\omega_o T_s \approx 0.83$; higher orders have progressively tighter boundaries (approximately $0.54$ for $n=2$, $0.40$ for $n=3$). The constructor enforces $\omega_o T_s < 0.5/n$ as a conservative order-aware hard precondition (aborts if violated). Keeping $\omega_o T_s \leq 0.1$ is further recommended for accuracy; beyond that 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. diff --git a/numerical/robust_control/ActiveDisturbanceRejection.hpp b/numerical/robust_control/ActiveDisturbanceRejection.hpp index e2ec9330..96a74b4e 100644 --- a/numerical/robust_control/ActiveDisturbanceRejection.hpp +++ b/numerical/robust_control/ActiveDisturbanceRejection.hpp @@ -7,6 +7,7 @@ #pragma GCC optimize("O3", "fast-math") #endif +#include "infra/util/ReallyAssert.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" #include @@ -69,7 +70,9 @@ namespace robust_control , controlGain{ ControlGainFromBandwidth(controlBandwidth) } , b0{ b0 } , sampleTime{ sampleTime } - {} + { + really_assert(observerBandwidth * sampleTime < T{ 0.5 } / static_cast(Order)); + } template OPTIMIZE_FOR_SPEED T ActiveDisturbanceRejectionControl::Compute(T reference, T measuredOutput) diff --git a/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp index c1088e94..64e6a3d4 100644 --- a/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp +++ b/numerical/robust_control/test/TestActiveDisturbanceRejection.cpp @@ -60,6 +60,9 @@ namespace robust_control::ActiveDisturbanceRejectionControl adrc{ kWo1, kWc1, kB0, kTs }; FirstOrderPlant plant{ kB0 }; }; + + class TestBinomialCoeff : public ::testing::Test + {}; } TEST_F(TestActiveDisturbanceRejection, bandwidth_gain_mapping) @@ -223,22 +226,42 @@ TEST_F(TestActiveDisturbanceRejection, reset_mid_run_matches_fresh_instance) EXPECT_FLOAT_EQ(adrc.AppliedPrev(), fresh.AppliedPrev()); } -TEST(TestBinomialCoeff, k_zero_returns_one) +TEST_F(TestActiveDisturbanceRejection, eso_stable_at_high_observer_bandwidth) +{ + static constexpr float kWoHigh{ 200.0f }; + robust_control::ActiveDisturbanceRejectionControl controller{ kWoHigh, kWc, kB0, kTs }; + SecondOrderPlant highBwPlant{ kB0 }; + + for (int i = 0; i < 1000; ++i) + highBwPlant.Step(controller.Compute(1.0f, highBwPlant.y)); + + EXPECT_FALSE(std::isnan(highBwPlant.y)); + EXPECT_FALSE(std::isinf(highBwPlant.y)); +} + +TEST_F(TestActiveDisturbanceRejection, constructor_aborts_on_unstable_wo_ts) +{ + EXPECT_DEATH_IF_SUPPORTED( + (robust_control::ActiveDisturbanceRejectionControl{ 300.0f, kWc, kB0, kTs }), + ""); +} + +TEST_F(TestBinomialCoeff, k_zero_returns_one) { EXPECT_EQ(robust_control::detail::BinomialCoeff(5, 0), 1u); } -TEST(TestBinomialCoeff, k_equals_n_returns_one) +TEST_F(TestBinomialCoeff, k_equals_n_returns_one) { EXPECT_EQ(robust_control::detail::BinomialCoeff(4, 4), 1u); } -TEST(TestBinomialCoeff, k_greater_than_n_returns_zero) +TEST_F(TestBinomialCoeff, k_greater_than_n_returns_zero) { EXPECT_EQ(robust_control::detail::BinomialCoeff(3, 5), 0u); } -TEST(TestBinomialCoeff, known_interior_values) +TEST_F(TestBinomialCoeff, known_interior_values) { EXPECT_EQ(robust_control::detail::BinomialCoeff(4, 2), 6u); EXPECT_EQ(robust_control::detail::BinomialCoeff(5, 3), 10u);