fix(lqr): forward MaxIterations through Lqr and IntegralStateFeedbackLqi (#279)

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 <noreply@anthropic.com>

Author: gabrielfrasantos

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<float, StateSize, InputSize, OutputSize, 30000>`.
 
 ## Variants & Generalizations
 

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<float, StateSize, InputSize, 30000>`. A hard-assert fires if the solver does not converge within the specified limit.
 
 ## Variants & Generalizations
 

numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp

@@ -11,7 +11,7 @@
 
 namespace controllers
 {
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations = 300>
     class IntegralStateFeedbackLqi
     {
         static_assert(std::is_floating_point_v<T>, "IntegralStateFeedbackLqi supports floating-point types");
@@ -53,16 +53,16 @@ namespace controllers
 
     // Implementation //
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
-    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::IntegralStateFeedbackLqi(
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::IntegralStateFeedbackLqi(
         const GainStateMatrix& kx, const GainIntegralMatrix& ki, T ts)
         : gainState{ kx }
         , gainIntegral{ ki }
         , sampleTime{ ts }
     {}
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
-    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::IntegralStateFeedbackLqi(
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::IntegralStateFeedbackLqi(
         const math::LinearTimeInvariant<T, StateSize, InputSize, OutputSize>& plant,
         const math::SquareMatrix<T, AugmentedSize>& Q,
         const math::SquareMatrix<T, InputSize>& R,
@@ -78,17 +78,17 @@ namespace controllers
             Aa.at(StateSize + r, StateSize + r) = T(1);
         Ba.SetBlock(plant.B, 0, 0);
 
-        Lqr<T, AugmentedSize, InputSize> lqr{ Aa, Ba, Q, R };
+        Lqr<T, AugmentedSize, InputSize, MaxIterations> lqr{ Aa, Ba, Q, R };
         const auto& Ka = lqr.GetGain();
 
         gainState = Ka.template GetBlock<InputSize, StateSize>(0, 0);
         gainIntegral = Ka.template GetBlock<InputSize, OutputSize>(0, StateSize);
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
     OPTIMIZE_FOR_SPEED
-        typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::InputVector
-        IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::ComputeControl(
+        typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::InputVector
+        IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::ComputeControl(
             const StateVector& x,
             const OutputVector& reference,
             const OutputVector& measured)
@@ -98,22 +98,22 @@ namespace controllers
         return (gainState * x + gainIntegral * integral) * T(-1);
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
-    void IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::Reset()
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
+    void IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::Reset()
     {
         integral = IntegralVector{};
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
-    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GainStateMatrix&
-    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GetGainState() const
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
+    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::GainStateMatrix&
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::GetGainState() const
     {
         return gainState;
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
-    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GainIntegralMatrix&
-    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GetGainIntegral() const
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize, std::size_t MaxIterations>
+    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::GainIntegralMatrix&
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize, MaxIterations>::GetGainIntegral() const
     {
         return gainIntegral;
     }

numerical/controllers/implementations/Lqr.hpp

@@ -13,7 +13,7 @@
 
 namespace controllers
 {
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations = 300>
     class Lqr
         : public StateFeedbackController<T, StateSize, InputSize>
     {
@@ -45,12 +45,12 @@ namespace controllers
         bool riccatiSolutionAvailable = false;
     };
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    Lqr<T, StateSize, InputSize>::Lqr(
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    Lqr<T, StateSize, InputSize, MaxIterations>::Lqr(
         const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R)
         : riccatiSolution([&A, &B, &Q, &R]
               {
-                  auto r = solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize>{}.Solve(A, B, Q, R);
+                  auto r = solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize, MaxIterations>{}.Solve(A, B, Q, R);
                   really_assert(r.converged);
                   return r.value;
               }())
@@ -59,43 +59,43 @@ namespace controllers
         ComputeGain(A, B, riccatiSolution, R);
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    Lqr<T, StateSize, InputSize>::Lqr(const GainMatrix& precomputedGain)
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    Lqr<T, StateSize, InputSize, MaxIterations>::Lqr(const GainMatrix& precomputedGain)
         : gain(precomputedGain)
     {}
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
     OPTIMIZE_FOR_SPEED
-        typename Lqr<T, StateSize, InputSize>::InputVector
-        Lqr<T, StateSize, InputSize>::ComputeControl(const StateVector& state)
+        typename Lqr<T, StateSize, InputSize, MaxIterations>::InputVector
+        Lqr<T, StateSize, InputSize, MaxIterations>::ComputeControl(const StateVector& state)
     {
         return gain * state * T(-1.0f);
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    const typename Lqr<T, StateSize, InputSize>::GainMatrix&
-    Lqr<T, StateSize, InputSize>::GetGain() const
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    const typename Lqr<T, StateSize, InputSize, MaxIterations>::GainMatrix&
+    Lqr<T, StateSize, InputSize, MaxIterations>::GetGain() const
     {
         return gain;
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    const typename Lqr<T, StateSize, InputSize>::StateMatrix&
-    Lqr<T, StateSize, InputSize>::GetRiccatiSolution() const
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    const typename Lqr<T, StateSize, InputSize, MaxIterations>::StateMatrix&
+    Lqr<T, StateSize, InputSize, MaxIterations>::GetRiccatiSolution() const
     {
         really_assert(riccatiSolutionAvailable);
         return riccatiSolution;
     }
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    Lqr<T, StateSize, InputSize>::Lqr(
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    Lqr<T, StateSize, InputSize, MaxIterations>::Lqr(
         const math::LinearTimeInvariant<T, StateSize, InputSize>& plant,
         const StateMatrix& Q, const InputWeightMatrix& R)
         : Lqr(plant.A, plant.B, Q, R)
     {}
 
-    template<typename T, std::size_t StateSize, std::size_t InputSize>
-    OPTIMIZE_FOR_SPEED void Lqr<T, StateSize, InputSize>::ComputeGain(
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t MaxIterations>
+    OPTIMIZE_FOR_SPEED void Lqr<T, StateSize, InputSize, MaxIterations>::ComputeGain(
         const StateMatrix& A, const InputMatrix& B, const StateMatrix& P, const InputWeightMatrix& R)
     {
         auto BtP = B.Transpose() * P;

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<float>());
 }
 
+TEST_F(TestLqr, high_frequency_plant_converges_with_raised_iteration_cap)
+{
+    math::SquareMatrix<float, 2> A{
+        { 1.0f, 0.001f },
+        { 0.0f, 1.0f }
+    };
+    math::Matrix<float, 2, 1> B{
+        { 0.0f },
+        { 0.001f }
+    };
+    math::SquareMatrix<float, 2> Q{
+        { 10.0f, 0.0f },
+        { 0.0f, 1.0f }
+    };
+    math::SquareMatrix<float, 1> R{ { 0.1f } };
+
+    controllers::Lqr<float, 2, 1, 30000> 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<float, 2, 1> lqr1{ A2, B2, Q2, R2 };