def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check,
q_check):
# 3.1 Compute Kalman Gain
tempoo = np.linalg.inv((h_jac @ p_cov_check @ h_jac.T) +
sensor_var * np.eye(3))
K_k = p_cov_check @ h_jac.T @ tempoo # NNNNNnote here that the variance is not so true because we have to multiply it by the identity
# 3.2 Compute error state
delta_x = (y_k - p_check)
#print(delta_x.shape)
# 3.3 Correct predicted state
p_hat = p_check + (K_k @ delta_x)[:3]
v_hat = v_check + (K_k @ delta_x)[3:6]
delta_q = Quaternion(axis_angle=(K_k @ delta_x)[6:])
q_hat = delta_q.quat_mult_right(q_check)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - K_k @ h_jac) @ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_chec, y_k, p_chec, v_chec, q_chec):
# 3.1 Compute Kalman Gain
r_km = np.eye(3) * sensor_var
kinv = np.linalg.inv((H_km.dot(p_cov_chec)).dot(H_km.T) + r_km)
k_k = (p_cov_chec.dot(H_km.T)).dot(kinv)
# 3.2 Compute error state
# print(p_chec)
delx = k_k.dot(y_k - p_chec)
# print(y_k)
# print(delx)
# 3.3 Correct predicted state
p_hat = p_chec + delx[0:3]
v_hat = v_chec + delx[3:6]
phi = (delx[6:9]).T
phi = phi.flatten()
q_error = Quaternion(euler=angle_normalize(phi))
q_hat = q_error.quat_mult_right(q_chec)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - (k_k.dot(H_km))).dot(p_cov_chec)
return p_hat, v_hat, q_hat, p_cov_hat
################################################################################################
# Now that everything is set up, we can start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################################
F_k = np.eye(9)
for k in range(1, imu_f.data.shape[0]
): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
C_ns = Quaternion(*q_est[k - 1]).to_mat() # I don't know why it's *
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (0.5 * (delta_t**2)) * (
C_ns @ imu_f.data[k - 1] + g)
v_est[k] = v_est[k - 1] + delta_t * (C_ns @ imu_f.data[k - 1] + g)
q_tempo = Quaternion(euler=imu_w.data[k - 1] * delta_t)
q_est[k] = q_tempo.quat_mult_right(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
F_k[0:3, 3:6] = np.eye(3) * delta_t
F_k[3:6, 3:6] = np.eye(3)
F_k[3:6, 6:9] = -(C_ns @ skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1)))) * delta_t
vfa = var_imu_f**2
vfw = var_imu_w**2
Q_km = (delta_t**2) * np.diag([vfa, vfa, vfa, vfw, vfw, vfw])
# 2. Propagate uncertainty
p_cov[k] = F_k @ p_cov[k - 1] @ F_k.T + l_jac @ Q_km @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
####
for i in range(len(gnss.t)):
if abs(gnss.t[i] - imu_f.t[k]) < 0.01:
#### 5. Main Filter Loop #######################################################################
################################################################################################
# Now that everything is set up, we can start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################################
for k in range(1, imu_f.data.shape[0]): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
c_ns = Quaternion(*q_est[k-1]).to_mat()
c_ns_dot_f_km = np.dot(c_ns, imu_f.data[k-1])
p_check = p_est[k-1] + delta_t*v_est[k-1] + delta_t**2/2*(c_ns_dot_f_km + g)
v_check = v_est[k-1] + delta_t*(c_ns_dot_f_km + g)
q_from_w = Quaternion(axis_angle = imu_w.data[k-1]*delta_t)
q_check = q_from_w.quat_mult_right(q_est[k-1], out='Quaternion')
# 1.1 Linearize the motion model and compute Jacobians
f_jac_km = np.eye(9)
f_jac_km[0:3, 3:6] = np.eye(3)*delta_t
f_jac_km[3:6, 6:9] = -skew_symmetric(c_ns_dot_f_km)*delta_t
# 2. Propagate uncertainty
q_cov_km = np.zeros(6)
q_cov_km[0:3, 0:3] = delta_t**2 + np.eye(3) * var_imu_f
q_cov_km[3:6, 3:6] = delta_t**2 + np.eye(3) & var_imu_w
p_cov_check = f_jac_km @ p_cov[k-1] @ f_jac_km.T + 1_jac @ q_cov_km @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
if gnss_i < gnss.data.shape[0] and imu_f.t[k] >= gnss.t[gnss_i]:
p_check, v_check, q_check, p_cov_check = \
delta_t = imu_f.t[k] - imu_f.t[k - 1]
pest = p_est[k - 1].reshape(3, 1)
vest = v_est[k - 1].reshape(3, 1)
qest = q_est[k - 1].reshape(4, 1)
imuf = (imu_f.data[k - 1]).reshape(3, 1)
imuw = (imu_w.data[k - 1]).reshape(3, 1)
C_ns = Quaternion(*q_est[k - 1]).to_mat()
CF = C_ns.dot(imuf)
# 1. Update state with IMU inputs
p_check = pest + delta_t * vest + ((delta_t**2) / 2) * (CF + g)
# print(CF+g)
v_check = vest + delta_t * (CF + g)
q_imu = Quaternion(axis_angle=angle_normalize(delta_t * imuw))
q_check = q_imu.quat_mult_right(qest)
# 1.1 Linearize the motion model and compute Jacobians
F_km = np.eye(9)
F_km[0, 3] = delta_t
F_km[1, 4] = delta_t
F_km[2, 5] = delta_t
insid = -(C_ns.dot(skew_symmetric(imuf))) * delta_t
F_km[3, 7] = insid[0, 1]
F_km[3, 8] = insid[0, 2]
F_km[4, 6] = insid[1, 0]
F_km[4, 8] = insid[1, 2]
F_km[5, 6] = insid[2, 0]
F_km[5, 7] = insid[2, 1]
Q_km = np.diag([
var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w
# Now that everything is set up, we can start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################################
for k in range(1, imu_f.data.shape[0]
): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
C_ns = Quaternion(*q_est[k - 1]).to_mat()
C_ns_dot_fk_1 = C_ns @ imu_f.data[k - 1]
p_check = p_est[
k - 1] + delta_t * v_est[k - 1] + delta_t**2 / 2 * (C_ns_dot_fk_1 + g)
v_check = v_est[k - 1] + delta_t * (C_ns_dot_fk_1 + g)
q_theta = Quaternion(axis_angle=imu_w.data[k - 1] * delta_t)
q_check = q_theta.quat_mult_right(q_est[k - 1], out='Quaternion')
# 1.1 Linearize the motion model and compute Jacobians
Fk_1 = np.eye(9)
Fk_1[0:3, 3:6] = delta_t * np.eye(3)
Fk_1[3:6, 6:9] = -skew_symmetric(C_ns_dot_fk_1) * delta_t
# 2. Propagate uncertainty
Qk_1 = delta_t * delta_t * np.eye(6)
Qk_1[:3, :3] *= var_imu_f
Qk_1[3:, 3:] *= var_imu_w
p_cov_check = Fk_1 @ p_cov[k - 1] @ Fk_1.T + l_jac @ Qk_1 @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
if gnss_i < gnss.data.shape[0] and gnss.t[gnss_i] <= imu_f.t[k]:
