From 919dc007faab91a7e9be065d3ab59df7068f4883 Mon Sep 17 00:00:00 2001 From: dekerr Date: Tue, 9 Oct 2018 00:27:28 -0400 Subject: [PATCH 1/2] use solve and eye func --- selfdrive/controls/lib/vehicle_model.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index a42b8112a78d0b..3c85ff77c48498 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import numpy as np -from numpy.linalg import inv +from numpy.linalg import inv, solve # dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## # Xdot = A*X + B*U @@ -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): @@ -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 From de252d50435068e2d087a9fed97a1edebdb88d8c Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 30 Oct 2018 22:08:40 +0100 Subject: [PATCH 2/2] remove uneeded import from vehicle model --- selfdrive/controls/lib/vehicle_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 3c85ff77c48498..7aac7d0a990236 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import numpy as np -from numpy.linalg import inv, solve +from numpy.linalg import solve # dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## # Xdot = A*X + B*U