From 3b2150a88496208af371849b30515e2527f33460 Mon Sep 17 00:00:00 2001
From: huxiaoyi-ovo <huxiaoyi_0106@163.com>
Date: Thu, 29 Oct 2026 02:27:36 -0400
Subject: [PATCH 1/3] fix(lqr): use matrix products in discrete Riccati
 iteration

---
 PathPlanning/LQRPlanner/lqr_planner.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/PathPlanning/LQRPlanner/lqr_planner.py b/PathPlanning/LQRPlanner/lqr_planner.py
index 0f58f93ea3..abf5c49e2f 100644
--- a/PathPlanning/LQRPlanner/lqr_planner.py
+++ b/PathPlanning/LQRPlanner/lqr_planner.py
@@ -76,8 +76,9 @@ def solve_dare(self, A, B, Q, R):
         X, Xn = Q, Q
 
         for i in range(self.MAX_ITER):
-            Xn = A.T * X * A - A.T * X * B * \
-                 la.inv(R + B.T * X * B) * B.T * X * A + Q
+            Xn = (A.T @ X @ A
+                  - A.T @ X @ B @ la.solve(
+                      R + B.T @ X @ B, B.T @ X @ A) + Q)
             if (abs(Xn - X)).max() < self.EPS:
                 break
             X = Xn

From 5995bb87569b2f12451cd7fcb7205c9c9e361e6f Mon Sep 17 00:00:00 2001
From: huxiaoyi-ovo <huxiaoyi_0106@163.com>
Date: Thu, 29 Oct 2026 02:27:59 -0400
Subject: [PATCH 2/3] test(lqr): compare Riccati solution and feedback gain
 with SciPy

---
 tests/test_LQR_planner.py | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py
index be12195eac..bbd19db442 100644
--- a/tests/test_LQR_planner.py
+++ b/tests/test_LQR_planner.py
@@ -1,4 +1,6 @@
 import conftest  # Add root path to sys.path
+import numpy as np
+from scipy.linalg import solve_discrete_are
 from PathPlanning.LQRPlanner import lqr_planner as m
 
 
@@ -7,5 +9,36 @@ def test_1():
     m.main()
 
 
+
+def test_dare_uses_matrix_products():
+    planner = m.LQRPlanner()
+    planner.EPS = 1e-10
+    A, B = planner.get_system_model()
+    Q, R = np.eye(2), np.eye(1)
+
+    expected = solve_discrete_are(A, B, Q, R)
+    np.testing.assert_allclose(
+        planner.solve_dare(A, B, Q, R), expected, rtol=1e-8, atol=1e-8
+    )
+
+    gain, _, _ = planner.dlqr(A, B, Q, R)
+    expected_gain = np.linalg.solve(R + B.T @ expected @ B, B.T @ expected @ A)
+    np.testing.assert_allclose(gain, expected_gain, rtol=1e-8, atol=1e-8)
+
+
+def test_dare_with_multiple_control_inputs():
+    planner = m.LQRPlanner()
+    planner.EPS = 1e-10
+    A = np.array([[0.9, 0.2], [-0.1, 0.8]])
+    B = np.eye(2)
+    Q = np.diag([2.0, 1.0])
+    R = np.diag([0.3, 0.7])
+
+    expected = solve_discrete_are(A, B, Q, R)
+    np.testing.assert_allclose(
+        planner.solve_dare(A, B, Q, R), expected, rtol=1e-8, atol=1e-8
+    )
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)

From 9800828c9955fa3a148f8885826f8d7535d5ddef Mon Sep 17 00:00:00 2001
From: huxiaoyi-ovo <huxiaoyi_0106@163.com>
Date: Thu, 29 Oct 2026 02:28:27 -0400
Subject: [PATCH 3/3] style(tests): normalize LQR test spacing

---
 tests/test_LQR_planner.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py
index bbd19db442..e921873087 100644
--- a/tests/test_LQR_planner.py
+++ b/tests/test_LQR_planner.py
@@ -9,7 +9,6 @@ def test_1():
     m.main()
 
 
-
 def test_dare_uses_matrix_products():
     planner = m.LQRPlanner()
     planner.EPS = 1e-10