diff --git a/doc/nonlinear_control/FeedbackLinearization.md b/doc/nonlinear_control/FeedbackLinearization.md index be3c5a97..279d5c1c 100644 --- a/doc/nonlinear_control/FeedbackLinearization.md +++ b/doc/nonlinear_control/FeedbackLinearization.md @@ -12,17 +12,17 @@ The technique applies to plants whose output $y \in \mathbb{R}^m$ satisfies, aft $$y^{(r)} = a(x) + B(x)\, u$$ -where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), $B(x) = M(q)$ is the inertia matrix and $a(x) = C(q, \dot{q})\dot{q} + g(q)$ is the Coriolis-plus-gravity term. +where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), written as $M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = u$, the control-affine form has $B(x) = M^{-1}(q)$ and $a(x) = -M^{-1}(q)\bigl(C(q,\dot{q})\dot{q} + g(q)\bigr)$. ### Inner Control Law (Cancellation) The inner law selects $u$ so that the term $a(x)$ is cancelled and the decoupling matrix is factored out: -$$u = B(x)\, v + a(x)$$ +$$u = B^{-1}(x)\,(v - a(x))$$ Substituting into the plant equation yields -$$y^{(r)} = a(x) + B(x)\bigl(B(x)\,v + a(x)\bigr) - a(x) = v$$ +$$y^{(r)} = a(x) + B(x)\,B^{-1}(x)\,(v - a(x)) = a(x) + (v - a(x)) = v$$ leaving pure integrator chains $y^{(r)} = v$, provided $B(x)$ is nonsingular. @@ -42,16 +42,16 @@ whose eigenvalues are set by choosing $K_p, K_d$. Critical damping per channel r Expanding yields the single expression evaluated on the hot path: -$$u = B(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e\bigr) + a(x)$$ +$$u = B^{-1}(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e - a(x)\bigr)$$ -No matrix inversion appears on the hot path: the law multiplies by $B(x)$, not by $B(x)^{-1}$. +The law requires solving the linear system $B(x)\,u = v - a(x)$ on the hot path; $B(x)$ must be nonsingular. ## Complexity Analysis | Operation | Time | Space | Notes | |--------------|--------------------|--------------|----------------------------------------| | Construction | $O(m^2)$ | $O(m^2)$ | Copy two gain matrices | -| ComputeInput | $O(m^2)$ | $O(m)$ extra | Two matrix-vector products dominate | +| ComputeInput | $O(m^3)$ | $O(m)$ extra | One linear solve dominates | | Model query | $O(m^2)$–$O(nm^2)$ | $O(m^2)$ | Implementation-defined; injected model | All storage is in fixed-size stack arrays; the law itself performs no heap allocation. @@ -66,7 +66,7 @@ Consider a 2-DOF planar arm with $m = 2$, $K_p = 100 I$, $K_d = 20 I$, and at on 1. Compute error: $e = [0.4, 0.3]^\top$, $\dot{e} = [0, 0]^\top$. 2. Compute virtual input: $v = 0 + 20 \cdot 0 + 100 \cdot [0.4, 0.3]^\top = [40, 30]^\top$. -3. Inner law: $u = I \cdot [40, 30]^\top + [0.3, 0.1]^\top = [40.3, 30.1]^\top$. +3. Inner law: $u = I^{-1}([40, 30]^\top - [0.3, 0.1]^\top) = [39.7, 29.9]^\top$. The gravity-like drift $a(x)$ is added directly; the outer PD term drives position error to zero. diff --git a/doc/solvers/RungeKuttaIntegrators.md b/doc/solvers/RungeKuttaIntegrators.md index 071ec092..cf4340df 100644 --- a/doc/solvers/RungeKuttaIntegrators.md +++ b/doc/solvers/RungeKuttaIntegrators.md @@ -41,13 +41,13 @@ Dormand and Prince (1980) selected a 7-stage Butcher tableau whose 5th-order pro The **5th-order** solution used to advance the state: $$ -y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 - \frac{125}{192}k_4 + \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right) +y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 + \frac{125}{192}k_4 - \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right) $$ The **4th-order** embedded solution used only for error estimation: $$ -y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 - \frac{393}{640}k_4 + \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right) +y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 + \frac{393}{640}k_4 - \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right) $$ ### Error Norm and Step-Size Control diff --git a/numerical/controllers/implementations/DeadbeatControl.cpp b/numerical/controllers/implementations/DeadbeatControl.cpp index 03e252d0..5705655a 100644 --- a/numerical/controllers/implementations/DeadbeatControl.cpp +++ b/numerical/controllers/implementations/DeadbeatControl.cpp @@ -3,6 +3,8 @@ namespace controllers { template class DeadbeatControl; + template class DeadbeatControl; template class DeadbeatControl; + template class DeadbeatControl; template class DeadbeatControl; } diff --git a/numerical/controllers/implementations/DeadbeatControl.hpp b/numerical/controllers/implementations/DeadbeatControl.hpp index b4f7f8f7..b801d582 100644 --- a/numerical/controllers/implementations/DeadbeatControl.hpp +++ b/numerical/controllers/implementations/DeadbeatControl.hpp @@ -4,11 +4,10 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "infra/util/ReallyAssert.hpp" #include "numerical/controllers/interfaces/StateFeedbackController.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/LinearTimeInvariant.hpp" -#include "numerical/solvers/SingularValueDecomposition.hpp" +#include "numerical/solvers/GaussianElimination.hpp" #include namespace controllers @@ -125,12 +124,7 @@ namespace controllers for (std::size_t i = 0; i < Steps; ++i) AN = A * AN; - static constexpr T kSvdTol = T(1e-5f); - solvers::SingularValueDecomposition svd; - svd.Decompose(gamma); - really_assert(svd.Rank(kSvdTol) == StateSize); - - auto gammaPinv = svd.PseudoInverse(kSvdTol); + auto gammaPinv = solvers::SolveSystem(gamma * gamma.Transpose(), gamma).Transpose(); gainRef = gammaPinv.template GetBlock(0, 0); gainState = gainRef * AN; @@ -138,7 +132,9 @@ namespace controllers #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class DeadbeatControl; + extern template class DeadbeatControl; extern template class DeadbeatControl; + extern template class DeadbeatControl; extern template class DeadbeatControl; #endif } diff --git a/numerical/controllers/implementations/Lqr.hpp b/numerical/controllers/implementations/Lqr.hpp index 32835dc5..a3226e86 100644 --- a/numerical/controllers/implementations/Lqr.hpp +++ b/numerical/controllers/implementations/Lqr.hpp @@ -48,7 +48,12 @@ namespace controllers template Lqr::Lqr( const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R) - : riccatiSolution(solvers::DiscreteAlgebraicRiccatiEquation{}.Solve(A, B, Q, R)) + : riccatiSolution([&A, &B, &Q, &R] + { + auto r = solvers::DiscreteAlgebraicRiccatiEquation{}.Solve(A, B, Q, R); + really_assert(r.converged); + return r.value; + }()) , riccatiSolutionAvailable(true) { ComputeGain(A, B, riccatiSolution, R); diff --git a/numerical/controllers/implementations/test/TestDeadbeatControl.cpp b/numerical/controllers/implementations/test/TestDeadbeatControl.cpp index 58931062..f4b6fef1 100644 --- a/numerical/controllers/implementations/test/TestDeadbeatControl.cpp +++ b/numerical/controllers/implementations/test/TestDeadbeatControl.cpp @@ -5,6 +5,7 @@ #include "numerical/controllers/implementations/DeadbeatControl.hpp" #include "numerical/math/LinearTimeInvariant.hpp" #include "numerical/math/Tolerance.hpp" +#include #include namespace @@ -105,16 +106,16 @@ TEST_F(TestDeadbeatControl, state_gain_matches_analytic_reachability_formula) { controllers::DeadbeatControl ctrl{ A2, B2 }; - EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, 1e-1f); - EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, 1e-1f); + EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, math::Tolerance()); + EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, math::Tolerance()); } TEST_F(TestDeadbeatControl, reference_gain_matches_analytic_reachability_formula) { controllers::DeadbeatControl ctrl{ A2, B2 }; - EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, 1e-1f); - EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, 1e-1f); + EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, math::Tolerance()); + EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, math::Tolerance()); } TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor) @@ -129,3 +130,43 @@ TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor) EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 0), ctrlLti.GetReferenceGain().at(0, 0), math::Tolerance()); EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 1), ctrlLti.GetReferenceGain().at(0, 1), math::Tolerance()); } + +TEST_F(TestDeadbeatControl, two_step_gain_amplifies_noise_less_than_one_step) +{ + math::SquareMatrix A{ { kScalarA } }; + math::Matrix B{ { kScalarB } }; + controllers::DeadbeatControl ctrl1{ A, B }; + controllers::DeadbeatControl ctrl2{ A, B }; + + EXPECT_LT(std::abs(ctrl2.GetReferenceGain().at(0, 0)), + std::abs(ctrl1.GetReferenceGain().at(0, 0))); +} + +TEST_F(TestDeadbeatControl, scalar_two_step_closed_loop_is_stable) +{ + math::SquareMatrix A{ { kScalarA } }; + math::Matrix B{ { kScalarB } }; + controllers::DeadbeatControl ctrl{ A, B }; + + const float closedLoopEig = kScalarA - kScalarB * ctrl.GetStateGain().at(0, 0); + + EXPECT_LT(std::abs(closedLoopEig), 1.0f); +} + +TEST_F(TestDeadbeatControl, two_state_three_step_asymptotically_converges) +{ + controllers::DeadbeatControl ctrl{ A2, B2 }; + + math::Vector x{ { 0.0f }, { 0.0f } }; + math::Vector r{ { 5.0f }, { 0.0f } }; + ctrl.SetReference(r); + + for (int i = 0; i < 50; ++i) + { + const auto u = ctrl.ComputeControl(x); + x = A2 * x + B2 * u; + } + + EXPECT_NEAR(x.at(0, 0), r.at(0, 0), math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), r.at(1, 0), math::Tolerance()); +} diff --git a/numerical/math/Matrix.hpp b/numerical/math/Matrix.hpp index 87cb6893..e2ecb6ae 100644 --- a/numerical/math/Matrix.hpp +++ b/numerical/math/Matrix.hpp @@ -151,13 +151,20 @@ namespace math template OPTIMIZE_FOR_SPEED constexpr Matrix::Matrix(std::initializer_list> init) + : data{} { size_t row = 0; for (const auto& row_list : init) { +#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS + really_assert(row < Rows); +#endif size_t col = 0; for (const auto& value : row_list) { +#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS + really_assert(col < Cols); +#endif at(row, col) = value; ++col; } diff --git a/numerical/math/Statistics.hpp b/numerical/math/Statistics.hpp index 7f06cf74..afcf6161 100644 --- a/numerical/math/Statistics.hpp +++ b/numerical/math/Statistics.hpp @@ -5,17 +5,24 @@ namespace math { + template + struct Result + { + V value{}; + bool valid{ false }; + }; + template [[nodiscard]] constexpr T Mean(const Matrix& data) { static_assert(detail::is_supported_type_v, "Statistical functions only support float or QNumber types"); - T sum{}; + float sum = 0.0f; for (size_t i = 0; i < data.size; ++i) - sum += data.begin()[i]; + sum += math::ToFloat(data.begin()[i]); - return T{ math::ToFloat(sum) / static_cast(data.size) }; + return T{ sum / static_cast(data.size) }; } template @@ -33,7 +40,9 @@ namespace math sum_sq += diff * diff; } - return T{ sum_sq / static_cast(sample ? data.size - 1 : data.size) }; + const float divisor = sample ? static_cast(data.size) - 1.0f : static_cast(data.size); + really_assert(divisor > 0.0f); + return T{ sum_sq / divisor }; } template @@ -85,7 +94,7 @@ namespace math } template - [[nodiscard]] constexpr T RSquaredScore(const Vector& actual, const Vector& predicted) + [[nodiscard]] constexpr Result RSquaredScore(const Vector& actual, const Vector& predicted) { static_assert(detail::is_supported_type_v, "Statistical functions only support float or QNumber types"); @@ -104,11 +113,13 @@ namespace math residual_ss += diff_pred * diff_pred; } - return T{ 1.0f - (residual_ss / total_ss) }; + if (total_ss == 0.0f) + return { T{}, false }; + return { T{ 1.0f - (residual_ss / total_ss) }, true }; } template - [[nodiscard]] constexpr Matrix AutoCorrelation(const Vector& data, size_t maxLag) + [[nodiscard]] constexpr Result> AutoCorrelation(const Vector& data, size_t maxLag) { static_assert(detail::is_supported_type_v, "Statistical functions only support float or QNumber types"); @@ -124,6 +135,9 @@ namespace math sum_sq += diff * diff; } + if (sum_sq == 0.0f) + return { Matrix{}, false }; + Matrix result; for (size_t lag = 0; lag <= maxLag; ++lag) @@ -133,12 +147,17 @@ namespace math sum += (math::ToFloat(data.at(t, 0)) - mean_val) * (math::ToFloat(data.at(t + lag, 0)) - mean_val); if (lag == 0) - result.at(lag, 0) = T{ 0.9999f }; + { + if constexpr (std::is_floating_point_v) + result.at(lag, 0) = T{ 1 }; + else + result.at(lag, 0) = T{ 0.9999f }; + } else result.at(lag, 0) = T{ sum / ((sum_sq / static_cast(Size)) * static_cast(Size - lag)) }; } - return result; + return { result, true }; } template diff --git a/numerical/math/test/TestMatrix.cpp b/numerical/math/test/TestMatrix.cpp index 71959109..127cdfff 100644 --- a/numerical/math/test/TestMatrix.cpp +++ b/numerical/math/test/TestMatrix.cpp @@ -204,6 +204,18 @@ TYPED_TEST(MatrixTest, AdditionIsCommutative) EXPECT_TRUE(AreMatricesNear(m1 + m2, m2 + m1)); } +TYPED_TEST(MatrixTest, PartialInitializerListZeroFillsRemainder) +{ + typename TestFixture::MatrixType m{ + { this->MakeValue(0.5f) } + }; + + EXPECT_NEAR(math::ToFloat(m.at(0, 0)), 0.5f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(0, 1)), 0.0f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(1, 0)), 0.0f, math::Tolerance()); + EXPECT_NEAR(math::ToFloat(m.at(1, 1)), 0.0f, math::Tolerance()); +} + TYPED_TEST(MatrixTest, MultiplicationByZeroMatrixYieldsZero) { auto m = this->MakeMatrix(0.5f, 0.3f, 0.2f, 0.4f); diff --git a/numerical/math/test/TestStatistics.cpp b/numerical/math/test/TestStatistics.cpp index a0d2e857..2fb99a78 100644 --- a/numerical/math/test/TestStatistics.cpp +++ b/numerical/math/test/TestStatistics.cpp @@ -122,7 +122,18 @@ TYPED_TEST(StatisticsTest, RSquaredScoreOfNearPerfectPredictions) auto result = math::RSquaredScore(actual, predicted); - EXPECT_NEAR(math::ToFloat(result), 0.8f, math::Tolerance()); + EXPECT_TRUE(result.valid); + EXPECT_NEAR(math::ToFloat(result.value), 0.8f, math::Tolerance()); +} + +TYPED_TEST(StatisticsTest, RSquaredScoreConstantTargetIsInvalid) +{ + auto actual = this->MakeVector(0.5f, 0.5f, 0.5f, 0.5f); + auto predicted = this->MakeVector(0.3f, 0.6f, 0.4f, 0.7f); + + auto result = math::RSquaredScore(actual, predicted); + + EXPECT_FALSE(result.valid); } TYPED_TEST(StatisticsTest, AutoCorrelationLagZeroIsUnity) @@ -131,7 +142,9 @@ TYPED_TEST(StatisticsTest, AutoCorrelationLagZeroIsUnity) auto result = math::AutoCorrelation(data, 2); - EXPECT_NEAR(math::ToFloat(result.at(0, 0)), 0.9999f, math::Tolerance()); + EXPECT_TRUE(result.valid); + constexpr float expectedLagZero = std::is_floating_point_v ? 1.0f : 0.9999f; + EXPECT_NEAR(math::ToFloat(result.value.at(0, 0)), expectedLagZero, math::Tolerance()); } TYPED_TEST(StatisticsTest, AutoCorrelationLagOneValue) @@ -140,7 +153,8 @@ TYPED_TEST(StatisticsTest, AutoCorrelationLagOneValue) auto result = math::AutoCorrelation(data, 2); - EXPECT_NEAR(math::ToFloat(result.at(1, 0)), 0.3333f, math::Tolerance()); + EXPECT_TRUE(result.valid); + EXPECT_NEAR(math::ToFloat(result.value.at(1, 0)), 0.3333f, math::Tolerance()); } TYPED_TEST(StatisticsTest, AutoCorrelationLagTwoValue) @@ -149,7 +163,26 @@ TYPED_TEST(StatisticsTest, AutoCorrelationLagTwoValue) auto result = math::AutoCorrelation(data, 2); - EXPECT_NEAR(math::ToFloat(result.at(2, 0)), -0.6f, math::Tolerance()); + EXPECT_TRUE(result.valid); + EXPECT_NEAR(math::ToFloat(result.value.at(2, 0)), -0.6f, math::Tolerance()); +} + +TYPED_TEST(StatisticsTest, AutoCorrelationConstantDataIsInvalid) +{ + auto data = this->MakeVector(0.3f, 0.3f, 0.3f, 0.3f); + + auto result = math::AutoCorrelation(data, 2); + + EXPECT_FALSE(result.valid); +} + +TYPED_TEST(StatisticsTest, PopulationVarianceOfSingleElementIsZero) +{ + math::Vector single{ { TestFixture::MakeValue(0.5f) } }; + + auto result = math::Variance(single, false); + + EXPECT_NEAR(math::ToFloat(result), 0.0f, math::Tolerance()); } TEST_F(StatisticsFloatTest, ZScoreNormalizesSymmetricData) diff --git a/numerical/nonlinear_control/CMakeLists.txt b/numerical/nonlinear_control/CMakeLists.txt index 66e2bf7c..867b5a4c 100644 --- a/numerical/nonlinear_control/CMakeLists.txt +++ b/numerical/nonlinear_control/CMakeLists.txt @@ -7,6 +7,7 @@ target_include_directories(numerical.nonlinear_control ${NUMERICAL_VISIBILITY} target_link_libraries(numerical.nonlinear_control ${NUMERICAL_VISIBILITY} numerical.math + numerical.solver infra.util ) diff --git a/numerical/nonlinear_control/FeedbackLinearization.hpp b/numerical/nonlinear_control/FeedbackLinearization.hpp index 3fc62012..910b17a6 100644 --- a/numerical/nonlinear_control/FeedbackLinearization.hpp +++ b/numerical/nonlinear_control/FeedbackLinearization.hpp @@ -6,6 +6,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" +#include "numerical/solvers/GaussianElimination.hpp" #include #include @@ -66,7 +67,7 @@ namespace nonlinear_control const StateVector v{ ydDdot + kd * eDot + kp * e }; const auto B{ model.DecouplingMatrixAt(x) }; const StateVector a{ model.DriftTerm(x) }; - return B * v + a; + return solvers::SolveSystem(B, v - a); } #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD diff --git a/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp index ac77b81f..2f8dc265 100644 --- a/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp +++ b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp @@ -29,12 +29,17 @@ namespace [[nodiscard]] DecouplingMatrix DecouplingMatrixAt(const StateVector& q) const override { - return InertiaMatrix(q); + const auto M = InertiaMatrix(q); + const T det = M.at(0, 0) * M.at(1, 1) - M.at(0, 1) * M.at(1, 0); + return DecouplingMatrix{ + M.at(1, 1) / det, -M.at(0, 1) / det, + -M.at(1, 0) / det, M.at(0, 0) / det + }; } [[nodiscard]] StateVector DriftTerm(const StateVector& q) const override { - return GravityTerm(q); + return SolveSpd(InertiaMatrix(q), GravityTerm(q) * T{ -1 }); } void Step(const StateVector& u, T dt) @@ -120,8 +125,8 @@ TEST_F(TestFeedbackLinearization, adds_drift_compensation) const auto u{ controller.ComputeInput(zero, zero, zero, zero, zero) }; - EXPECT_NEAR(u.at(0, 0), drift.at(0, 0), math::Tolerance()); - EXPECT_NEAR(u.at(1, 0), drift.at(1, 0), math::Tolerance()); + EXPECT_NEAR(u.at(0, 0), -drift.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), -drift.at(1, 0), math::Tolerance()); } TEST_F(TestFeedbackLinearization, pd_law_drives_position_error) @@ -171,8 +176,8 @@ TEST_F(TestFeedbackLinearization, decoupling_matrix_scales_virtual_input) const auto u{ controller.ComputeInput(zero, zero, zero, zero, v) }; - EXPECT_NEAR(u.at(0, 0), 2.0f, math::Tolerance()); - EXPECT_NEAR(u.at(1, 0), 3.0f, math::Tolerance()); + EXPECT_NEAR(u.at(0, 0), 0.5f, math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), 1.0f / 3.0f, math::Tolerance()); } TEST_F(TestFeedbackLinearization, closed_loop_error_decays) @@ -221,7 +226,12 @@ TEST_F(TestFeedbackLinearization, superposition_all_terms_active) 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 }; + const math::Vector vMinusDrift{ v - drift }; + const float det = B.at(0, 0) * B.at(1, 1) - B.at(0, 1) * B.at(1, 0); + const math::Vector expected{ + { (B.at(1, 1) * vMinusDrift.at(0, 0) - B.at(0, 1) * vMinusDrift.at(1, 0)) / det }, + { (B.at(0, 0) * vMinusDrift.at(1, 0) - B.at(1, 0) * vMinusDrift.at(0, 0)) / det } + }; EXPECT_NEAR(u.at(0, 0), expected.at(0, 0), math::Tolerance()); EXPECT_NEAR(u.at(1, 0), expected.at(1, 0), math::Tolerance()); diff --git a/numerical/robust_control/HInfinityStateFeedback.hpp b/numerical/robust_control/HInfinityStateFeedback.hpp index 421b0c93..6433f506 100644 --- a/numerical/robust_control/HInfinityStateFeedback.hpp +++ b/numerical/robust_control/HInfinityStateFeedback.hpp @@ -126,7 +126,10 @@ namespace robust_control const Augmented aug = BuildAugmented(g); solvers::DiscreteAlgebraicRiccatiEquation dare{}; - auto Xcandidate = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + auto xcResult = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + if (!xcResult.converged) + return false; + auto Xcandidate = xcResult.value; for (std::size_t i = 0; i < StateSize; ++i) if (Xcandidate.at(i, i) < T{ 0 }) @@ -155,7 +158,10 @@ namespace robust_control const Augmented aug = BuildAugmented(g); solvers::DiscreteAlgebraicRiccatiEquation dare{}; - X = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + auto xResult = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + if (!xResult.converged) + return; + X = xResult.value; K = FullGain(aug, X).template GetBlock(0, 0); } diff --git a/numerical/robust_control/test/TestHInfinityStateFeedback.cpp b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp index af07731a..fdd64918 100644 --- a/numerical/robust_control/test/TestHInfinityStateFeedback.cpp +++ b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp @@ -159,8 +159,9 @@ TEST_F(TestHInfinityStateFeedback, gain_matches_gare_solution) auto Q = plant.C1.Transpose() * plant.C1; solvers::DiscreteAlgebraicRiccatiEquation dare{}; auto Xref = dare.Solve(plant.A, B, Q, Rtilde); + ASSERT_TRUE(Xref.converged); - auto BtX = B.Transpose() * Xref; + auto BtX = B.Transpose() * Xref.value; auto S = Rtilde + BtX * B; auto BtXA = BtX * plant.A; auto Kfull = solvers::SolveSystem(S, BtXA); @@ -294,7 +295,8 @@ TEST_F(TestHInfinityStateFeedbackSynthesized, riccati_solution_diagonal_is_posit auto Q = plant.C1.Transpose() * plant.C1; solvers::DiscreteAlgebraicRiccatiEquation dare{}; auto Xref = dare.Solve(plant.A, B, Q, Rtilde); + ASSERT_TRUE(Xref.converged); - EXPECT_GT(Xref.at(0, 0), 0.0f); - EXPECT_GT(Xref.at(1, 1), 0.0f); + EXPECT_GT(Xref.value.at(0, 0), 0.0f); + EXPECT_GT(Xref.value.at(1, 1), 0.0f); } diff --git a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp index f0c3f98c..a84a7cbb 100644 --- a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp +++ b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp @@ -9,10 +9,11 @@ #include "numerical/math/QNumber.hpp" #include "numerical/math/Tolerance.hpp" #include "numerical/solvers/GaussianElimination.hpp" +#include namespace solvers { - template + template class DiscreteAlgebraicRiccatiEquation { static_assert(math::detail::is_supported_type_v, @@ -27,9 +28,15 @@ namespace solvers using InputMatrix = math::Matrix; using InputWeightMatrix = math::SquareMatrix; + struct SolveResult + { + StateMatrix value{}; + bool converged{ false }; + }; + DiscreteAlgebraicRiccatiEquation() = default; - StateMatrix Solve(const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R) const; + SolveResult Solve(const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R) const; private: StateMatrix Iterate(const StateMatrix& P, const StateMatrix& A, const InputMatrix& B, @@ -53,7 +60,7 @@ namespace solvers } template - typename DiscreteAlgebraicRiccatiEquation::StateMatrix + typename DiscreteAlgebraicRiccatiEquation::SolveResult DiscreteAlgebraicRiccatiEquation::Solve( const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R) const { @@ -65,10 +72,10 @@ namespace solvers P = Iterate(P, A, B, Q, R); if (HasConverged(P, Pprev, math::Tolerance())) - break; + return { P, true }; } - return P; + return { P, false }; } template @@ -77,8 +84,13 @@ namespace solvers { for (std::size_t i = 0; i < StateSize; ++i) for (std::size_t j = 0; j < StateSize; ++j) - if (math::Abs(math::ToFloat(P.at(i, j)) - math::ToFloat(Pprev.at(i, j))) > tolerance) + { + const float pij = math::ToFloat(P.at(i, j)); + if (!std::isfinite(pij)) + return false; + if (math::Abs(pij - math::ToFloat(Pprev.at(i, j))) > tolerance) return false; + } return true; } diff --git a/numerical/solvers/DormandPrince45.hpp b/numerical/solvers/DormandPrince45.hpp index a82432e7..265bcb7f 100644 --- a/numerical/solvers/DormandPrince45.hpp +++ b/numerical/solvers/DormandPrince45.hpp @@ -162,13 +162,13 @@ namespace solvers t + h) }; StateVector y5{ - x + k1 * (T{ 35 } / T{ 384 } * h) + k3 * (T{ 500 } / T{ 1113 } * h) + k4 * (T{ 125 } / T{ 192 } * h * T{ -1 }) + k5 * (T{ 2187 } / T{ 6784 } * h) + k6 * (T{ 11 } / T{ 84 } * h) + x + k1 * (T{ 35 } / T{ 384 } * h) + k3 * (T{ 500 } / T{ 1113 } * h) + k4 * (T{ 125 } / T{ 192 } * h) + k5 * (T{ 2187 } / T{ 6784 } * h * T{ -1 }) + k6 * (T{ 11 } / T{ 84 } * h) }; StateVector k7{ deriv(y5, t + h) }; StateVector y4{ - x + k1 * (T{ 5179 } / T{ 57600 } * h) + k3 * (T{ 7571 } / T{ 16695 } * h) + k4 * (T{ 393 } / T{ 640 } * h * T{ -1 }) + k5 * (T{ 92097 } / T{ 339200 } * h) + k6 * (T{ 187 } / T{ 2100 } * h) + k7 * (T{ 1 } / T{ 40 } * h) + x + k1 * (T{ 5179 } / T{ 57600 } * h) + k3 * (T{ 7571 } / T{ 16695 } * h) + k4 * (T{ 393 } / T{ 640 } * h) + k5 * (T{ 92097 } / T{ 339200 } * h * T{ -1 }) + k6 * (T{ 187 } / T{ 2100 } * h) + k7 * (T{ 1 } / T{ 40 } * h) }; T err{ DpWeightedNorm(y5 - y4, x, cfg.absTol, cfg.relTol) }; diff --git a/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp b/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp index 1f4be057..fd37c2a2 100644 --- a/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp +++ b/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp @@ -44,10 +44,11 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, scalar_unit_system_matches_golden_r math::SquareMatrix Q{ { 1.0f } }; math::SquareMatrix R{ { 1.0f } }; - auto P = scalarSolver.Solve(A, B, Q, R); + auto result = scalarSolver.Solve(A, B, Q, R); + EXPECT_TRUE(result.converged); constexpr float expected = 1.6180339887f; - EXPECT_NEAR(P.at(0, 0), expected, 1e-3f); + EXPECT_NEAR(result.value.at(0, 0), expected, 1e-3f); } TEST_F(TestDiscreteAlgebraicRiccatiEquation, scalar_stable_system_matches_closed_form_quadratic_root) @@ -57,10 +58,11 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, scalar_stable_system_matches_closed math::SquareMatrix Q{ { 1.0f } }; math::SquareMatrix R{ { 1.0f } }; - auto P = scalarSolver.Solve(A, B, Q, R); + auto result = scalarSolver.Solve(A, B, Q, R); + EXPECT_TRUE(result.converged); constexpr float expected = 1.483901f; - EXPECT_NEAR(P.at(0, 0), expected, 1e-3f); + EXPECT_NEAR(result.value.at(0, 0), expected, 1e-3f); } TEST_F(TestDiscreteAlgebraicRiccatiEquation, zero_dynamics_converges_immediately_to_Q) @@ -70,9 +72,10 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, zero_dynamics_converges_immediately math::SquareMatrix Q{ { 2.0f } }; math::SquareMatrix R{ { 1.0f } }; - auto P = scalarSolver.Solve(A, B, Q, R); + auto result = scalarSolver.Solve(A, B, Q, R); - EXPECT_NEAR(P.at(0, 0), 2.0f, math::Tolerance()); + EXPECT_TRUE(result.converged); + EXPECT_NEAR(result.value.at(0, 0), 2.0f, math::Tolerance()); } TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_satisfies_dare_residual) @@ -88,9 +91,10 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_satisfies_dare_ auto Q = math::SquareMatrix::Identity(); math::SquareMatrix R{ { 1.0f } }; - auto P = twoByOneSolver.Solve(A, B, Q, R); + auto result = twoByOneSolver.Solve(A, B, Q, R); - float residual = DareResidual2x1(P, A, B, Q, R); + EXPECT_TRUE(result.converged); + float residual = DareResidual2x1(result.value, A, B, Q, R); EXPECT_NEAR(residual, 0.0f, 1e-2f); } @@ -107,9 +111,10 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_symmetric) auto Q = math::SquareMatrix::Identity(); math::SquareMatrix R{ { 1.0f } }; - auto P = twoByOneSolver.Solve(A, B, Q, R); + auto result = twoByOneSolver.Solve(A, B, Q, R); - EXPECT_NEAR(P.at(0, 1), P.at(1, 0), math::Tolerance()); + EXPECT_TRUE(result.converged); + EXPECT_NEAR(result.value.at(0, 1), result.value.at(1, 0), math::Tolerance()); } TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_positive_definite) @@ -125,10 +130,11 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_positive_def auto Q = math::SquareMatrix::Identity(); math::SquareMatrix R{ { 1.0f } }; - auto P = twoByOneSolver.Solve(A, B, Q, R); + auto result = twoByOneSolver.Solve(A, B, Q, R); - float det = P.at(0, 0) * P.at(1, 1) - P.at(0, 1) * P.at(1, 0); - EXPECT_GT(P.at(0, 0), 0.0f); + EXPECT_TRUE(result.converged); + float det = result.value.at(0, 0) * result.value.at(1, 1) - result.value.at(0, 1) * result.value.at(1, 0); + EXPECT_GT(result.value.at(0, 0), 0.0f); EXPECT_GT(det, 0.0f); } @@ -145,10 +151,24 @@ TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_two_full_input_solution_is_s auto Q = math::SquareMatrix::Identity(); auto R = math::SquareMatrix::Identity(); - auto P = twoByTwoSolver.Solve(A, B, Q, R); + auto result = twoByTwoSolver.Solve(A, B, Q, R); - float det = P.at(0, 0) * P.at(1, 1) - P.at(0, 1) * P.at(1, 0); - EXPECT_NEAR(P.at(0, 1), P.at(1, 0), math::Tolerance()); - EXPECT_GT(P.at(0, 0), 0.0f); + EXPECT_TRUE(result.converged); + float det = result.value.at(0, 0) * result.value.at(1, 1) - result.value.at(0, 1) * result.value.at(1, 0); + EXPECT_NEAR(result.value.at(0, 1), result.value.at(1, 0), math::Tolerance()); + EXPECT_GT(result.value.at(0, 0), 0.0f); EXPECT_GT(det, 0.0f); } + +TEST_F(TestDiscreteAlgebraicRiccatiEquation, solve_reports_not_converged_for_single_iteration_unstable_system) +{ + solvers::DiscreteAlgebraicRiccatiEquation singleIterSolver; + math::SquareMatrix A{ { 10.0f, 0.0f }, { 0.0f, 10.0f } }; + math::Matrix B{ { 1.0f }, { 1.0f } }; + auto Q = math::SquareMatrix::Identity(); + math::SquareMatrix R{ { 1.0f } }; + + auto result = singleIterSolver.Solve(A, B, Q, R); + + EXPECT_FALSE(result.converged); +} diff --git a/numerical/solvers/test/TestRungeKuttaIntegrators.cpp b/numerical/solvers/test/TestRungeKuttaIntegrators.cpp index e3b73103..d1f52087 100644 --- a/numerical/solvers/test/TestRungeKuttaIntegrators.cpp +++ b/numerical/solvers/test/TestRungeKuttaIntegrators.cpp @@ -238,3 +238,21 @@ TEST_F(TestRungeKutta, dp45_deterministic_same_input) EXPECT_FLOAT_EQ(a.hNext, b.hNext); EXPECT_EQ(a.accepted, b.accepted); } + +TEST_F(TestRungeKutta, dp45_fifth_order_more_accurate_than_fourth) +{ + State1 x{ { 1.0f } }; + float t{ 0.0f }; + float h{ 0.1f }; + for (int i{ 0 }; i < 10; ++i) + { + auto result{ dp45.Step(x, t, h) }; + if (result.accepted) + { + x = result.xNext; + t += result.hUsed; + } + h = result.hNext; + } + EXPECT_NEAR(x.at(0, 0), std::exp(-t), math::Tolerance()); +}