Fix documentation bug in dlqr
Turns out the cost function is X.T * S * X, not 1/2. Verified by
summing up the costs and it not matching.
Change-Id: Iaa5afeacc07e6809af916c50263a71ab2c12af08
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 4cb10a2..7c7c310 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -85,11 +85,11 @@
J = sum(0, inf, x.T * Q * x + u.T * R * u)
"""
- # P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
- # 0.5 * X.T * P * X -> optimal cost to infinity
+ # P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P * B) * (A.T * P.T * B).T + Q
+ # X.T * P * X -> optimal cost to infinity
P = scipy.linalg.solve_discrete_are(a=A, b=B, q=Q, r=R)
- F = numpy.linalg.inv(R + B.T * P * B) * B.T * P * A
+ F = numpy.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
if optimal_cost_function:
return F, P
else: