From 341deaaf1a3b984e2329ad1fa9b2e94d2f804923 Mon Sep 17 00:00:00 2001 From: Miles Calloway <26465212+mcallbosco@users.noreply.github.com> Date: Sun, 13 Feb 2022 15:23:15 -0500 Subject: [PATCH] Revert "Road Roll Compensation Rebased (#23251)" This reverts commit c94b745c754948ea1e13e38ca2f00d21293821f2. --- selfdrive/car/ford/carcontroller.py | 2 +- selfdrive/car/honda/interface.py | 1 + selfdrive/controls/controlsd.py | 5 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_indi.py | 4 +- selfdrive/controls/lib/latcontrol_lqr.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- .../controls/lib/tests/test_vehicle_model.py | 70 ------------------- selfdrive/controls/lib/vehicle_model.py | 63 ++++------------- selfdrive/locationd/models/car_kf.py | 19 +---- selfdrive/locationd/models/constants.py | 3 - selfdrive/locationd/paramsd.py | 38 +++------- 12 files changed, 35 insertions(+), 176 deletions(-) delete mode 100755 selfdrive/controls/lib/tests/test_vehicle_model.py diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 57eb168888ca00..c4fe5c784d9243 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -49,7 +49,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel, dragon if (frame % 3) == 0: - curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0) + curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo) # 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 f77498272449b5..a0bc97b65ad11c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -418,6 +418,7 @@ def update(self, c, can_strings, dragonconf): ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl) ret.lkMode = self.CS.lkMode 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) #dp ret.engineRPM = self.CS.engineRPM diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 00f7c268dbc9fa..f688815611d2ee 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -650,9 +650,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log): # Curvature & Steering angle params = self.sm['liveParameters'] - - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) - curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll) + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg) + curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo) # controlsState dat = messaging.new_message('controlsState') diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 3fe4ce0fdbcddb..8fcb9ae7bfd066 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -18,7 +18,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur 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, params.roll)) + angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) 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 6d432e2d85afa7..50a8e22b3cfa02 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -93,11 +93,11 @@ def update(self, active, CS, CP, VM, params, curvature, curvature_rate): 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, params.roll) + steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo) steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) - rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0) + rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo) 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 a87ff4286005c8..16fffac2791b1c 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -53,7 +53,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur # 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, params.roll)) + desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) 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 e63c96e8349388..c7730d011cea79 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -21,7 +21,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur 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, params.roll)) + angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) 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 deleted file mode 100755 index f2636027e88201..00000000000000 --- a/selfdrive/controls/lib/tests/test_vehicle_model.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/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 7fd9c90088af95..ae6ada314d975c 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -5,7 +5,7 @@ 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], and roll [rad] +The input u is the steering angle [rad] The system is defined by x_dot = A*x + B*u @@ -20,9 +20,6 @@ from cereal import car from common.params import Params -ACCELERATION_DUE_TO_GRAVITY = 9.8 - - class VehicleModel: def __init__(self, CP: car.CarParams): """ @@ -47,7 +44,7 @@ def update_params(self, stiffness_factor: float, steer_ratio: float) -> None: self.cR = stiffness_factor * self.cR_orig self.sR = steer_ratio - def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray: + def steady_state_sol(self, sa: float, u: 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), @@ -56,28 +53,26 @@ def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray: 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, roll, self) + return dyn_ss_sol(sa, u, self) else: return kin_ss_sol(sa, u, self) - def calc_curvature(self, sa: float, u: float, roll: float) -> float: + def calc_curvature(self, sa: float, u: 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) + self.roll_compensation(roll, u) + return self.curvature_factor(u) * sa / self.sR def curvature_factor(self, u: float) -> float: """Returns the curvature factor. @@ -92,63 +87,43 @@ def curvature_factor(self, u: float) -> float: 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, roll: float) -> float: + def get_steer_from_curvature(self, curv: float, u: 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.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] + return curv * self.sR * 1.0 / self.curvature_factor(u) - 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: + def get_steer_from_yaw_rate(self, yaw_rate: float, u: 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, roll) + return self.get_steer_from_curvature(curv, u) - def yaw_rate(self, sa: float, u: float, roll: float) -> float: + def yaw_rate(self, sa: float, u: 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, roll) * u + return self.calc_curvature(sa, u) * u def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray: @@ -178,7 +153,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 2x2 B matrix + A tuple with the 2x2 A matrix, and 2x1 B matrix Parameters in the vehicle model: cF: Tire stiffness Front [N/rad] @@ -191,38 +166,30 @@ 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, 2)) + B = np.zeros((2, 1)) 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, roll: float, VM: VehicleModel) -> np.ndarray: +def dyn_ss_sol(sa: float, u: 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) - inp = np.array([[sa], [roll]]) - return -solve(A, B) @ inp + return -solve(A, B) * sa def calc_slip_factor(VM): diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 031c56ceb9edeb..dac95dcd7bbce3 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -5,7 +5,6 @@ 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 @@ -38,7 +37,6 @@ 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): @@ -53,7 +51,6 @@ class CarKalman(KalmanFilter): 10.0, 0.0, 0.0, 0.0, - 0.0 ]) # process noise @@ -66,14 +63,12 @@ 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), @@ -92,7 +87,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 @@ -111,7 +106,6 @@ def generate_code(generated_dir): 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] @@ -128,12 +122,8 @@ def generate_code(generated_dir): 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) - C * theta + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) dt = sp.Symbol('dt') state_dot = sp.Matrix(np.zeros((dim_state, 1))) @@ -155,12 +145,11 @@ def generate_code(generated_dir): [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, P_initial=None): # pylint: disable=super-init-not-called + def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): # 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 @@ -168,8 +157,6 @@ def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offs 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 f22f503ee0cd74..7450f76be7e3bb 100644 --- a/selfdrive/locationd/models/constants.py +++ b/selfdrive/locationd/models/constants.py @@ -38,7 +38,6 @@ class ObservationKind: STIFFNESS = 28 # [-] STEER_RATIO = 29 # [-] ROAD_FRAME_X_SPEED = 30 # (x) [m/s] - ROAD_ROLL = 31 # [rad] names = [ 'Unknown', @@ -70,8 +69,6 @@ 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 e5666419dcc092..3164ea3bd9c029 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -16,12 +16,10 @@ 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, P_initial=None): - self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial) + def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset): + self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset) self.kf.filter.set_global("mass", CP.mass) self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) @@ -32,10 +30,9 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No self.active = False - self.speed = 0.0 - self.roll = 0.0 + self.speed = 0 self.steering_pressed = False - self.steering_angle = 0.0 + self.steering_angle = 0 self.valid = True @@ -44,34 +41,16 @@ def handle_log(self, t, which, msg): 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: - - 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)])) - + if msg.inputsOK and msg.posenetOK and yaw_rate_valid: self.kf.predict_and_observe(t, - ObservationKind.ROAD_ROLL, - np.array([[self.roll]]), - np.array([np.atleast_2d(roll_std**2)])) + 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.ANGLE_OFFSET_FAST, np.array([[0]])) elif which == 'carState': @@ -173,7 +152,6 @@ 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((