DARE reports convergence on a barely-iterated `P` when `Q`/`R` are small, silently returning wrong LQR gains

[gabrielsantosphilips]

Follow-up to #274. PR #279 forwarded MaxIterations through Lqr and IntegralStateFeedbackLqi, which resolved the reported abort — verified working. Points 2 and 3 of that issue were not addressed, and while re-verifying I found the tolerance problem is materially worse than I originally described.

I could not reopen #274 (closed by a merged PR), hence this issue.

The absolute tolerance silently returns wrong gains

HasConverged compares against math::Tolerance<T>(), a flat 1e-3:

if (math::Abs(pij - math::ToFloat(Pprev.at(i, j))) > tolerance)
    return false;

Scaling Q and R by a common factor is a no-op on the optimal gain — it scales P linearly and leaves K identical. With an absolute stopping test it is not a no-op: once P falls near 1e-3, the very first sweep satisfies the test and the solver returns converged = true on a P that has barely moved from Q.

#include "numerical/controllers/implementations/Lqr.hpp"
#include <cstdio>

int main()
{
    const math::SquareMatrix<float, 2> a{ { 0.999f, 0.0f }, { -1.0f, 1.0f } };
    const math::Matrix<float, 2, 1>    b{ { 5.0f }, { 0.0f } };

    for (float s : { 1.0f, 1e-2f, 1e-3f, 1e-4f, 1e-5f })
    {
        const math::SquareMatrix<float, 2> q{ { 1.0f * s, 0.0f }, { 0.0f, 0.1f * s } };
        const math::SquareMatrix<float, 1> r{ 1.0f * s };

        auto res = solvers::DiscreteAlgebraicRiccatiEquation<float, 2, 1, 300>{}.Solve(a, b, q, r);
        controllers::Lqr<float, 2, 1, 300> lqr{ a, b, q, r };

        std::printf("scale=%-7.0e converged=%d P00=%-12.6g K=[%.6f %.6f]\n",
            s, res.converged, res.value.at(0, 0), lqr.GetGain().at(0, 0), lqr.GetGain().at(0, 1));
    }
}

scale=1e+00  converged=1 P00=1.43212      K=[0.246392 -0.052021]
scale=1e-02  converged=1 P00=0.0123686    K=[0.224070 -0.030529]
scale=1e-03  converged=1 P00=0.00113838   K=[0.209990 -0.016972]
scale=1e-04  converged=1 P00=0.000113838  K=[0.209990 -0.016972]
scale=1e-05  converged=1 P00=1.13838e-05  K=[0.209990 -0.016972]

Same plant, same Q/R ratio, same optimal gain — five different answers, all reported as converged.

Against the trustworthy scale = 1 row, the scale <= 1e-3 result is off by 15 % on the state gain and 67 % on the integral gain. P00 at scale = 1e-3 is 1.138e-3 where a correctly scaled solution would be 1.432e-3, i.e. the iteration stopped after roughly one sweep.

This is worse than the scale-dependence I described in #274. There the failure was loud — converged = false, then an abort. Here it is silent: a plausible-looking gain, converged = true, and a closed loop that is simply not the one that was designed. Anyone working in SI units on a small-signal plant (Q entries below ~1e-3) is exposed.

Suggested fix

A mixed absolute/relative criterion removes the scale dependence:

const float scale = std::max(std::abs(pij), std::abs(prev));
if (math::Abs(pij - prev) > tolerance * (1.0f + scale))
    return false;

Worth pairing with a scale-invariance regression test: solve (Q, R) and (kQ, kR) for a few decades of k and assert the gains match.

Still open from #274 — Lqr aborts rather than reporting

riccatiSolution([&A, &B, &Q, &R] {
    auto r = solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize, MaxIterations>{}.Solve(A, B, Q, R);
    really_assert(r.converged);
    return r.value;
}())

Now that MaxIterations is a template parameter, the failure mode has moved rather than gone: previously it always aborted at 300, now it aborts whenever the caller guesses too low. There is no way to ask whether the solve succeeded — SolveResult::converged is discarded inside the member-initializer lambda, so a caller cannot retry with a larger budget or fall back to a precomputed gain.

really_assert in a constructor is a hard abort on an embedded target. A static std::optional<Lqr> TryCreate(...), or storing converged and exposing it, would let callers degrade instead of resetting the device. The precomputed-gain constructor already provides the fallback path; it just cannot be reached from a failed solve.

Verification of what #279 did fix

Confirmed working — IntegralStateFeedbackLqi<float, 1, 1, 1, 30000> now constructs the Ts-scaled design that previously aborted, producing Kx = 1.92452, Ki = -0.60592.