Skip to content

Commit

Permalink
Improve VM dynamic sol accuracy (#391)
Browse files Browse the repository at this point in the history
* use solve and eye func

* remove uneeded import from vehicle model
  • Loading branch information
dek3rr authored and pd0wm committed Oct 30, 2018
1 parent 2f8034b commit 00429e6
Showing 1 changed file with 3 additions and 3 deletions.
6 changes: 3 additions & 3 deletions selfdrive/controls/lib/vehicle_model.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#!/usr/bin/env python
import numpy as np
from numpy.linalg import inv
from numpy.linalg import solve

# dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
# Xdot = A*X + B*U
Expand Down Expand Up @@ -33,7 +33,7 @@ def kin_ss_sol(sa, u, VM):
def dyn_ss_sol(sa, u, VM):
# Dynamic solution, useful when speed > 0
A, B = create_dyn_state_matrices(u, VM)
return - np.matmul(inv(A), B) * sa
return - solve(A, B) * sa


def calc_slip_factor(VM):
Expand Down Expand Up @@ -87,7 +87,7 @@ def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
return np.matmul((A * self.dt + np.eye(2)), self.state) + B * sa * self.dt

def yaw_rate(self, sa, u):
return self.calc_curvature(sa, u) * u
Expand Down

0 comments on commit 00429e6

Please sign in to comment.