Road Roll Compensation Rebased (#23251)

* first commit

* update refs
old-commit-hash: cf466222f6

selfdrive/car/ford/carcontroller.py

@@ -32,7 +32,7 @@ class CarController():
 
     if (frame % 3) == 0:
 
-      curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
+      curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
 
       # The use of the toggle below is handy for trying out the various LKAS modes
       if TOGGLE_DEBUG:

selfdrive/car/honda/interface.py

@@ -350,7 +350,6 @@ class CarInterface(CarInterfaceBase):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
-    ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
 
     buttonEvents = []
 

selfdrive/controls/controlsd.py

@@ -631,8 +631,9 @@ class Controls:
 
     # Curvature & Steering angle
     params = self.sm['liveParameters']
-    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
+
-    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
+    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)
 
     # controlsState
     dat = messaging.new_message('controlsState')

selfdrive/controls/lib/latcontrol_angle.py

@@ -18,7 +18,7 @@ class LatControlAngle():
       angle_steers_des = float(CS.steeringAngleDeg)
     else:
       angle_log.active = True
-      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
       angle_steers_des += params.angleOffsetDeg
 
     angle_log.saturated = False

selfdrive/controls/lib/latcontrol_indi.py

@@ -93,11 +93,11 @@ class LatControlINDI():
     indi_log.steeringRateDeg = math.degrees(self.x[1])
     indi_log.steeringAccelDeg = math.degrees(self.x[2])
 
-    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
+    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
     steers_des += math.radians(params.angleOffsetDeg)
     indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
 
-    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
+    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
     indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
 
     if CS.vEgo < 0.3 or not active:

selfdrive/controls/lib/latcontrol_lqr.py

@@ -53,7 +53,7 @@ class LatControlLQR():
     # Subtract offset. Zero angle should correspond to zero torque
     steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
 
-    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
 
     instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
     desired_angle += instant_offset  # Only add offset that originates from vehicle model errors

selfdrive/controls/lib/latcontrol_pid.py

@@ -21,7 +21,7 @@ class LatControlPID():
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
-    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des

selfdrive/controls/lib/tests/test_vehicle_model.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+import math
+import unittest
+
+import numpy as np
+from control import StateSpace
+
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.car.honda.values import CAR
+from selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices
+
+
+class TestVehicleModel(unittest.TestCase):
+  def setUp(self):
+    CP = CarInterface.get_params(CAR.CIVIC)
+    self.VM = VehicleModel(CP)
+
+  def test_round_trip_yaw_rate(self):
+    # TODO: fix VM to work at zero speed
+    for u in np.linspace(1, 30, num=10):
+      for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+          yr = self.VM.yaw_rate(sa, u, roll)
+          new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll)
+
+          self.assertAlmostEqual(sa, new_sa)
+
+  def test_dyn_ss_sol_against_yaw_rate(self):
+    """Verify that the yaw_rate helper function matches the results
+    from the state space model."""
+
+    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+      for u in np.linspace(1, 30, num=10):
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+
+          # Compute yaw rate based on state space model
+          _, yr1 = dyn_ss_sol(sa, u, roll, self.VM)
+
+          # Compute yaw rate using direct computations
+          yr2 = self.VM.yaw_rate(sa, u, roll)
+          self.assertAlmostEqual(float(yr1), yr2)
+
+  def test_syn_ss_sol_simulate(self):
+    """Verifies that dyn_ss_sol mathes a simulation"""
+
+    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+      for u in np.linspace(1, 30, num=10):
+        A, B = create_dyn_state_matrices(u, self.VM)
+
+        # Convert to discrete time system
+        ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2)))
+        ss = ss.sample(0.01)
+
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+          inp = np.array([[sa], [roll]])
+
+          # Simulate for 1 second
+          x1 = np.zeros((2, 1))
+          for _ in range(100):
+            x1 = ss.A @ x1 + ss.B @ inp
+
+          # Compute steady state solution directly
+          x2 = dyn_ss_sol(sa, u, roll, self.VM)
+
+          np.testing.assert_almost_equal(x1, x2, decimal=3)
+
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/lib/vehicle_model.py

@@ -5,7 +5,7 @@ Dynamic bicycle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani
 The state is x = [v, r]^T
 with v lateral speed [m/s], and r rotational speed [rad/s]
 
-The input u is the steering angle [rad]
+The input u is the steering angle [rad], and roll [rad]
 
 The system is defined by
 x_dot = A*x + B*u
@@ -19,6 +19,9 @@ from numpy.linalg import solve
 
 from cereal import car
 
+ACCELERATION_DUE_TO_GRAVITY = 9.8
+
+
 class VehicleModel:
   def __init__(self, CP: car.CarParams):
     """
@@ -43,7 +46,7 @@ class VehicleModel:
     self.cR = stiffness_factor * self.cR_orig
     self.sR = steer_ratio
 
-  def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
+  def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray:
     """Returns the steady state solution.
 
     If the speed is too low we can't use the dynamic model (tire slip is undefined),
@@ -52,26 +55,28 @@ class VehicleModel:
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       2x1 matrix with steady state solution (lateral speed, rotational speed)
     """
     if u > 0.1:
-      return dyn_ss_sol(sa, u, self)
+      return dyn_ss_sol(sa, u, roll, self)
     else:
       return kin_ss_sol(sa, u, self)
 
-  def calc_curvature(self, sa: float, u: float) -> float:
+  def calc_curvature(self, sa: float, u: float, roll: float) -> float:
     """Returns the curvature. Multiplied by the speed this will give the yaw rate.
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Curvature factor [1/m]
     """
-    return self.curvature_factor(u) * sa / self.sR
+    return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u)
 
   def curvature_factor(self, u: float) -> float:
     """Returns the curvature factor.
@@ -86,43 +91,63 @@ class VehicleModel:
     sf = calc_slip_factor(self)
     return (1. - self.chi) / (1. - sf * u**2) / self.l
 
-  def get_steer_from_curvature(self, curv: float, u: float) -> float:
+  def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float:
     """Calculates the required steering wheel angle for a given curvature
 
     Args:
       curv: Desired curvature [1/m]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
 
-    return curv * self.sR * 1.0 / self.curvature_factor(u)
+    return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u)
+
+  def roll_compensation(self, roll: float, u: float) -> float:
+    """Calculates the roll-compensation to curvature
+
+    Args:
+      roll: Road Roll [rad]
+      u: Speed [m/s]
 
-  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float) -> float:
+    Returns:
+      Roll compensation curvature [rad]
+    """
+    sf = calc_slip_factor(self)
+
+    if abs(sf) < 1e-6:
+      return 0
+    else:
+      return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2)
+
+  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float:
     """Calculates the required steering wheel angle for a given yaw_rate
 
     Args:
       yaw_rate: Desired yaw rate [rad/s]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
     curv = yaw_rate / u
-    return self.get_steer_from_curvature(curv, u)
+    return self.get_steer_from_curvature(curv, u, roll)
 
-  def yaw_rate(self, sa: float, u: float) -> float:
+  def yaw_rate(self, sa: float, u: float, roll: float) -> float:
     """Calculate yaw rate
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Yaw rate [rad/s]
     """
-    return self.calc_curvature(sa, u) * u
+    return self.calc_curvature(sa, u, roll) * u
 
 
 def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
@@ -152,7 +177,7 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     VM: Vehicle model
 
   Returns:
-    A tuple with the 2x2 A matrix, and 2x1 B matrix
+    A tuple with the 2x2 A matrix, and 2x2 B matrix
 
   Parameters in the vehicle model:
     cF: Tire stiffness Front [N/rad]
@@ -165,30 +190,38 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     chi: Steer ratio rear [-]
   """
   A = np.zeros((2, 2))
-  B = np.zeros((2, 1))
+  B = np.zeros((2, 2))
   A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
   A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
   A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
   A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
+
+  # Steering input
   B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
   B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
+
+  # Roll input
+  B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY
+
   return A, B
 
 
-def dyn_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
+def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray:
   """Calculate the steady state solution when x_dot = 0,
   Ax + Bu = 0 => x = -A^{-1} B u
 
   Args:
     sa: Steering angle [rad]
     u: Speed [m/s]
+    roll: Road Roll [rad]
     VM: Vehicle model
 
   Returns:
     2x1 matrix with steady state solution
   """
   A, B = create_dyn_state_matrices(u, VM)
-  return -solve(A, B) * sa
+  inp = np.array([[sa], [roll]])
+  return -solve(A, B) @ inp
 
 
 def calc_slip_factor(VM):

selfdrive/locationd/models/car_kf.py

@@ -5,6 +5,7 @@ from typing import Any, Dict
 
 import numpy as np
 
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from selfdrive.locationd.models.constants import ObservationKind
 from selfdrive.swaglog import cloudlog
 
@@ -37,6 +38,7 @@ class States():
   VELOCITY = _slice(2)  # (x, y) [m/s]
   YAW_RATE = _slice(1)  # [rad/s]
   STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
 
 
 class CarKalman(KalmanFilter):
@@ -51,6 +53,7 @@ class CarKalman(KalmanFilter):
     10.0, 0.0,
     0.0,
     0.0,
+    0.0
   ])
 
   # process noise
@@ -63,12 +66,14 @@ class CarKalman(KalmanFilter):
     .1**2, .01**2,
     math.radians(0.1)**2,
     math.radians(0.1)**2,
+    math.radians(1)**2,
   ])
   P_initial = Q.copy()
 
   obs_noise: Dict[int, Any] = {
     ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
     ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
     ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
     ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
     ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
@@ -106,6 +111,7 @@ class CarKalman(KalmanFilter):
     cF, cR = x * cF_orig, x * cR_orig
     angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
     angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
     sa = state[States.STEER_ANGLE, :][0, 0]
 
     sR = state[States.STEER_RATIO, :][0, 0]
@@ -122,8 +128,12 @@ class CarKalman(KalmanFilter):
     B[0, 0] = cF / m / sR
     B[1, 0] = (cF * aF) / j / sR
 
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
     x = sp.Matrix([v, r])  # lateral velocity, yaw rate
-    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
 
     dt = sp.Symbol('dt')
     state_dot = sp.Matrix(np.zeros((dim_state, 1)))
@@ -145,11 +155,12 @@ class CarKalman(KalmanFilter):
       [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
       [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
       [sp.Matrix([x]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
     ]
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
 
-  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0):  # pylint: disable=super-init-not-called
+  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):  # pylint: disable=super-init-not-called
     dim_state = self.initial_x.shape[0]
     dim_state_err = self.P_initial.shape[0]
     x_init = self.initial_x
@@ -157,6 +168,8 @@ class CarKalman(KalmanFilter):
     x_init[States.STIFFNESS] = stiffness_factor
     x_init[States.ANGLE_OFFSET] = angle_offset
 
+    if P_initial is not None:
+      self.P_initial = P_initial
     # init filter
     self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
 

selfdrive/locationd/models/constants.py

@@ -38,6 +38,7 @@ class ObservationKind:
   STIFFNESS = 28  # [-]
   STEER_RATIO = 29  # [-]
   ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
 
   names = [
     'Unknown',
@@ -69,6 +70,8 @@ class ObservationKind:
     'Fast Angle Offset',
     'Stiffness',
     'Steer Ratio',
+    'Road Frame x speed',
+    'Road Roll',
   ]
 
   @classmethod

selfdrive/locationd/paramsd.py

@@ -16,10 +16,12 @@ from selfdrive.swaglog import cloudlog
 
 
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
+ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
 
 class ParamsLearner:
-  def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
+  def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
-    self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
+    self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
 
     self.kf.filter.set_global("mass", CP.mass)
     self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
@@ -30,9 +32,10 @@ class ParamsLearner:
 
     self.active = False
 
-    self.speed = 0
+    self.speed = 0.0
+    self.roll = 0.0
     self.steering_pressed = False
-    self.steering_angle = 0
+    self.steering_angle = 0.0
 
     self.valid = True
 
@@ -41,16 +44,34 @@ class ParamsLearner:
       yaw_rate = msg.angularVelocityCalibrated.value[2]
       yaw_rate_std = msg.angularVelocityCalibrated.std[2]
 
+      localizer_roll = msg.orientationNED.value[0]
+      roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
+      if roll_valid:
+        roll = localizer_roll
+        roll_std = np.radians(1.0)
+      else:
+        # This is done to bound the road roll estimate when localizer values are invalid
+        roll = 0.0
+        roll_std = np.radians(10.0)
+      self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+
       yaw_rate_valid = msg.angularVelocityCalibrated.valid
       yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
       yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
 
       if self.active:
-        if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
+        if msg.inputsOK and msg.posenetOK:
+
+          if yaw_rate_valid:
             self.kf.predict_and_observe(t,
                                         ObservationKind.ROAD_FRAME_YAW_RATE,
                                         np.array([[-yaw_rate]]),
                                         np.array([np.atleast_2d(yaw_rate_std**2)]))
+
+          self.kf.predict_and_observe(t,
+                                      ObservationKind.ROAD_ROLL,
+                                      np.array([[self.roll]]),
+                                      np.array([np.atleast_2d(roll_std**2)]))
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
     elif which == 'carState':
@@ -152,6 +173,7 @@ def main(sm=None, pm=None):
       msg.liveParameters.sensorValid = True
       msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
       msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
+      msg.liveParameters.roll = float(x[States.ROAD_ROLL])
       msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
       msg.liveParameters.angleOffsetDeg = angle_offset
       msg.liveParameters.valid = all((

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8118420b292f4c67ce7e196fc2121571da330e65
+80430e515ea7ca44f2c73f047692d357856e74c1