From d3890ce8025d06a5fce269c226926e860284d93e Mon Sep 17 00:00:00 2001 From: gfs Date: Sat, 8 Aug 2026 15:40:32 +0000 Subject: [PATCH] improve nonlinear control algorithms --- .../test/TestBacksteppingControl.cpp | 99 ++++++++ .../test/TestFeedbackLinearization.cpp | 60 ++++- .../TestModelReferenceAdaptiveControl.cpp | 211 ++++++++++++++++-- 3 files changed, 343 insertions(+), 27 deletions(-) diff --git a/numerical/nonlinear_control/test/TestBacksteppingControl.cpp b/numerical/nonlinear_control/test/TestBacksteppingControl.cpp index 63376a9d..261681f5 100644 --- a/numerical/nonlinear_control/test/TestBacksteppingControl.cpp +++ b/numerical/nonlinear_control/test/TestBacksteppingControl.cpp @@ -362,3 +362,102 @@ TEST_F(TestBacksteppingRealPlantOrder2, stabilizes_double_integrator) EXPECT_NEAR(x[0], 0.0f, 1.0e-2f); EXPECT_NEAR(x[1], 0.0f, 1.0e-2f); } + +TEST_F(TestBacksteppingRealPlantOrder1, reset_restores_initial_response_order1) +{ + std::array x{ { 0.8f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + const float first = controller.ComputeControl(x, ref); + + std::array xOther{ { 2.0f } }; + controller.ComputeControl(xOther, ref); + controller.Reset(); + + const float afterReset = controller.ComputeControl(x, ref); + + EXPECT_NEAR(afterReset, first, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, zero_error_output_equals_negative_drift_over_gain) +{ + const float d{ 1.2f }; + const float g{ 2.0f }; + std::array x{ { 0.5f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.5f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(d)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, -d / g, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, drift_and_error_combined_output) +{ + const float e{ 0.6f }; + const float d{ 1.0f }; + const float g{ 2.5f }; + std::array x{ { e } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(d)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(g)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_NEAR(u, (-d - gains[0] * e) / g, math::Tolerance()); +} + +TEST_F(TestBacksteppingControlOrder1, two_instances_do_not_share_state) +{ + ::testing::StrictMock> modelB; + std::array gainsB{ { 2.0f } }; + nonlinear_control::BacksteppingControl ctrlB{ modelB, gainsB }; + + std::array xA{ { 1.0f } }; + std::array xB{ { 1.0f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, xA)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, xA)).WillOnce(::testing::Return(1.0f)); + EXPECT_CALL(modelB, Drift(0, xB)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(modelB, Gain(0, xB)).WillOnce(::testing::Return(1.0f)); + + const float uA = controller.ComputeControl(xA, ref); + const float uB = ctrlB.ComputeControl(xB, ref); + + EXPECT_FLOAT_EQ(uA, uB); +} + +TEST_F(TestBacksteppingControlOrder1, large_state_produces_finite_output) +{ + const float large{ 1.0e6f }; + std::array x{ { large } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.0f, 0.0f }; + + EXPECT_CALL(model, Drift(0, x)).WillOnce(::testing::Return(0.0f)); + EXPECT_CALL(model, Gain(0, x)).WillOnce(::testing::Return(1.0f)); + + const float u = controller.ComputeControl(x, ref); + + EXPECT_TRUE(std::isfinite(u)); + EXPECT_NEAR(u, -gains[0] * large, 1.0f); +} + +TEST_F(TestBacksteppingControlOrder1, determinism_same_input_same_output) +{ + std::array x{ { 0.7f } }; + nonlinear_control::BacksteppingControl::Reference ref{ 0.2f, 0.1f }; + + EXPECT_CALL(model, Drift(0, x)).Times(2).WillRepeatedly(::testing::Return(0.3f)); + EXPECT_CALL(model, Gain(0, x)).Times(2).WillRepeatedly(::testing::Return(1.5f)); + + controller.Reset(); + const float u1 = controller.ComputeControl(x, ref); + controller.Reset(); + const float u2 = controller.ComputeControl(x, ref); + + EXPECT_FLOAT_EQ(u1, u2); +} diff --git a/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp index d0a323be..ac77b81f 100644 --- a/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp +++ b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp @@ -203,19 +203,65 @@ TEST_F(TestFeedbackLinearization, closed_loop_error_decays) EXPECT_LT(finalNorm, prevErrorNorm); } -TEST_F(TestFeedbackLinearization, reference_feedforward_used) +TEST_F(TestFeedbackLinearization, superposition_all_terms_active) { - const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::SquareMatrix B{ 2.0f, 1.0f, 0.5f, 3.0f }; + const math::Vector drift{ { 4.0f }, { -1.0f } }; + const math::Vector x{ { 1.0f }, { -1.0f } }; + const math::Vector xDot{ { 0.5f }, { 0.25f } }; + const math::Vector yd{ { 3.0f }, { 1.0f } }; + const math::Vector ydDot{ { 1.5f }, { -0.5f } }; + const math::Vector ydDdot{ { 0.2f }, { -0.3f } }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(B)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(drift)); + + const auto u{ controller.ComputeInput(x, xDot, yd, ydDot, ydDdot) }; + + const math::Vector e{ yd - x }; + const math::Vector eDot{ ydDot - xDot }; + const math::Vector v{ ydDdot + kd * eDot + kp * e }; + const math::Vector expected{ B * v + drift }; + + EXPECT_NEAR(u.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), expected.at(1, 0), math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, off_diagonal_decoupling_matrix_couples_axes) +{ + const math::SquareMatrix B{ 0.0f, 1.0f, 1.0f, 0.0f }; const math::Vector a{ { 0.0f }, { 0.0f } }; - const math::Vector c{ { 7.0f }, { -4.0f } }; + const math::Vector v{ { 5.0f }, { 7.0f } }; - EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(B)); EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); - const auto u{ controller.ComputeInput(zero, zero, zero, zero, c) }; + const auto u{ controller.ComputeInput(zero, zero, zero, zero, v) }; - EXPECT_NEAR(u.at(0, 0), c.at(0, 0), math::Tolerance()); - EXPECT_NEAR(u.at(1, 0), c.at(1, 0), math::Tolerance()); + EXPECT_NEAR(u.at(0, 0), 7.0f, math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), 5.0f, math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, model_queried_with_exact_state) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector xQuery{ { 2.5f }, { -3.1f } }; + + math::Vector capturedB{}; + math::Vector capturedA{}; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&capturedB), ::testing::Return(identity))); + EXPECT_CALL(model, DriftTerm(::testing::_)) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&capturedA), ::testing::Return(a))); + + controller.ComputeInput(xQuery, zero, zero, zero, zero); + + EXPECT_FLOAT_EQ(capturedB.at(0, 0), xQuery.at(0, 0)); + EXPECT_FLOAT_EQ(capturedB.at(1, 0), xQuery.at(1, 0)); + EXPECT_FLOAT_EQ(capturedA.at(0, 0), xQuery.at(0, 0)); + EXPECT_FLOAT_EQ(capturedA.at(1, 0), xQuery.at(1, 0)); } namespace diff --git a/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp b/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp index 40503af9..517ad554 100644 --- a/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp +++ b/numerical/nonlinear_control/test/TestModelReferenceAdaptiveControl.cpp @@ -23,6 +23,15 @@ namespace refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov }; }; + + class TestModelReferenceAdaptiveControlMitRule : public ::testing::Test + { + protected: + math::LinearTimeInvariant refModel{ MakeFirstOrderReference() }; + nonlinear_control::ModelReferenceAdaptiveControl mracMit{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::MitRule + }; + }; } TEST_F(TestModelReferenceAdaptiveControl, reference_model_advances) @@ -116,32 +125,45 @@ TEST_F(TestModelReferenceAdaptiveControl, signB_flips_adaptation_direction) EXPECT_NEAR(deltaPos, -deltaNeg, math::Tolerance()); } -TEST_F(TestModelReferenceAdaptiveControl, control_law_combines_terms) +TEST_F(TestModelReferenceAdaptiveControl, initial_control_output_is_zero_with_zero_thetas) { - float xPlant{ 0.5f }; - const float aPlant{ -1.5f }; - const float bPlant{ 2.0f }; - const float dt{ 0.01f }; + math::Vector x{}; + x.at(0, 0) = 3.5f; + math::Vector r{}; + r.at(0, 0) = 1.2f; + const float dt{ 0.05f }; + + const auto u = mrac.ComputeControl(x, r, dt); + EXPECT_NEAR(u.at(0, 0), 0.0f, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControl, control_output_matches_hand_computed_thetaX_thetaR) +{ + math::Vector x{}; + x.at(0, 0) = 1.0f; math::Vector r{}; - r.at(0, 0) = 1.0f; + r.at(0, 0) = 0.0f; + const float dt{ 0.1f }; + const float gamma{ 1.0f }; + const float signB{ 1.0f }; - for (int k = 0; k < 500; ++k) - { - math::Vector xVec{}; - xVec.at(0, 0) = xPlant; - const auto u = mrac.ComputeControl(xVec, r, dt); - xPlant += dt * (aPlant * xPlant + bPlant * u.at(0, 0)); - } + mrac.ComputeControl(x, r, dt); - math::Vector xNow{}; - xNow.at(0, 0) = xPlant; - const float txCurrent{ mrac.GetThetaX().at(0, 0) }; - const float trCurrent{ mrac.GetThetaR().at(0, 0) }; - const auto u = mrac.ComputeControl(xNow, r, dt); + const float xmStep1{ 0.0f + (refModel.A.at(0, 0) * 0.0f + refModel.B.at(0, 0) * 0.0f) * dt }; + const float e1{ 1.0f - xmStep1 }; + const float thetaXAfter{ 0.0f - gamma * signB * dt * e1 * 1.0f }; + const float thetaRAfter{ 0.0f - gamma * signB * dt * e1 * 0.0f }; - const float expectedU{ txCurrent * xPlant + trCurrent * r.at(0, 0) }; - EXPECT_NEAR(u.at(0, 0), expectedU, math::Tolerance()); + math::Vector x2{}; + x2.at(0, 0) = 2.0f; + math::Vector r2{}; + r2.at(0, 0) = 0.5f; + + const auto u2 = mrac.ComputeControl(x2, r2, dt); + + const float expectedU{ thetaXAfter * 2.0f + thetaRAfter * 0.5f }; + EXPECT_NEAR(u2.at(0, 0), expectedU, math::Tolerance()); } TEST_F(TestModelReferenceAdaptiveControl, tracks_reference_over_time) @@ -166,6 +188,23 @@ TEST_F(TestModelReferenceAdaptiveControl, tracks_reference_over_time) EXPECT_LT(std::abs(xPlant - xm), 0.1f); } +TEST_F(TestModelReferenceAdaptiveControl, reference_model_steady_state_equals_analytic_value) +{ + math::Vector x{}; + math::Vector r{}; + r.at(0, 0) = 1.0f; + const float dt{ 0.001f }; + + for (int k = 0; k < 10000; ++k) + { + x.at(0, 0) = mrac.GetReferenceState().at(0, 0); + mrac.ComputeControl(x, r, dt); + } + + const float expectedSteadyState{ refModel.B.at(0, 0) / (-refModel.A.at(0, 0)) }; + EXPECT_NEAR(mrac.GetReferenceState().at(0, 0), expectedSteadyState, 1e-2f); +} + TEST_F(TestModelReferenceAdaptiveControl, gamma_scales_adaptation_speed) { nonlinear_control::ModelReferenceAdaptiveControl mracSlow{ @@ -249,3 +288,135 @@ TEST_F(TestModelReferenceAdaptiveControl, reset_clears_state_and_params) EXPECT_NEAR(mrac.GetThetaX().at(0, 0), 0.0f, math::Tolerance()); EXPECT_NEAR(mrac.GetThetaR().at(0, 0), 0.0f, math::Tolerance()); } + +TEST_F(TestModelReferenceAdaptiveControl, reset_restores_fresh_instance_behaviour) +{ + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + const float dt{ 0.1f }; + + nonlinear_control::ModelReferenceAdaptiveControl fresh{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + mrac.ComputeControl(x, r, dt); + mrac.Reset(); + const auto uAfterReset = mrac.ComputeControl(x, r, dt); + const auto uFresh = fresh.ComputeControl(x, r, dt); + + EXPECT_FLOAT_EQ(uAfterReset.at(0, 0), uFresh.at(0, 0)); + EXPECT_FLOAT_EQ( + mrac.GetReferenceState().at(0, 0), fresh.GetReferenceState().at(0, 0)); +} + +TEST_F(TestModelReferenceAdaptiveControl, determinism_same_input_same_output) +{ + math::Vector x{}; + x.at(0, 0) = 1.5f; + math::Vector r{}; + r.at(0, 0) = 0.8f; + const float dt{ 0.05f }; + + nonlinear_control::ModelReferenceAdaptiveControl mrac1{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + nonlinear_control::ModelReferenceAdaptiveControl mrac2{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + const auto u1 = mrac1.ComputeControl(x, r, dt); + const auto u2 = mrac2.ComputeControl(x, r, dt); + + EXPECT_FLOAT_EQ(u1.at(0, 0), u2.at(0, 0)); + EXPECT_FLOAT_EQ( + mrac1.GetReferenceState().at(0, 0), mrac2.GetReferenceState().at(0, 0)); +} + +TEST_F(TestModelReferenceAdaptiveControl, zero_dt_freezes_state_and_parameters) +{ + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + + mrac.ComputeControl(x, r, 0.1f); + + const float xmBefore{ mrac.GetReferenceState().at(0, 0) }; + const float txBefore{ mrac.GetThetaX().at(0, 0) }; + const float trBefore{ mrac.GetThetaR().at(0, 0) }; + + mrac.ComputeControl(x, r, 0.0f); + + EXPECT_FLOAT_EQ(mrac.GetReferenceState().at(0, 0), xmBefore); + EXPECT_FLOAT_EQ(mrac.GetThetaX().at(0, 0), txBefore); + EXPECT_FLOAT_EQ(mrac.GetThetaR().at(0, 0), trBefore); +} + +TEST_F(TestModelReferenceAdaptiveControl, large_state_produces_finite_output) +{ + math::Vector x{}; + x.at(0, 0) = 1.0e4f; + math::Vector r{}; + r.at(0, 0) = 1.0e4f; + const float dt{ 0.01f }; + + const auto u = mrac.ComputeControl(x, r, dt); + + EXPECT_TRUE(std::isfinite(u.at(0, 0))); + EXPECT_TRUE(std::isfinite(mrac.GetReferenceState().at(0, 0))); + EXPECT_TRUE(std::isfinite(mrac.GetThetaX().at(0, 0))); + EXPECT_TRUE(std::isfinite(mrac.GetThetaR().at(0, 0))); +} + +TEST_F(TestModelReferenceAdaptiveControlMitRule, mit_rule_reference_model_advances) +{ + math::Vector x{}; + x.at(0, 0) = 0.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + const float dt{ 0.1f }; + + mracMit.ComputeControl(x, r, dt); + + const float expectedXm{ refModel.B.at(0, 0) * r.at(0, 0) * dt }; + EXPECT_NEAR(mracMit.GetReferenceState().at(0, 0), expectedXm, math::Tolerance()); +} + +TEST_F(TestModelReferenceAdaptiveControlMitRule, mit_rule_matches_lyapunov_update) +{ + nonlinear_control::ModelReferenceAdaptiveControl mracLyap{ + refModel, 1.0f, +1.0f, nonlinear_control::AdaptationLaw::Lyapunov + }; + + math::Vector x{}; + x.at(0, 0) = 1.5f; + math::Vector r{}; + r.at(0, 0) = 0.5f; + const float dt{ 0.05f }; + + const auto uMit = mracMit.ComputeControl(x, r, dt); + const auto uLyap = mracLyap.ComputeControl(x, r, dt); + + EXPECT_FLOAT_EQ(uMit.at(0, 0), uLyap.at(0, 0)); + EXPECT_FLOAT_EQ( + mracMit.GetThetaX().at(0, 0), mracLyap.GetThetaX().at(0, 0)); + EXPECT_FLOAT_EQ( + mracMit.GetThetaR().at(0, 0), mracLyap.GetThetaR().at(0, 0)); +} + +TEST_F(TestModelReferenceAdaptiveControlMitRule, mit_rule_reset_clears_state) +{ + math::Vector x{}; + x.at(0, 0) = 2.0f; + math::Vector r{}; + r.at(0, 0) = 1.0f; + + mracMit.ComputeControl(x, r, 0.1f); + mracMit.Reset(); + + EXPECT_NEAR(mracMit.GetReferenceState().at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(mracMit.GetThetaX().at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(mracMit.GetThetaR().at(0, 0), 0.0f, math::Tolerance()); +}