lat_control_steer_model: use model without memory

of the form

steering_angle = B * u + R * p

with u = ['can_cmds', 'can_cmds_squared', 'cmds_by_speed',
                 'cmds_by_vsquared', 'cmds_squared_by_vsquared', 'cmds_squared_by_v']

p = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']

selfdrive/car/toyota/tunes.py

@@ -75,7 +75,7 @@ def set_lat_tune(tune, name):
 
   elif name == LatTunes.STEER_MODEL:
     tune.init('steerModel')
-    tune.steerModel.modelparam = [-0.24920679563392328, -1.4672914589520551, 1.0, -2.0135118084867423, 0.3854821759310787, 2.7299565568145, 6.028477073868584, 35.14569314491929, 681.7140233552714, 3907.9261417429816, 0.424673690403614, 2.4532935195322603, -0.0049418799970871365, -0.02784710973471798, -0.01271876926827733, -0.07446888002803297, -8.821937960393297, -51.527840611686]
+    tune.steerModel.modelparam = [-4.31675765e+00, 1.84600886e+01, 1.13741407e+02, 6.64955530e+02, 1.18758903e+03, -5.26240219e+00, 6.43072642e-02, -1.00595898e-01, 3.55391699e+00, -2.11952439e+02]
 
   elif 'PID' in str(name):
     tune.init('pid')

selfdrive/controls/lib/latcontrol_steer_model.py

@@ -4,14 +4,13 @@ import numpy as np
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from cereal import log
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
-from common.realtime import DT_CTRL
 
-NX = 2
 
-controls_list = ['can_cmds', 'can_cmds_squared', 'cmds_by_vsquared']
+controls_list = ['can_cmds', 'can_cmds_squared', 'cmds_by_speed',
+                 'cmds_by_vsquared', 'cmds_squared_by_vsquared', 'cmds_squared_by_v']
 NU = len(controls_list)
 
-live_param_list = ['roll', 'speed_times_roll', 'roll_squared', 'roll_by_speed']
+live_param_list = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']
 N_live_param = len(live_param_list)
 
 
@@ -19,22 +18,10 @@ class LatControlSteerModel(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
 
-    self.xcurrent = np.zeros((NX, ))
-
     model_param = np.asarray(list(CP.lateralTuning.steerModel.modelparam))
-
-    self.A = model_param[0:NX*NX].reshape((NX, NX), order='F')
-    self.B = model_param[NX*NX:NX*(NX+NU)].reshape((NX, NU), order='F')
-    self.R = model_param[NX*(NX+NU):].reshape((NX, N_live_param), order='F')
-
-    # weights of (angle, dangle, torque, delta_torque)
-    self.W = np.diag([1., 0.2, .02, 10.])
-    self.torque = 0.0
-
-  def reset(self):
-    # when is this called? only in if below?
-    super().reset()
-    self.xcurrent = np.zeros((NX, ))
+    self.B = model_param[0:NU]
+    self.R = model_param[NU:NU+N_live_param]
+    assert(NU+N_live_param == len(model_param))
 
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     model_log = log.ControlsState.LateralSteerModelState.new_message()
@@ -53,48 +40,23 @@ class LatControlSteerModel(LatControl):
       self.reset()
     else:
       # NOTE: live_param_list dependent.
-      # live_param_list = ['roll', 'speed_times_roll', 'roll_squared', 'roll_by_speed'] #, 'speed_times_angle']
+      # live_param_list = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']
+      torque_prev = last_actuators.steer
       roll_deg = np.degrees(params.roll)
-      live_param = np.array([roll_deg, CS.vEgo * roll_deg, roll_deg * abs(roll_deg), roll_deg / CS.vEgo])
+      live_param = np.array([roll_deg, roll_deg * abs(roll_deg), roll_deg / CS.vEgo, roll_deg/(CS.vEgo**2)])
       Rp = self.R @ live_param
 
       # NOTE: controls_list dependent.
-      u = np.array([last_actuators.steer, last_actuators.steer*abs(last_actuators.steer), last_actuators.steer/(CS.vEgo**2)])
-      # update state estimate with forward simulation
-      dt_AxpRppBu = DT_CTRL * (self.A @ self.xcurrent + Rp + (self.B @ u))
-      self.xcurrent = self.xcurrent + dt_AxpRppBu
-
-      # NOTE: controls_list dependent.
-      du_dtorque = np.array([1, 2*abs(self.torque), 1/(CS.vEgo**2)])
+      # controls_list = ['can_cmds', 'can_cmds_squared', 'cmds_by_speed',
+                #  'cmds_by_vsquared', 'cmds_squared_by_vsquared', 'cmds_squared_by_v']
+      du_dtorque = np.array([1, 2*abs(torque_prev), 1/CS.vEgo, 1/(CS.vEgo**2), 2*abs(torque_prev)/(CS.vEgo**2), 2*abs(torque_prev)/(CS.vEgo) ])
       B_tilde = self.B @ du_dtorque
-      desired_angle_rate = math.degrees(VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0.0))
+      output_steer = float( (1/B_tilde) * (angle_steers_des_no_offset - Rp) )
 
-      # SOLVE optimization problem:
-      # min_(\delta_torque) norm(angle - desired_angle, dangle - desired_angle_rate, torque_current+\delta_torque, delta_torque)_W
-      # with torque_next = torque_current + delta_torque
-      # angle, dangle = euler forward simulation with model and (torque_next)
-      y = np.zeros((4,))
-      # actual values
-      y[:2] = self.xcurrent
-      y[2] = last_actuators.steer
-      # subtract desired values
-      y[0] -= angle_steers_des_no_offset
-      y[1] -= desired_angle_rate
-      # fix contribution dynamic system
-      y[:2] += dt_AxpRppBu
-      #
-      Phi_tilde = np.append(DT_CTRL * B_tilde, [1.0, 1.0] )
-      M_tilde = - 1/(Phi_tilde.T @ self.W @ Phi_tilde) * (Phi_tilde.T@self.W)
-      delta_torque = M_tilde @ y
-      torque_np = last_actuators.steer + delta_torque
-
-      output_steer = float(torque_np)
-      self.torque = output_steer
       model_log.active = True
 
-    model_log.steeringAngleDeg = float(self.xcurrent[0])
+    # model_log.steeringAngleDeg = float(self.xcurrent[0])
     model_log.output = output_steer
     model_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
 
-    # print(f"LatControlSteerModel: x {self.xcurrent}")
     return output_steer, angle_steers_des, model_log