paramsd: fix VehicleModelInvalid errors (#23726)

* bound steerratio, stiffness std

* remove start steer_ratio limits after looking at data

* reduce sf obs noise

* update refs

* update refs

* add comment explaining change
old-commit-hash: 8a2cbfe00b

selfdrive/locationd/models/car_kf.py

@@ -71,11 +71,11 @@ class CarKalman(KalmanFilter):
   P_initial = Q.copy()
 
   obs_noise: Dict[int, Any] = {
-    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
     ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
     ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
     ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
-    ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
     ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
   }
 
@@ -106,9 +106,9 @@ class CarKalman(KalmanFilter):
     state = sp.Matrix(state_sym)
 
     # Vehicle model constants
-    x = state[States.STIFFNESS, :][0, 0]
+    sf = state[States.STIFFNESS, :][0, 0]
 
-    cF, cR = x * cF_orig, x * cR_orig
+    cF, cR = sf * cF_orig, sf * cR_orig
     angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
     angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
     theta = state[States.ROAD_ROLL, :][0, 0]
@@ -154,7 +154,7 @@ class CarKalman(KalmanFilter):
       [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
       [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
       [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
-      [sp.Matrix([x]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
       [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
     ]
 

selfdrive/locationd/paramsd.py

@@ -45,7 +45,7 @@ class ParamsLearner:
       yaw_rate_std = msg.angularVelocityCalibrated.std[2]
 
       localizer_roll = msg.orientationNED.value[0]
-      localizer_roll_std = msg.orientationNED.std[0]
+      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
       roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
       if roll_valid:
         roll = localizer_roll
@@ -76,6 +76,14 @@ class ParamsLearner:
                                       np.array([np.atleast_2d(roll_std**2)]))
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
+        # We observe the current stiffness and steer ratio (with a high observation noise) to bound
+        # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
+        # states in longer routes (especially straight stretches).
+        stiffness = float(self.kf.x[States.STIFFNESS])
+        steer_ratio = float(self.kf.x[States.STEER_RATIO])
+        self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
+
     elif which == 'carState':
       self.steering_angle = msg.steeringAngleDeg
       self.steering_pressed = msg.steeringPressed

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-5cec6f1575235206c4e0341d49de53be8d4e3429
+3bc128c5b47e036021ccfaab9a9924d61eeb59e2