mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 21:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

matrix subclass not recommended way to represent matrices (#2348)

Browse Source
This commit is contained in:
Shane Smiskol
2020-10-20 05:12:23 -05:00
committed by GitHub
parent 32496626b2
commit 5bcac27d98
1 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_indi.py
+10 -10
View File
@@ -13,24 +13,24 @@ class LatControlINDI():
def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT_CTRL, 0.0],
[0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0]])
A = np.array([[1.0, DT_CTRL, 0.0],
[0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
K = np.array([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.x = np.array([[0.], [0.], [0.]])
self.enforce_rate_limit = CP.carName == "toyota"
@@ -64,7 +64,7 @@ class LatControlINDI():
def update(self, active, CS, CP, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()