diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 26df70a0bf..b984bf7665 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -49,8 +49,8 @@ class CarKalman(KalmanFilter): Q = np.diag([ (.05/100)**2, .01**2, - math.radians(0.002)**2, - math.radians(0.1)**2, + math.radians(0.02)**2, + math.radians(0.25)**2, .1**2, .01**2, math.radians(0.1)**2, @@ -60,7 +60,7 @@ class CarKalman(KalmanFilter): obs_noise = { ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2), - ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.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), @@ -139,7 +139,7 @@ class CarKalman(KalmanFilter): def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): dim_state = self.initial_x.shape[0] - dim_state_err = self.initial_P_diag.shape[0] + dim_state_err = self.P_initial.shape[0] x_init = self.initial_x x_init[States.STEER_RATIO] = steer_ratio x_init[States.STIFFNESS] = stiffness_factor diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index e359870ba8..c994eced7b 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -5,12 +5,14 @@ import json import numpy as np import cereal.messaging as messaging -from cereal import car +from cereal import car, log from common.params import Params, put_nonblocking from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from selfdrive.locationd.models.constants import GENERATED_DIR from selfdrive.swaglog import cloudlog +FixStatus = log.LiveLocationKalman.Status + CARSTATE_DECIMATION = 5 @@ -32,6 +34,8 @@ class ParamsLearner: self.steering_angle = 0 self.carstate_counter = 0 + self.valid = True + def handle_log(self, t, which, msg): if which == 'liveLocationKalman': @@ -39,21 +43,13 @@ class ParamsLearner: yaw_rate_std = msg.angularVelocityCalibrated.std[2] 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)])) - + if msg.inputsOK and msg.posenetOK and msg.status == FixStatus.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.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]]])) - - # 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 if self.carstate_counter % CARSTATE_DECIMATION == 0: @@ -105,6 +101,7 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner resetting to default values") learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio i = 0 while True: @@ -132,6 +129,12 @@ def main(sm=None, pm=None): msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST]) + msg.liveParameters.valid = all(( + abs(msg.liveParameters.angleOffsetAverage) < 10.0, + abs(msg.liveParameters.angleOffset) < 10.0, + 0.5 <= msg.liveParameters.stiffnessFactor <= 2.0, + min_sr <= msg.liveParameters.steerRatio <= max_sr, + )) i += 1 if i % 6000 == 0: # once a minute @@ -143,13 +146,6 @@ def main(sm=None, pm=None): } put_nonblocking("LiveParameters", json.dumps(params)) - # P = learner.kf.P - # print() - # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5) - # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5) - # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5) - # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5) - pm.send('liveParameters', msg)