retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

no magic numbers

albatross
Harald Schafer 2020-02-12 14:05:48 -08:00
parent 1de5d84a06
commit 2b5a8fd7af
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
selfdrive/locationd/kalman/models/live_kf.py
View File

@ -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[3:7, 3:6] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[7:, 6:] = np.eye(dim_state - 7)
H_mod_sym[States.ECEF_POSITION, States.ECEF_POSITION_ERR] = np.eye(States.ECEF_POSITION.stop - States.ECEF_POSTION.stop)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
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, :])
err_function_sym[0:3, :] = sp.Matrix(nom_x[0:3, :] + delta_x[0:3, :])
err_function_sym[3:7, 0] = quat_matrix_r(nom_x[3:7, 0]) * delta_quat
err_function_sym[7:, :] = sp.Matrix(nom_x[7:, :] + delta_x[6:, :])
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POSITION, :] = sp.Matrix(nom_x[States.ECEF_POSITION, :] + delta_x[States.ECEF_POSITION_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
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])
delta_quat = quat_matrix_r(nom_x[3:7, 0]).T * true_x[3:7, 0]
inv_err_function_sym[3:6, 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_POSITION_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POSITION, 0] + true_x[States.ECEF_POSITION, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
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],