feat: improve robust controllers

numerical/robust_control/test/TestActiveDisturbanceRejection.cpp

@@ -29,12 +29,37 @@ namespace
         }
     };
 
+    struct FirstOrderPlant
+    {
+        float y{ 0.0f };
+        float bTrue;
+
+        explicit FirstOrderPlant(float b)
+            : bTrue{ b }
+        {}
+
+        float Step(float u, float disturbance = 0.0f)
+        {
+            y += kTs * (bTrue * u + disturbance);
+            return y;
+        }
+    };
+
     class TestActiveDisturbanceRejection : public ::testing::Test
     {
     protected:
         robust_control::ActiveDisturbanceRejectionControl<float, 2> adrc{ kWo, kWc, kB0, kTs };
         SecondOrderPlant plant{ kB0 };
     };
+
+    class TestActiveDisturbanceRejectionOrder1 : public ::testing::Test
+    {
+    protected:
+        static constexpr float kWo1{ 20.0f };
+        static constexpr float kWc1{ 5.0f };
+        robust_control::ActiveDisturbanceRejectionControl<float, 1> adrc{ kWo1, kWc1, kB0, kTs };
+        FirstOrderPlant plant{ kB0 };
+    };
 }
 
 TEST_F(TestActiveDisturbanceRejection, bandwidth_gain_mapping)
@@ -136,3 +161,64 @@ TEST_F(TestActiveDisturbanceRejection, near_model_free_robustness)
 
     EXPECT_NEAR(mismatchedPlant.y, reference, 0.1f);
 }
+
+TEST_F(TestActiveDisturbanceRejectionOrder1, bandwidth_gain_mapping_order1)
+{
+    const auto og = robust_control::ActiveDisturbanceRejectionControl<float, 1>::ObserverGainFromBandwidth(kWo1);
+    const auto cg = robust_control::ActiveDisturbanceRejectionControl<float, 1>::ControlGainFromBandwidth(kWc1);
+
+    EXPECT_NEAR(og.at(0, 0), 2.0f * kWo1, math::Tolerance<float>());
+    EXPECT_NEAR(og.at(1, 0), 1.0f * kWo1 * kWo1, math::Tolerance<float>());
+
+    EXPECT_NEAR(cg.at(0, 0), 1.0f * kWc1, math::Tolerance<float>());
+}
+
+TEST_F(TestActiveDisturbanceRejection, zero_input_zero_output_first_call)
+{
+    const float u = adrc.Compute(0.0f, 0.0f);
+    EXPECT_NEAR(u, 0.0f, math::Tolerance<float>());
+}
+
+TEST_F(TestActiveDisturbanceRejection, long_horizon_bounded_output)
+{
+    const float reference{ 1.0f };
+    float maxOutput{ 0.0f };
+
+    for (int i = 0; i < 20000; ++i)
+    {
+        const float u = adrc.Compute(reference, plant.y);
+        plant.Step(u);
+        const float absY = std::abs(plant.y);
+        if (absY > maxOutput)
+            maxOutput = absY;
+    }
+
+    EXPECT_LT(maxOutput, 10.0f);
+    EXPECT_FALSE(std::isnan(plant.y));
+    EXPECT_FALSE(std::isinf(plant.y));
+}
+
+TEST_F(TestActiveDisturbanceRejection, reset_mid_run_matches_fresh_instance)
+{
+    const float reference{ 1.0f };
+
+    for (int i = 0; i < 500; ++i)
+        plant.Step(adrc.Compute(reference, plant.y));
+
+    adrc.Reset();
+    plant = SecondOrderPlant{ kB0 };
+
+    robust_control::ActiveDisturbanceRejectionControl<float, 2> fresh{ kWo, kWc, kB0, kTs };
+    SecondOrderPlant freshPlant{ kB0 };
+
+    for (int i = 0; i < 200; ++i)
+    {
+        const float uReset = adrc.Compute(reference, plant.y);
+        const float uFresh = fresh.Compute(reference, freshPlant.y);
+        plant.Step(uReset);
+        freshPlant.Step(uFresh);
+    }
+
+    EXPECT_FLOAT_EQ(plant.y, freshPlant.y);
+    EXPECT_FLOAT_EQ(adrc.AppliedPrev(), fresh.AppliedPrev());
+}

numerical/robust_control/test/TestDisturbanceObserver.cpp

@@ -1,4 +1,3 @@
-// Copyright (c) 2024, Numerical Toolbox Contributors. All rights reserved.
 #include "numerical/filters/passive/BiquadCascade.hpp"
 #include "numerical/math/LinearTimeInvariant.hpp"
 #include "numerical/math/Matrix.hpp"
@@ -26,6 +25,18 @@ namespace
         return plant;
     }
 
+    math::LinearTimeInvariant<float, 2, 2, 2> MakeTwoChannelPlant()
+    {
+        math::LinearTimeInvariant<float, 2, 2, 2> plant{};
+        plant.A.at(0, 0) = kPlantA;
+        plant.A.at(1, 1) = kPlantA;
+        plant.B.at(0, 0) = kPlantB;
+        plant.B.at(1, 1) = kPlantB;
+        plant.C.at(0, 0) = kPlantC;
+        plant.C.at(1, 1) = kPlantC;
+        return plant;
+    }
+
     filters::passive::BiquadCoeffs<float> MakeLowPassQ()
     {
         return filters::passive::Biquad<float>::LowPass(kCutoffHz, kSampleRateHz, kQ);
@@ -241,3 +252,147 @@ TEST_F(TestDisturbanceObserver, robust_to_small_model_mismatch)
 
     EXPECT_LT(std::abs(dob.Disturbance().at(0, 0) - kDisturbance), kDisturbance);
 }
+
+TEST_F(TestDisturbanceObserver, zero_disturbance_passthrough)
+{
+    static constexpr int kSteps{ 4000 };
+
+    math::Vector<float, 1> x{};
+    math::Vector<float, 1> u{};
+
+    math::Vector<float, 1> c{};
+    c.at(0, 0) = 0.7f;
+
+    for (int k{ 0 }; k < kSteps; ++k)
+    {
+        const math::Vector<float, 1> y{ nominal.Output(x, u) };
+        u = dob.Compute(c, y);
+        x = nominal.Step(x, u);
+    }
+
+    EXPECT_NEAR(u.at(0, 0), c.at(0, 0), 1e-2f);
+}
+
+TEST_F(TestDisturbanceObserver, initial_disturbance_is_zero)
+{
+    EXPECT_NEAR(dob.Disturbance().at(0, 0), 0.0f, math::Tolerance<float>());
+}
+
+TEST_F(TestDisturbanceObserver, reset_then_reconverge)
+{
+    static constexpr float kDisturbance{ 0.5f };
+    static constexpr int kWarmup{ 4000 };
+
+    math::Vector<float, 1> x{};
+    math::Vector<float, 1> c{};
+    math::Vector<float, 1> u{};
+
+    for (int k{ 0 }; k < kWarmup; ++k)
+    {
+        const math::Vector<float, 1> y{ nominal.Output(x, u) };
+        u = dob.Compute(c, y);
+        math::Vector<float, 1> uActual{};
+        uActual.at(0, 0) = u.at(0, 0) + kDisturbance;
+        x = nominal.Step(x, uActual);
+    }
+
+    dob.Reset();
+    x = math::Vector<float, 1>{};
+    u = math::Vector<float, 1>{};
+
+    for (int k{ 0 }; k < kWarmup; ++k)
+    {
+        const math::Vector<float, 1> y{ nominal.Output(x, u) };
+        u = dob.Compute(c, y);
+        math::Vector<float, 1> uActual{};
+        uActual.at(0, 0) = u.at(0, 0) + kDisturbance;
+        x = nominal.Step(x, uActual);
+    }
+
+    EXPECT_NEAR(dob.Disturbance().at(0, 0), kDisturbance, 1e-2f);
+}
+
+TEST_F(TestDisturbanceObserver, determinism_two_instances_agree)
+{
+    static constexpr float kDisturbance{ 0.3f };
+    static constexpr int kSteps{ 2000 };
+
+    robust_control::DisturbanceObserver<float, 1, 1, 1> dob2{ nominal, qCoeffs };
+
+    math::Vector<float, 1> x1{};
+    math::Vector<float, 1> x2{};
+    math::Vector<float, 1> c{};
+    math::Vector<float, 1> u1{};
+    math::Vector<float, 1> u2{};
+
+    for (int k{ 0 }; k < kSteps; ++k)
+    {
+        const math::Vector<float, 1> y1{ nominal.Output(x1, u1) };
+        const math::Vector<float, 1> y2{ nominal.Output(x2, u2) };
+        u1 = dob.Compute(c, y1);
+        u2 = dob2.Compute(c, y2);
+        math::Vector<float, 1> ua1{};
+        math::Vector<float, 1> ua2{};
+        ua1.at(0, 0) = u1.at(0, 0) + kDisturbance;
+        ua2.at(0, 0) = u2.at(0, 0) + kDisturbance;
+        x1 = nominal.Step(x1, ua1);
+        x2 = nominal.Step(x2, ua2);
+    }
+
+    EXPECT_FLOAT_EQ(dob.Disturbance().at(0, 0), dob2.Disturbance().at(0, 0));
+}
+
+TEST_F(TestDisturbanceObserver, two_channel_independent_disturbance_estimation)
+{
+    static constexpr float kDist0{ 0.4f };
+    static constexpr float kDist1{ 0.8f };
+    static constexpr int kSteps{ 4000 };
+
+    const math::LinearTimeInvariant<float, 2, 2, 2> plant2ch{ MakeTwoChannelPlant() };
+    filters::passive::BiquadCoeffs<float> q2{ MakeLowPassQ() };
+    robust_control::DisturbanceObserver<float, 2, 2, 2> dob2ch{ plant2ch, q2 };
+
+    math::Vector<float, 2> x{};
+    math::Vector<float, 2> c{};
+    math::Vector<float, 2> u{};
+
+    for (int k{ 0 }; k < kSteps; ++k)
+    {
+        const math::Vector<float, 2> y{ plant2ch.Output(x, u) };
+        u = dob2ch.Compute(c, y);
+        math::Vector<float, 2> uActual{};
+        uActual.at(0, 0) = u.at(0, 0) + kDist0;
+        uActual.at(1, 0) = u.at(1, 0) + kDist1;
+        x = plant2ch.Step(x, uActual);
+    }
+
+    EXPECT_NEAR(dob2ch.Disturbance().at(0, 0), kDist0, 1e-2f);
+    EXPECT_NEAR(dob2ch.Disturbance().at(1, 0), kDist1, 1e-2f);
+}
+
+TEST_F(TestDisturbanceObserver, step_disturbance_transient_crosses_half_value)
+{
+    static constexpr float kDisturbance{ 1.0f };
+    static constexpr int kMaxTransientSteps{ 500 };
+
+    math::Vector<float, 1> x{};
+    math::Vector<float, 1> c{};
+    math::Vector<float, 1> u{};
+
+    bool crossedHalf{ false };
+    for (int k{ 0 }; k < kMaxTransientSteps; ++k)
+    {
+        const math::Vector<float, 1> y{ nominal.Output(x, u) };
+        u = dob.Compute(c, y);
+        math::Vector<float, 1> uActual{};
+        uActual.at(0, 0) = u.at(0, 0) + kDisturbance;
+        x = nominal.Step(x, uActual);
+        if (dob.Disturbance().at(0, 0) >= 0.5f * kDisturbance)
+        {
+            crossedHalf = true;
+            break;
+        }
+    }
+
+    EXPECT_TRUE(crossedHalf);
+}

numerical/robust_control/test/TestHInfinityStateFeedback.cpp

@@ -37,6 +37,18 @@ namespace
         Plant plant{ MakeGeneralizedPlant() };
         HInf hinf{ plant };
     };
+
+    class TestHInfinityStateFeedbackSynthesized : public ::testing::Test
+    {
+    protected:
+        Plant plant{ MakeGeneralizedPlant() };
+        HInf hinf{ plant };
+
+        void SetUp() override
+        {
+            hinf.Synthesize(0.5f, 100.0f, 1e-3f);
+        }
+    };
 }
 
 TEST_F(TestHInfinityStateFeedback, reduces_to_lqr_as_gamma_large)
@@ -205,3 +217,84 @@ TEST_F(TestHInfinityStateFeedback, compute_control_is_negative_feedback)
     const float expected = -(K.at(0, 0) * x.at(0, 0) + K.at(0, 1) * x.at(1, 0));
     EXPECT_NEAR(u.at(0, 0), expected, math::Tolerance<float>());
 }
+
+TEST_F(TestHInfinityStateFeedbackSynthesized, zero_state_produces_zero_control)
+{
+    const math::Vector<float, 2> x{};
+    const auto u = hinf.ComputeControl(x);
+    EXPECT_NEAR(u.at(0, 0), 0.0f, math::Tolerance<float>());
+}
+
+TEST_F(TestHInfinityStateFeedbackSynthesized, closed_loop_state_converges_from_nonzero_ic)
+{
+    const auto& K = hinf.Gain();
+    auto clA = plant.A - plant.B2 * K;
+
+    math::Vector<float, 2> x{};
+    x.at(0, 0) = 1.0f;
+    x.at(1, 0) = 0.0f;
+
+    for (int step = 0; step < 200; ++step)
+        x = clA * x;
+
+    EXPECT_NEAR(x.at(0, 0), 0.0f, 1e-2f);
+    EXPECT_NEAR(x.at(1, 0), 0.0f, 1e-2f);
+}
+
+TEST_F(TestHInfinityStateFeedbackSynthesized, compute_control_is_homogeneous)
+{
+    math::Vector<float, 2> x{};
+    x.at(0, 0) = 0.7f;
+    x.at(1, 0) = -0.3f;
+
+    const float alpha = 3.5f;
+    math::Vector<float, 2> xScaled{};
+    xScaled.at(0, 0) = alpha * x.at(0, 0);
+    xScaled.at(1, 0) = alpha * x.at(1, 0);
+
+    const auto u = hinf.ComputeControl(x);
+    const auto uScaled = hinf.ComputeControl(xScaled);
+
+    EXPECT_NEAR(uScaled.at(0, 0), alpha * u.at(0, 0), math::Tolerance<float>());
+}
+
+TEST_F(TestHInfinityStateFeedbackSynthesized, synthesize_is_deterministic)
+{
+    const float gamma1 = hinf.Gamma();
+    const auto K1 = hinf.Gain();
+
+    HInf hinf2{ plant };
+    hinf2.Synthesize(0.5f, 100.0f, 1e-3f);
+
+    EXPECT_FLOAT_EQ(hinf2.Gamma(), gamma1);
+    for (std::size_t c = 0; c < 2; ++c)
+        EXPECT_FLOAT_EQ(hinf2.Gain().at(0, c), K1.at(0, c));
+}
+
+TEST_F(TestHInfinityStateFeedback, synthesize_fails_with_gammamax_too_small)
+{
+    const bool ok = hinf.Synthesize(0.001f, 0.01f, 1e-4f);
+    EXPECT_FALSE(ok);
+}
+
+TEST_F(TestHInfinityStateFeedbackSynthesized, riccati_solution_diagonal_is_positive)
+{
+    constexpr std::size_t AugInputSize = 2;
+    math::Matrix<float, 2, AugInputSize> B{};
+    B.at(0, 0) = plant.B2.at(0, 0);
+    B.at(1, 0) = plant.B2.at(1, 0);
+    B.at(0, 1) = plant.B1.at(0, 0);
+    B.at(1, 1) = plant.B1.at(1, 0);
+
+    const float g = hinf.Gamma();
+    math::SquareMatrix<float, AugInputSize> Rtilde{};
+    Rtilde.at(0, 0) = 1.0f;
+    Rtilde.at(1, 1) = -(g * g);
+
+    auto Q = plant.C1.Transpose() * plant.C1;
+    solvers::DiscreteAlgebraicRiccatiEquation<float, 2, AugInputSize> dare{};
+    auto Xref = dare.Solve(plant.A, B, Q, Rtilde);
+
+    EXPECT_GT(Xref.at(0, 0), 0.0f);
+    EXPECT_GT(Xref.at(1, 1), 0.0f);
+}