diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py index 2ec5ba0ddb..fd07abbe57 100644 --- a/selfdrive/locationd/kalman/helpers/__init__.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -62,6 +62,7 @@ class ObservationKind(): ANGLE_OFFSET_FAST = 27 # [rad] STIFFNESS = 28 # [-] STEER_RATIO = 29 # [-] + ROAD_FRAME_X_SPEED = 30 # (x) [m/s] names = [ 'Unknown', diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py index b31affa429..0cc90090b4 100755 --- a/selfdrive/locationd/kalman/models/car_kf.py +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -44,35 +44,25 @@ class CarKalman(): 0.0, ]) - # state covariance - P_initial = np.diag([ - 0.1**2, - 0.1**2, - math.radians(0.1)**2, - math.radians(0.1)**2, - - 10**2, 10**2, - 1.0**2, - 1.0**2, - ]) - # process noise Q = np.diag([ (.05/100)**2, .0001**2, - math.radians(0.001)**2, - math.radians(0.05)**2, + math.radians(0.0001)**2, + math.radians(0.1)**2, - .1**2, .1**2, + .1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, ]) + P_initial = Q.copy() obs_noise = { - ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), - ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2), - ObservationKind.STIFFNESS: np.atleast_2d(50.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), } maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED] @@ -139,6 +129,7 @@ class CarKalman(): obs_eqs = [ [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None], [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 80eddedf34..aaf5017dcb 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -35,24 +35,14 @@ class ParamsLearner: def handle_log(self, t, which, msg): if which == 'liveLocationKalman': - v_calibrated = msg.velocityCalibrated.value - v_calibrated_std = msg.velocityCalibrated.std - yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate_std = msg.angularVelocityCalibrated.std[2] - self.active = v_calibrated[0] > 5 - in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed - - if self.active and in_linear_region: + if self.active: 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_XY_SPEED, - np.array([[[v_calibrated[0], -v_calibrated[1]]]]), - np.array([np.diag([v_calibrated_std[0]**2, 1e6*v_calibrated_std[1]**2])])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) @@ -69,9 +59,14 @@ class ParamsLearner: if self.carstate_counter % CARSTATE_DECIMATION == 0: self.steering_angle = msg.steeringAngle self.steering_pressed = msg.steeringPressed + self.speed = msg.vEgo + + in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed + self.active = self.speed > 5 and in_linear_region if self.active: self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]])) if not self.active: # Reset time when stopped so uncertainty doesn't grow