Full localizer: Use standard naming conventions (#25007)

Use standard naming conventions

Author: haraschax

selfdrive/locationd/models/loc_kf.py

@@ -25,15 +25,15 @@ class States():
   ODO_SCALE_UNUSED = slice(18, 19)  # odometer scale
   ACCELERATION = slice(19, 22)  # Acceleration in device frame in m/s**2
   FOCAL_SCALE_UNUSED = slice(22, 23)  # focal length scale
-  IMU_OFFSET = slice(23, 26)  # imu offset angles in radians
+  IMU_FROM_DEVICE_EULER = slice(23, 26)  # imu offset angles in radians
   GLONASS_BIAS = slice(26, 27)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
   GLONASS_FREQ_SLOPE = slice(27, 28)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
   CLOCK_ACCELERATION = slice(28, 29)  # clock acceleration in light-meters/s**2,
   ACCELEROMETER_SCALE_UNUSED = slice(29, 30)  # scale of mems accelerometer
   ACCELEROMETER_BIAS = slice(30, 33)  # bias of mems accelerometer
   # TODO the offset is likely a translation of the sensor, not a rotation of the camera
-  WIDE_CAM_OFFSET = slice(33, 36)  # wide camera offset angles in radians (tici only)
-  # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET).
+  WIDE_FROM_DEVICE_EULER = slice(33, 36)  # wide camera offset angles in radians (tici only)
+  # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_FROM_DEVICE_EULER).
   # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE
 
   # Error-state has different slices because it is an ESKF
@@ -47,13 +47,13 @@ class States():
   ODO_SCALE_ERR_UNUSED = slice(17, 18)
   ACCELERATION_ERR = slice(18, 21)
   FOCAL_SCALE_ERR_UNUSED = slice(21, 22)
-  IMU_OFFSET_ERR = slice(22, 25)
+  IMU_FROM_DEVICE_EULER_ERR = slice(22, 25)
   GLONASS_BIAS_ERR = slice(25, 26)
   GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
   CLOCK_ACCELERATION_ERR = slice(27, 28)
   ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29)
   ACCELEROMETER_BIAS_ERR = slice(29, 32)
-  WIDE_CAM_OFFSET_ERR = slice(32, 35)
+  WIDE_FROM_DEVICE_EULER_ERR = slice(32, 35)
 
 
 class LocKalman():
@@ -140,15 +140,15 @@ def generate_code(generated_dir, N=4):
     cd = state[States.CLOCK_DRIFT, :]
     roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
     acceleration = state[States.ACCELERATION, :]
-    imu_angles = state[States.IMU_OFFSET, :]
-    imu_angles[0, 0] = 0  # not observable enough
-    imu_angles[2, 0] = 0  # not observable enough
+    imu_from_device_euler = state[States.IMU_FROM_DEVICE_EULER, :]
+    imu_from_device_euler[0, 0] = 0  # not observable enough
+    imu_from_device_euler[2, 0] = 0  # not observable enough
     glonass_bias = state[States.GLONASS_BIAS, :]
     glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
     ca = state[States.CLOCK_ACCELERATION, :]
     accel_bias = state[States.ACCELEROMETER_BIAS, :]
-    wide_cam_angles = state[States.WIDE_CAM_OFFSET, :]
-    wide_cam_angles[0, 0] = 0  # not observable enough
+    wide_from_device_euler = state[States.WIDE_FROM_DEVICE_EULER, :]
+    wide_from_device_euler[0, 0] = 0  # not observable enough
 
     dt = sp.Symbol('dt')
 
@@ -273,15 +273,15 @@ def generate_code(generated_dir, N=4):
                                         los_vector[2] * (sat_vz - vz) +
                                         cd[0]])
 
-    imu_rot = euler_rotate(*imu_angles)
-    h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
+    imu_from_device = euler_rotate(*imu_from_device_euler)
+    h_gyro_sym = imu_from_device * sp.Matrix([vroll + roll_bias,
                                       vpitch + pitch_bias,
                                       vyaw + yaw_bias])
 
     pos = sp.Matrix([x, y, z])
     # add 1 for stability, prevent division by 0
     gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos)
-    h_acc_sym = imu_rot * (gravity + acceleration + accel_bias)
+    h_acc_sym = imu_from_device * (gravity + acceleration + accel_bias)
     h_acc_stationary_sym = acceleration
     h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
     h_relative_motion = sp.Matrix(quat_rot.T * v)
@@ -297,7 +297,7 @@ def generate_code(generated_dir, N=4):
                [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
                [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
 
-    wide_cam_rot = euler_rotate(*wide_cam_angles)
+    wide_from_device = euler_rotate(*wide_from_device_euler)
     # MSCKF configuration
     if N > 0:
       # experimentally found this is correct value for imx298 with 910 focal length
@@ -312,7 +312,7 @@ def generate_code(generated_dir, N=4):
 
       track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
       track_pos_rot_sym = quat_rot.T * track_pos_sym
-      track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym
+      track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym
       h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
                                        focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
       h_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]),
@@ -329,7 +329,7 @@ def generate_code(generated_dir, N=4):
         quat_rot = quat_rotate(*q)
         track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
         track_pos_rot_sym = quat_rot.T * track_pos_sym
-        track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym
+        track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym
         h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
                                                      focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
         h_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]),