diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py index 4adb8c291..b31affa42 100755 --- a/selfdrive/locationd/kalman/models/car_kf.py +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -46,22 +46,22 @@ class CarKalman(): # state covariance P_initial = np.diag([ - .1**2, - .1**2, + 0.1**2, + 0.1**2, math.radians(0.1)**2, math.radians(0.1)**2, 10**2, 10**2, - 1**2, - 1**2, + 1.0**2, + 1.0**2, ]) # process noise Q = np.diag([ - (.05/10)**2, + (.05/100)**2, .0001**2, - math.radians(0.01)**2, - math.radians(0.2)**2, + math.radians(0.001)**2, + math.radians(0.05)**2, .1**2, .1**2, math.radians(0.1)**2, diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 375ae4ad4..80eddedf3 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -52,17 +52,17 @@ class ParamsLearner: 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, v_calibrated_std[1]**2])])) + 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]]])) # Clamp values - x = self.kf.x - if not (10 < x[States.STEER_RATIO] < 25): - self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]])) + # x = self.kf.x + # if not (10 < x[States.STEER_RATIO] < 25): + # self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]])) - if not (0.5 < x[States.STIFFNESS] < 3.0): - self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]])) + # if not (0.5 < x[States.STIFFNESS] < 3.0): + # self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]])) elif which == 'carState': self.carstate_counter += 1 @@ -136,7 +136,7 @@ def main(sm=None, pm=None): msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) - msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST]) + msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST]) i += 1 if i % 6000 == 0: # once a minute