def generate_residual(K): x_sym = sp.MatrixSymbol('abr', 3, 1) poses_sym = sp.MatrixSymbol('poses', 7 * K, 1) img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1) alpha, beta, rho = x_sym to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)) pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0]) q = poses_sym[K * 7 - 4:K * 7] quat_rot = quat_rotate(*q) rot_g_to_0 = to_c * quat_rot.T rows = [] for i in range(K): pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0]) q = poses_sym[7 * i + 3:7 * i + 7] quat_rot = quat_rotate(*q) rot_g_to_i = to_c * quat_rot.T rot_0_to_i = rot_g_to_i * rot_g_to_0.T trans_0_to_i = rot_g_to_i * (pos_0 - pos_i) funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1 ]) + rho * trans_0_to_i h1, h2, h3 = funct_vec rows.append(h1 / h3 - img_pos_sym[i * 2 + 0]) rows.append(h2 / h3 - img_pos_sym[i * 2 + 1]) img_pos_residual_sym = sp.Matrix(rows) # sympy into c sympy_functions = [] sympy_functions.append( ('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym])) sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym])) return sympy_functions
def generate_orient_error_jac(K): import sympy as sp from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_rotate x_sym = sp.MatrixSymbol('abr', 3, 1) dtheta = sp.MatrixSymbol('dtheta', 3, 1) delta_quat = sp.Matrix(np.ones(4)) delta_quat[1:, :] = sp.Matrix(0.5 * dtheta[0:3, :]) poses_sym = sp.MatrixSymbol('poses', 7 * K, 1) img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1) alpha, beta, rho = x_sym to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)) pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0]) q = quat_matrix_l(poses_sym[K * 7 - 4:K * 7]) * delta_quat quat_rot = quat_rotate(*q) rot_g_to_0 = to_c * quat_rot.T rows = [] for i in range(K): pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0]) q = quat_matrix_l(poses_sym[7 * i + 3:7 * i + 7]) * delta_quat quat_rot = quat_rotate(*q) rot_g_to_i = to_c * quat_rot.T rot_0_to_i = rot_g_to_i * (rot_g_to_0.T) trans_0_to_i = rot_g_to_i * (pos_0 - pos_i) funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1 ]) + rho * trans_0_to_i h1, h2, h3 = funct_vec rows.append(h1 / h3 - img_pos_sym[i * 2 + 0]) rows.append(h2 / h3 - img_pos_sym[i * 2 + 1]) img_pos_residual_sym = sp.Matrix(rows) # sympy into c sympy_functions = [] sympy_functions.append( ('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta])) return sympy_functions
def generate_code(N=4): dim_augment = LocKalman.dim_augment dim_augment_err = LocKalman.dim_augment_err dim_main = LocKalman.x_initial.shape[0] dim_main_err = LocKalman.P_initial.shape[0] dim_state = dim_main + dim_augment * N dim_state_err = dim_main_err + dim_augment_err * N maha_test_kinds = LocKalman.maha_test_kinds name = f"{LocKalman.name}_{N}" # make functions and jacobians with sympy # state variables state_sym = sp.MatrixSymbol('state', dim_state, 1) state = sp.Matrix(state_sym) x, y, z = state[States.ECEF_POS, :] q = state[States.ECEF_ORIENTATION, :] v = state[States.ECEF_VELOCITY, :] vx, vy, vz = v omega = state[States.ANGULAR_VELOCITY, :] vroll, vpitch, vyaw = omega #cb = state[States.CLOCK_BIAS, :][0, 0] #cd = state[States.CLOCK_DRIFT, :][0, 0] cb, cd = state[13:15, :] roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] #odo_scale = state[States.ODO_SCALE, :][0,0] odo_scale = state[18, :] acceleration = state[States.ACCELERATION, :] #focal_scale = state[States.FOCAL_SCALE, :][0,0] focal_scale = state[22, :] imu_angles = state[States.IMU_OFFSET, :] glonass_bias, glonass_freq_slope = state[26:28, :] 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') # calibration and attitude rotation matrices quat_rot = quat_rotate(*q) # Got the quat predict equations from here # A New Quaternion-Based Kalman Filter for # Real-Time Attitude Estimation Using the Two-Step # Geometrically-Intuitive Correction Algorithm A = 0.5 * sp.Matrix( [[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch], [vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]]) q_dot = A * q # Time derivative of the state as a function of state state_dot = sp.Matrix(np.zeros((dim_state, 1))) state_dot[States.ECEF_POS, :] = v state_dot[States.ECEF_ORIENTATION, :] = q_dot state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration state_dot[13, 0] = cd state_dot[14, 0] = ca #state_dot[States.CLOCK_BIAS, 0][0,0] = cd state_dot[States.CLOCK_DRIFT, 0][0, 0] = ca # Basic descretization, 1st order intergrator # Can be pretty bad if dt is big f_sym = state + dt * state_dot state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) state_err = sp.Matrix(state_err_sym) quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] v_err = state_err[States.ECEF_VELOCITY_ERR, :] omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] #cd_err = state_err[States.CLOCK_DRIFT_ERR, :][0,:] cd_err = state_err[13, :] acceleration_err = state_err[States.ACCELERATION_ERR, :] ca_err = state_err[27, :] # 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[States.ECEF_POS_ERR, :] = v_err state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot state_err_dot[ States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * ( acceleration + acceleration_err) #state_err_dot[States.CLOCK_BIAS_ERR, :][0,:] = cd_err #state_err_dot[States.CLOCK_DRIFT_ERR, :][0,:] = ca_err state_err_dot[12, :][0, :] = cd_err state_err_dot[13, :][0, :] = ca_err f_err_sym = state_err + dt * state_err_dot # convenient indexing # q idxs are for quats and p idxs are for other q_idxs = [[3, dim_augment]] + [[ dim_main + n * dim_augment + 3, dim_main + (n + 1) * dim_augment ] for n in range(N)] q_err_idxs = [[3, dim_augment_err]] + [[ dim_main_err + n * dim_augment_err + 3, dim_main_err + (n + 1) * dim_augment_err ] for n in range(N)] p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[ dim_main + n * dim_augment, dim_main + n * dim_augment + 3 ] for n in range(N)] p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[ dim_main_err + n * dim_augment_err, dim_main_err + n * dim_augment_err + 3 ] for n in range(N)] # Observation matrix modifier H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r( state[q_idx[0]:q_idx[1]])[:, 1:] # these error functions are defined so that say there # is a nominal x and true x: # true x = err_function(nominal x, delta x) # delta x = inv_err_function(nominal x, true x) nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) true_x = sp.MatrixSymbol('true_x', dim_state, 1) delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): delta_quat = sp.Matrix(np.ones((4))) delta_quat[1:, :] = sp.Matrix( 0.5 * delta_x[q_err_idx[0]:q_err_idx[1], :]) err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r( nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix( nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): delta_quat = quat_matrix_r( nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) eskf_params = [[err_function_sym, nom_x, delta_x], [inv_err_function_sym, nom_x, true_x], H_mod_sym, f_err_sym, state_err_sym] # # Observation functions # # extra args sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1) # expand extra args sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] los_x, los_y, los_z = sat_los_sym orb_x, orb_y, orb_z = orb_epos_sym h_pseudorange_sym = sp.Matrix( [sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb]) h_pseudorange_glonass_sym = sp.Matrix([ sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb + glonass_bias + glonass_freq_slope * glonass_freq ]) los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) h_pseudorange_rate_sym = sp.Matrix([ los_vector[0] * (sat_vx - vx) + los_vector[1] * (sat_vy - vy) + los_vector[2] * (sat_vz - vz) + cd ]) imu_rot = euler_rotate(*imu_angles) h_gyro_sym = imu_rot * sp.Matrix( [vroll + roll_bias, vpitch + pitch_bias, vyaw + yaw_bias]) pos = sp.Matrix([x, y, z]) gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) h_acc_sym = imu_rot * (gravity + acceleration) h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) speed = sp.sqrt(vx**2 + vy**2 + vz**2) h_speed_sym = sp.Matrix([speed * odo_scale]) # orb stuff orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z]) orb_pos_rot_sym = quat_rot.T * orb_pos_sym s = orb_pos_rot_sym[0] h_orb_point_sym = sp.Matrix([(1 / s) * (orb_pos_rot_sym[1]), (1 / s) * (orb_pos_rot_sym[2])]) h_pos_sym = sp.Matrix([x, y, z]) h_imu_frame_sym = sp.Matrix(imu_angles) h_relative_motion = sp.Matrix(quat_rot.T * v) obs_eqs = [ [h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], [h_gyro_sym, ObservationKind.PHONE_GYRO, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None], [ h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym ], [ h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym ], [ h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym ], [ h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym ], [h_pos_sym, ObservationKind.ECEF_POS, None], [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_imu_frame_sym, ObservationKind.IMU_FRAME, None], [h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym] ] # MSCKF configuration if N > 0: focal_scale = 1 # Add observation functions for orb feature tracks track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1) track_x, track_y, track_z = track_epos_sym h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1))) track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) track_pos_rot_sym = quat_rot.T * track_pos_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_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1))) h_msckf_test_sym[-3:, :] = sp.Matrix( [track_x - x, track_y - y, track_z - z]) for n in range(N): idx = dim_main + n * dim_augment err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used? x, y, z = state[idx:idx + 3] q = state[idx + 3:idx + 7] 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 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_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix( [track_x - x, track_y - y, track_z - z]) obs_eqs.append( [h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym]) obs_eqs.append( [h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym]) obs_eqs.append([ h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym ]) msckf_params = [ dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES] ] else: msckf_params = None gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
def generate_code(): name = LiveKalman.name dim_state = LiveKalman.initial_x.shape[0] dim_state_err = LiveKalman.initial_P_diag.shape[0] state_sym = sp.MatrixSymbol('state', dim_state, 1) state = sp.Matrix(state_sym) x, y, z = state[States.ECEF_POS, :] q = state[States.ECEF_ORIENTATION, :] v = state[States.ECEF_VELOCITY, :] vx, vy, vz = v omega = state[States.ANGULAR_VELOCITY, :] vroll, vpitch, vyaw = omega roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] odo_scale = state[States.ODO_SCALE, :][0,:] acceleration = state[States.ACCELERATION, :] imu_angles = state[States.IMU_OFFSET, :] dt = sp.Symbol('dt') # calibration and attitude rotation matrices quat_rot = quat_rotate(*q) # Got the quat predict equations from here # A New Quaternion-Based Kalman Filter for # Real-Time Attitude Estimation Using the Two-Step # Geometrically-Intuitive Correction Algorithm A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch], [vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]]) q_dot = A * q # Time derivative of the state as a function of state state_dot = sp.Matrix(np.zeros((dim_state, 1))) state_dot[States.ECEF_POS, :] = v state_dot[States.ECEF_ORIENTATION, :] = q_dot state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration # Basic descretization, 1st order intergrator # Can be pretty bad if dt is big f_sym = state + dt * state_dot state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) state_err = sp.Matrix(state_err_sym) quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] v_err = state_err[States.ECEF_VELOCITY_ERR, :] omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] acceleration_err = state_err[States.ACCELERATION_ERR, :] # 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[States.ECEF_POS_ERR, :] = v_err state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) f_err_sym = state_err + dt * state_err_dot # Observation matrix modifier H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) 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: # true x = err_function(nominal x, delta x) # delta x = inv_err_function(nominal x, true x) nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) true_x = sp.MatrixSymbol('true_x', dim_state, 1) delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) 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[States.ECEF_ORIENTATION_ERR, :]) err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_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[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 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], H_mod_sym, f_err_sym, state_err_sym] # # Observation functions # imu_rot = euler_rotate(*imu_angles) h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias, vpitch + pitch_bias, vyaw + yaw_bias]) pos = sp.Matrix([x, y, z]) gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) h_acc_sym = imu_rot * (gravity + acceleration) h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) speed = sp.sqrt(vx**2 + vy**2 + vz**2) h_speed_sym = sp.Matrix([speed * odo_scale]) h_pos_sym = sp.Matrix([x, y, z]) h_imu_frame_sym = sp.Matrix(imu_angles) h_relative_motion = sp.Matrix(quat_rot.T * v) obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], [h_gyro_sym, ObservationKind.PHONE_GYRO, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_pos_sym, ObservationKind.ECEF_POS, None], [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)