Correct reference frame name in car_kf

old-commit-hash: 01a14400cb

selfdrive/locationd/kalman/helpers/__init__.py

@@ -56,8 +56,8 @@ class ObservationKind():
   PSEUDORANGE = 22
   PSEUDORANGE_RATE = 23
 
-  CAL_DEVICE_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
-  CAL_DEVICE_FRAME_YAW_RATE = 25  # [rad/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
   STEER_ANGLE = 26  # [rad]
   ANGLE_OFFSET_FAST = 27  # [rad]
   STIFFNESS = 28  # [-]
@@ -87,8 +87,8 @@ class ObservationKind():
     'GLONASS pseudorange',
     'GLONASS pseudorange rate',
 
-    'Calibrated Device Frame x,y speed',
+    'Road Frame x,y speed',
-    'Calibrated Device Frame yaw rate',
+    'Road Frame yaw rate',
     'Steer Angle',
     'Fast Angle Offset',
     'Stiffness',

selfdrive/locationd/kalman/models/car_kf.py

@@ -69,15 +69,15 @@ class CarKalman():
   ])
 
   obs_noise = {
-    ObservationKind.CAL_DEVICE_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
+    ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
-    ObservationKind.CAL_DEVICE_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
+    ObservationKind.ROAD_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
     ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**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),
   }
 
-  maha_test_kinds = []  # [ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED]
+  maha_test_kinds = []  # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
 
   @staticmethod
   def generate_code():
@@ -141,8 +141,8 @@ class CarKalman():
     # Observation functions
     #
     obs_eqs = [
-      [sp.Matrix([r]), ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, None],
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
-      [sp.Matrix([u, v]), ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_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

@@ -32,8 +32,8 @@ class ParamsLearner:
 
       self.update_active()
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-yaw_rate])
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate])
-        self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
 
         # Clamp values
         x = self.kf.x