misc cleanup

old-commit-hash: 8257c60f71

selfdrive/locationd/kalman/models/live_kf.py

@@ -22,8 +22,9 @@ class States():
   ACCELERATION = slice(17, 20)  # Acceleration in device frame in m/s**2
   IMU_OFFSET = slice(20, 23)  # imu offset angles in radians
 
+  # Error-state has different slices because it is an ESKF
   ECEF_POS_ERR = slice(0, 3)
-  ECEF_ORIENTATION_ERR = slice(3, 6)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
   ECEF_VELOCITY_ERR = slice(6, 9)
   ANGULAR_VELOCITY_ERR = slice(9, 12)
   GYRO_BIAS_ERR = slice(12, 15)

selfdrive/locationd/kalman/models/loc_kf.py

@@ -34,7 +34,7 @@ def parse_pr(m):
 
 class States():
   ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
-  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
+  ECEF_ORIENTATION = slice(3, 7)  # quat for orientation of phone in ecef
   ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
   ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
   CLOCK_BIAS = slice(13, 14)  # clock bias in light-meters,
@@ -48,6 +48,22 @@ class States():
   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,
 
+  # Error-state has different slices because it is an ESKF
+  ECEF_POS_ERR = slice(0, 3)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
+  ECEF_VELOCITY_ERR = slice(6, 9)
+  ANGULAR_VELOCITY_ERR = slice(9, 12)
+  CLOCK_BIAS_ERR = slice(12, 13)
+  CLOCK_DRIFT_ERR = slice(13, 14)
+  GYRO_BIAS_ERR = slice(14, 17)
+  ODO_SCALE_ERR = slice(17, 18)
+  ACCELERATION_ERR = slice(18, 21)
+  FOCAL_SCALE_ERR = slice(21, 22)
+  IMU_OFFSET_ERR = slice(22, 25)
+  GLONASS_BIAS_ERR = slice(25, 26)
+  GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
+  CLOCK_ACCELERATION_ERR = slice(27, 28)
+
 
 class LocKalman():
   name = "loc"
@@ -92,6 +108,7 @@ class LocKalman():
                (.1)**2, (.01)**2,
                0.005**2])
 
+  # measurements that need to pass mahalanobis distance outlier rejector
   maha_test_kinds = [ObservationKind.ORB_FEATURES]  # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
   dim_augment = 7
   dim_augment_err = 6
@@ -113,20 +130,22 @@ class LocKalman():
     # state variables
     state_sym = sp.MatrixSymbol('state', dim_state, 1)
     state = sp.Matrix(state_sym)
-    x, y, z = state[0:3, :]
+    x, y, z = state[States.ECEF_POS, :]
-    q = state[3:7, :]
+    q = state[States.ECEF_ORIENTATION, :]
-    v = state[7:10, :]
+    v = state[States.ECEF_VELOCITY, :]
     vx, vy, vz = v
-    omega = state[10:13, :]
+    omega = state[States.ANGULAR_VELOCITY, :]
     vroll, vpitch, vyaw = omega
-    cb, cd = state[13:15, :]
+    cb = state[States.CLOCK_BIAS, :][0, 0]
-    roll_bias, pitch_bias, yaw_bias = state[15:18, :]
+    cd = state[States.CLOCK_DRIFT, :][0, 0]
-    odo_scale = state[18, :]
+    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
-    acceleration = state[19:22, :]
+    odo_scale = state[States.ODO_SCALE, :][0,0]
-    focal_scale = state[22, :]
+    acceleration = state[States.ACCELERATION, :]
-    imu_angles = state[23:26, :]
+    focal_scale = state[States.FOCAL_SCALE, :][0,0]
-    glonass_bias, glonass_freq_slope = state[26:28, :]
+    imu_angles = state[States.IMU_OFFSET, :]
-    ca = state[28, 0]
+    glonass_bias = state[States.GLONASS_BIAS, :][0,0]
+    glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :][0,0]
+    ca = state[States.CLOCK_ACCELERATION, :][0,0]
 
     dt = sp.Symbol('dt')
 
@@ -145,11 +164,11 @@ class LocKalman():
 
     # Time derivative of the state as a function of state
     state_dot = sp.Matrix(np.zeros((dim_state, 1)))
-    state_dot[:3, :] = v
+    state_dot[States.ECEF_POS, :] = v
-    state_dot[3:7, :] = q_dot
+    state_dot[States.ECEF_ORIENTATION, :] = q_dot
-    state_dot[7:10, 0] = quat_rot * acceleration
+    state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
-    state_dot[13, 0] = cd
+    state_dot[States.CLOCK_BIAS, 0][0,0] = cd
-    state_dot[14, 0] = ca
+    state_dot[States.CLOCK_DRIFT, 0][0,0] = ca
 
     # Basic descretization, 1st order intergrator
     # Can be pretty bad if dt is big
@@ -157,22 +176,22 @@ class LocKalman():
 
     state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
     state_err = sp.Matrix(state_err_sym)
-    quat_err = state_err[3:6, :]
+    quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
-    v_err = state_err[6:9, :]
+    v_err = state_err[States.ECEF_VELOCITY_ERR, :]
-    omega_err = state_err[9:12, :]
+    omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
-    cd_err = state_err[13, :]
+    cd_err = state_err[States.CLOCK_DRIFT_ERR, :][0,:]
-    acceleration_err = state_err[18:21, :]
+    acceleration_err = state_err[States.ACCELERATION_ERR, :]
-    ca_err = state_err[27, :]
+    ca_err = state_err[States.CLOCK_ACCELERATION_ERR, :][0,:]
 
     # Time derivative of the state error as a function of state error and state
     quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
     q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
     state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
-    state_err_dot[:3, :] = v_err
+    state_err_dot[States.ECEF_POS_ERR, :] = v_err
-    state_err_dot[3:6, :] = q_err_dot
+    state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
-    state_err_dot[6:9, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
+    state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
-    state_err_dot[12, :] = cd_err
+    state_err_dot[States.CLOCK_BIAS_ERR, :][0,:] = cd_err
-    state_err_dot[13, :] = ca_err
+    state_err_dot[States.CLOCK_DRIFT_ERR, :][0,:] = ca_err
     f_err_sym = state_err + dt * state_err_dot
 
     # convenient indexing