no magic numbers

old-commit-hash: 2b5a8fd7af

selfdrive/locationd/kalman/models/live_kf.py

@@ -79,7 +79,7 @@ class LiveKalman():
     omega = state[States.ANGULAR_VELOCITY, :]
     vroll, vpitch, vyaw = omega
     roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
-    odo_scale = state[16, :]
+    odo_scale = state[States.ODO_SCALE, :][0,:]
     acceleration = state[States.ACCELERATION, :]
     imu_angles = state[States.IMU_OFFSET, :]
 
@@ -126,9 +126,9 @@ class LiveKalman():
 
     # Observation matrix modifier
     H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
-    H_mod_sym[0:3, 0:3] = np.eye(3)
+    H_mod_sym[States.ECEF_POSITION, States.ECEF_POSITION_ERR] = np.eye(States.ECEF_POSITION.stop - States.ECEF_POSTION.stop)
-    H_mod_sym[3:7, 3:6] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
+    H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
-    H_mod_sym[7:, 6:] = np.eye(dim_state - 7)
+    H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
 
     # these error functions are defined so that say there
     # is a nominal x and true x:
@@ -140,16 +140,16 @@ class LiveKalman():
 
     err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
     delta_quat = sp.Matrix(np.ones((4)))
-    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[3:6, :])
+    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
-    err_function_sym[0:3, :] = sp.Matrix(nom_x[0:3, :] + delta_x[0:3, :])
+    err_function_sym[States.ECEF_POSITION, :] = sp.Matrix(nom_x[States.ECEF_POSITION, :] + delta_x[States.ECEF_POSITION_ERR, :])
-    err_function_sym[3:7, 0] = quat_matrix_r(nom_x[3:7, 0]) * delta_quat
+    err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
-    err_function_sym[7:, :] = sp.Matrix(nom_x[7:, :] + delta_x[6:, :])
+    err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
 
     inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
-    inv_err_function_sym[0:3, 0] = sp.Matrix(-nom_x[0:3, 0] + true_x[0:3, 0])
+    inv_err_function_sym[States.ECEF_POSITION_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POSITION, 0] + true_x[States.ECEF_POSITION, 0])
-    delta_quat = quat_matrix_r(nom_x[3:7, 0]).T * true_x[3:7, 0]
+    delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
-    inv_err_function_sym[3:6, 0] = sp.Matrix(2 * delta_quat[1:])
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
-    inv_err_function_sym[6:, 0] = sp.Matrix(-nom_x[7:, 0] + true_x[7:, 0])
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
 
     eskf_params = [[err_function_sym, nom_x, delta_x],
                    [inv_err_function_sym, nom_x, true_x],