diff --git a/doc/controllers/IntegralStateFeedbackLqi.md b/doc/controllers/IntegralStateFeedbackLqi.md index d594806..d277147 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 3d1b83c..8ce726f 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 0c59af8..2f1775f 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 a3226e8..6bbf8cc 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 5770888..1e68f20 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 };