From 01a14400cb0190f7c24e8c9878c2b76c68125efe Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Mon, 9 Mar 2020 12:49:52 -0700
Subject: [PATCH] Correct reference frame name in car_kf

---
 selfdrive/locationd/kalman/helpers/__init__.py |  8 ++++----
 selfdrive/locationd/kalman/models/car_kf.py    | 10 +++++-----
 selfdrive/locationd/paramsd.py                 |  4 ++--
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py
index 9c3ae5356..2ec5ba0dd 100644
--- a/selfdrive/locationd/kalman/helpers/__init__.py
+++ b/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]
-  CAL_DEVICE_FRAME_YAW_RATE = 25  # [rad/s]
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/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',
-    'Calibrated Device Frame yaw rate',
+    'Road Frame x,y speed',
+    'Road Frame yaw rate',
     'Steer Angle',
     'Fast Angle Offset',
     'Stiffness',
diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py
index d457dbe20..3ba2b4b84 100755
--- a/selfdrive/locationd/kalman/models/car_kf.py
+++ b/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.CAL_DEVICE_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
+    ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 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([u, v]), ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, None],
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, 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],
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index e2d248ab5..7076a4649 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/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.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate])
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
 
         # Clamp values
         x = self.kf.x