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)