[huxiaoyi-ovo]

Summary

The LQRPlanner.solve_dare() iteration implements the discrete algebraic Riccati equation using NumPy * (elementwise multiplication) where the equation requires matrix products.

For the planner's default system matrices with Q=I and R=I:

• Current implementation returns an incorrect diagonal Riccati matrix (reference error: approximately 1.03 in Frobenius norm).
• Correct matrix products bring the error against SciPy's solve_discrete_are down to approximately 1.65e-7 at the planner's default iteration tolerance.
• The error also affects the computed LQR feedback gain.

Changes

• Compute the Riccati iteration with @ matrix products rather than *.
• Use scipy.linalg.solve instead of an explicit inverse in the Riccati correction term.
• Keep the existing iteration limit, tolerance, public API and system model unchanged.

Regression tests

Two new tests compare against SciPy's standard solve_discrete_are reference:

1. The actual model used by the LQR path planner, including its feedback gain.
2. A nontrivial two-input system with separate state and control penalties.

Only two files changed, with no new dependencies.

Independent cross-platform CI validation has been started; verification results will follow.

AI disclosure: ChatGPT assisted with the analysis, fix, and regression tests.