Paramsd handle liveLocation not valid and add alerts

old-commit-hash: 44484102db

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

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)