diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 2eaacd301c..6257cfd209 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/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: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 47bbd6715d..6604f7d106 100755 --- a/selfdrive/car/honda/interface.py +++ b/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 = [] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cc449537a6..1933ab5282 100755 --- a/selfdrive/controls/controlsd.py +++ b/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') diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 5975fa66b7..dc184cf58b 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 169d504d5b..75b27ac8c1 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/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: diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 9f58aea2e7..5777cde8e8 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index ca78d5fa4e..bcb8ccba56 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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 diff --git a/selfdrive/controls/lib/tests/test_vehicle_model.py b/selfdrive/controls/lib/tests/test_vehicle_model.py new file mode 100755 index 0000000000..f2636027e8 --- /dev/null +++ b/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() diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index a0b1dddfd7..3f180d3252 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/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): diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index dac95dcd7b..031c56ceb9 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/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), @@ -87,7 +92,7 @@ class CarKalman(KalmanFilter): def generate_code(generated_dir): dim_state = CarKalman.initial_x.shape[0] name = CarKalman.name - + # vehicle models comes from The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars # Model used is in 6.15 with formula from 6.198 @@ -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) diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py index 7450f76be7..f22f503ee0 100644 --- a/selfdrive/locationd/models/constants.py +++ b/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 diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 914e0d0223..1542f29d34 100755 --- a/selfdrive/locationd/paramsd.py +++ b/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): - self.kf = CarKalman(GENERATED_DIR, 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, 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_FRAME_YAW_RATE, - np.array([[-yaw_rate]]), - np.array([np.atleast_2d(yaw_rate_std**2)])) + 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(( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8415bc8de7..e4a1605d04 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8118420b292f4c67ce7e196fc2121571da330e65 \ No newline at end of file +80430e515ea7ca44f2c73f047692d357856e74c1 \ No newline at end of file