github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 16:47:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

remove np.matrix from lqr_steer_control

Browse Source
This commit is contained in:
Atsushi Sakai
2018-12-09 08:23:48 +09:00
parent 3afe984243
commit 93053cad93
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
PathTracking/lqr_steer_control/lqr_steer_control.py
View File

@@ -73,8 +73,8 @@ def solve_DARE(A, B, Q, R):
eps = 0.01
for i in range(maxiter):
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.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q
if (abs(Xn - X)).max() < eps:
X = Xn
break
@@ -94,9 +94,9 @@ def dlqr(A, B, Q, R):
X = solve_DARE(A, B, Q, R)
# compute the LQR gain
K = np.matrix(la.inv(B.T * X * B + R) * (B.T * X * A))
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
eigVals, eigVecs = la.eig(A - B * K)
eigVals, eigVecs = la.eig(A - B @ K)
return K, X, eigVals
@@ -108,7 +108,7 @@ def lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e):
v = state.v
th_e = pi_2_pi(state.yaw - cyaw[ind])
A = np.matrix(np.zeros((4, 4)))
A = np.zeros((4, 4))
A[0, 0] = 1.0
A[0, 1] = dt
A[1, 2] = v
@@ -116,12 +116,12 @@ def lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e):
A[2, 3] = dt
# print(A)
B = np.matrix(np.zeros((4, 1)))
B = np.zeros((4, 1))
B[3, 0] = v / L
K, _, _ = dlqr(A, B, Q, R)
x = np.matrix(np.zeros((4, 1)))
x = np.zeros((4, 1))
x[0, 0] = e
x[1, 0] = (e - pe) / dt
@@ -129,7 +129,7 @@ def lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e):
x[3, 0] = (th_e - pth_e) / dt
ff = math.atan2(L * k, 1)
fb = pi_2_pi((-K * x)[0, 0])
fb = pi_2_pi((-K @ x)[0, 0])
delta = ff + fb