Car_kf tuning

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',

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],

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