From 86995fa0b8052a83cc07890d32024e049906f413 Mon Sep 17 00:00:00 2001 From: gfs Date: Fri, 14 Aug 2026 14:24:36 +0000 Subject: [PATCH] fix(lqr): forward MaxIterations through Lqr and IntegralStateFeedbackLqi Systems sampled at high frequencies produce discrete poles near the unit circle that require far more than the default 300 DARE iterations to converge. Without a way to raise the cap, Lqr hard-asserted on the converged=false result regardless of whether the caller could tolerate a slower solve. Add MaxIterations = 300 as a fourth template parameter to Lqr and a fifth to IntegralStateFeedbackLqi, forwarding it to DiscreteAlgebraicRiccatiEquation. All existing call sites are unaffected by the defaulted parameter. Closes #274. Co-Authored-By: Claude Sonnet 4.6 --- doc/controllers/IntegralStateFeedbackLqi.md | 1 + doc/controllers/Lqr.md | 1 + .../IntegralStateFeedbackLqi.hpp | 34 ++++++++--------- numerical/controllers/implementations/Lqr.hpp | 38 +++++++++---------- .../implementations/test/TestLqr.cpp | 24 ++++++++++++ 5 files changed, 62 insertions(+), 36 deletions(-) diff --git a/doc/controllers/IntegralStateFeedbackLqi.md b/doc/controllers/IntegralStateFeedbackLqi.md index d594806a..d2771473 100644 --- a/doc/controllers/IntegralStateFeedbackLqi.md +++ b/doc/controllers/IntegralStateFeedbackLqi.md @@ -82,6 +82,7 @@ Consider a scalar plant ($n=1$, $m=1$, $p=1$, $T_s = 0.01$): - **Sample-time mismatch**: the discrete integral $x_i$ accumulates $T_s$-scaled errors. Using the wrong $T_s$ at run time shifts the effective integral gain and breaks zero-error convergence. +- **Slow DARE convergence**: the augmented plant $(A_a, B_a)$ has eigenvalues at exactly 1, and at high sampling rates those eigenvalues approach 1 from many directions, requiring many more DARE iterations than the default 300. Raise the cap via the `MaxIterations` template parameter: `IntegralStateFeedbackLqi`. ## Variants & Generalizations diff --git a/doc/controllers/Lqr.md b/doc/controllers/Lqr.md index 3d1b83c1..8ce726f7 100644 --- a/doc/controllers/Lqr.md +++ b/doc/controllers/Lqr.md @@ -87,6 +87,7 @@ $$Q = \begin{bmatrix} 100 & 0 \\ 0 & 1 \end{bmatrix}, \quad R = [1]$$ - **Q/R scaling.** Only the *ratio* of $Q$ to $R$ matters. Scaling both by the same factor does not change $K$. - **Full state required.** LQR assumes all states are measured. If only partial measurements are available, combine LQR with a [Kalman Filter](../filters/active/KalmanFilter.md) to form an LQG controller. - **Fixed-point limitations.** The DARE involves matrix inversions and multiplications that can overflow Q15/Q31 ranges. Prefer floating-point for the offline solve; use the pre-computed $K$ constructor for fixed-point runtime. +- **Slow DARE convergence.** Systems with eigenvalues near the unit circle (e.g., high-frequency sampling, integrating plants) may require far more than the default 300 iterations to converge. Use the `MaxIterations` template parameter to raise the cap: `Lqr`. A hard-assert fires if the solver does not converge within the specified limit. ## Variants & Generalizations diff --git a/numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp b/numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp index 0c59af80..2f1775f5 100644 --- a/numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp +++ b/numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp @@ -11,7 +11,7 @@ namespace controllers { - template + template class IntegralStateFeedbackLqi { static_assert(std::is_floating_point_v, "IntegralStateFeedbackLqi supports floating-point types"); @@ -53,16 +53,16 @@ namespace controllers // Implementation // - template - IntegralStateFeedbackLqi::IntegralStateFeedbackLqi( + template + IntegralStateFeedbackLqi::IntegralStateFeedbackLqi( const GainStateMatrix& kx, const GainIntegralMatrix& ki, T ts) : gainState{ kx } , gainIntegral{ ki } , sampleTime{ ts } {} - template - IntegralStateFeedbackLqi::IntegralStateFeedbackLqi( + template + IntegralStateFeedbackLqi::IntegralStateFeedbackLqi( const math::LinearTimeInvariant& plant, const math::SquareMatrix& Q, const math::SquareMatrix& R, @@ -78,17 +78,17 @@ namespace controllers Aa.at(StateSize + r, StateSize + r) = T(1); Ba.SetBlock(plant.B, 0, 0); - Lqr lqr{ Aa, Ba, Q, R }; + Lqr lqr{ Aa, Ba, Q, R }; const auto& Ka = lqr.GetGain(); gainState = Ka.template GetBlock(0, 0); gainIntegral = Ka.template GetBlock(0, StateSize); } - template + template OPTIMIZE_FOR_SPEED - typename IntegralStateFeedbackLqi::InputVector - IntegralStateFeedbackLqi::ComputeControl( + typename IntegralStateFeedbackLqi::InputVector + IntegralStateFeedbackLqi::ComputeControl( const StateVector& x, const OutputVector& reference, const OutputVector& measured) @@ -98,22 +98,22 @@ namespace controllers return (gainState * x + gainIntegral * integral) * T(-1); } - template - void IntegralStateFeedbackLqi::Reset() + template + void IntegralStateFeedbackLqi::Reset() { integral = IntegralVector{}; } - template - const typename IntegralStateFeedbackLqi::GainStateMatrix& - IntegralStateFeedbackLqi::GetGainState() const + template + const typename IntegralStateFeedbackLqi::GainStateMatrix& + IntegralStateFeedbackLqi::GetGainState() const { return gainState; } - template - const typename IntegralStateFeedbackLqi::GainIntegralMatrix& - IntegralStateFeedbackLqi::GetGainIntegral() const + template + const typename IntegralStateFeedbackLqi::GainIntegralMatrix& + IntegralStateFeedbackLqi::GetGainIntegral() const { return gainIntegral; } diff --git a/numerical/controllers/implementations/Lqr.hpp b/numerical/controllers/implementations/Lqr.hpp index a3226e86..6bbf8cc5 100644 --- a/numerical/controllers/implementations/Lqr.hpp +++ b/numerical/controllers/implementations/Lqr.hpp @@ -13,7 +13,7 @@ namespace controllers { - template + template class Lqr : public StateFeedbackController { @@ -45,12 +45,12 @@ namespace controllers bool riccatiSolutionAvailable = false; }; - template - Lqr::Lqr( + template + Lqr::Lqr( const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R) : riccatiSolution([&A, &B, &Q, &R] { - auto r = solvers::DiscreteAlgebraicRiccatiEquation{}.Solve(A, B, Q, R); + auto r = solvers::DiscreteAlgebraicRiccatiEquation{}.Solve(A, B, Q, R); really_assert(r.converged); return r.value; }()) @@ -59,43 +59,43 @@ namespace controllers ComputeGain(A, B, riccatiSolution, R); } - template - Lqr::Lqr(const GainMatrix& precomputedGain) + template + Lqr::Lqr(const GainMatrix& precomputedGain) : gain(precomputedGain) {} - template + template OPTIMIZE_FOR_SPEED - typename Lqr::InputVector - Lqr::ComputeControl(const StateVector& state) + typename Lqr::InputVector + Lqr::ComputeControl(const StateVector& state) { return gain * state * T(-1.0f); } - template - const typename Lqr::GainMatrix& - Lqr::GetGain() const + template + const typename Lqr::GainMatrix& + Lqr::GetGain() const { return gain; } - template - const typename Lqr::StateMatrix& - Lqr::GetRiccatiSolution() const + template + const typename Lqr::StateMatrix& + Lqr::GetRiccatiSolution() const { really_assert(riccatiSolutionAvailable); return riccatiSolution; } - template - Lqr::Lqr( + template + Lqr::Lqr( const math::LinearTimeInvariant& plant, const StateMatrix& Q, const InputWeightMatrix& R) : Lqr(plant.A, plant.B, Q, R) {} - template - OPTIMIZE_FOR_SPEED void Lqr::ComputeGain( + template + OPTIMIZE_FOR_SPEED void Lqr::ComputeGain( const StateMatrix& A, const InputMatrix& B, const StateMatrix& P, const InputWeightMatrix& R) { auto BtP = B.Transpose() * P; diff --git a/numerical/controllers/implementations/test/TestLqr.cpp b/numerical/controllers/implementations/test/TestLqr.cpp index 57708883..1e68f202 100644 --- a/numerical/controllers/implementations/test/TestLqr.cpp +++ b/numerical/controllers/implementations/test/TestLqr.cpp @@ -145,6 +145,30 @@ TEST_F(TestLqr, lti_constructor_produces_identical_gain_to_matrix_constructor) EXPECT_NEAR(lqrMat.GetGain().at(0, 1), lqrLti.GetGain().at(0, 1), math::Tolerance()); } +TEST_F(TestLqr, high_frequency_plant_converges_with_raised_iteration_cap) +{ + math::SquareMatrix A{ + { 1.0f, 0.001f }, + { 0.0f, 1.0f } + }; + math::Matrix B{ + { 0.0f }, + { 0.001f } + }; + math::SquareMatrix Q{ + { 10.0f, 0.0f }, + { 0.0f, 1.0f } + }; + math::SquareMatrix R{ { 0.1f } }; + + controllers::Lqr lqr{ A, B, Q, R }; + + EXPECT_NEAR(lqr.GetGain().at(0, 0), 9.972051f, 0.1f); + EXPECT_NEAR(lqr.GetGain().at(0, 1), 5.472012f, 0.1f); + EXPECT_NEAR(lqr.GetRiccatiSolution().at(0, 0), 5487.013672f, 5.0f); + EXPECT_NEAR(lqr.GetRiccatiSolution().at(1, 1), 549.203735f, 1.0f); +} + TEST_F(TestLqr, two_instances_with_same_matrices_produce_identical_gains) { controllers::Lqr lqr1{ A2, B2, Q2, R2 };