diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 031c56ceb9..75534efa5a 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -71,11 +71,11 @@ class CarKalman(KalmanFilter): P_initial = Q.copy() obs_noise: Dict[int, Any] = { - ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**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.STIFFNESS: np.atleast_2d(0.5**2), ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), } @@ -106,9 +106,9 @@ class CarKalman(KalmanFilter): state = sp.Matrix(state_sym) # Vehicle model constants - x = state[States.STIFFNESS, :][0, 0] + sf = state[States.STIFFNESS, :][0, 0] - cF, cR = x * cF_orig, x * cR_orig + cF, cR = sf * cF_orig, sf * 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] @@ -154,7 +154,7 @@ class CarKalman(KalmanFilter): [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], [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([sf]), ObservationKind.STIFFNESS, None], [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None], ] diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 9557568ba2..0efb4d04be 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -45,7 +45,7 @@ class ParamsLearner: yaw_rate_std = msg.angularVelocityCalibrated.std[2] localizer_roll = msg.orientationNED.value[0] - localizer_roll_std = msg.orientationNED.std[0] + localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX if roll_valid: roll = localizer_roll @@ -76,6 +76,14 @@ class ParamsLearner: np.array([np.atleast_2d(roll_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) + # We observe the current stiffness and steer ratio (with a high observation noise) to bound + # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the + # states in longer routes (especially straight stretches). + stiffness = float(self.kf.x[States.STIFFNESS]) + steer_ratio = float(self.kf.x[States.STEER_RATIO]) + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) + elif which == 'carState': self.steering_angle = msg.steeringAngleDeg self.steering_pressed = msg.steeringPressed diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index eb53e2b58b..f30368ac41 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5cec6f1575235206c4e0341d49de53be8d4e3429 \ No newline at end of file +3bc128c5b47e036021ccfaab9a9924d61eeb59e2 \ No newline at end of file