def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check):
# 3.1 Compute Kalman Gain
K_k=p_check@h_jac.T@(h_jac@p_check@h_jac.T+sensor_var)
# 3.2 Compute error state
delta_x=K_k@(y_k-p_check)
# 3.3 Correct predicted state
p_hat=p_check+delta_x[0]
v_hat=v_check+delta_x[1]
q_hat=Quaternion.quat_mult_left(q_check,out="np")
# 3.4 Compute corrected covariance
p_cov_hat=(np.eye(3)-K_k@h_jac)@p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check,
q_check):
# 3.1 Compute Kalman Gain
R = np.eye(3) * sensor_var
# 9x9 @ 9x3 @ (3x9 @ 9x9 @ 9x3 + 3x3)
K = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T +
R)
# 3.2 Compute error state
Dx = K @ (y_k - p_check)
# 3.3 Correct predicted state
p_hat = p_check + Dx[0:3]
v_hat = v_check + Dx[3:6]
q_Phi = Quaternion(axis_angle=Dx[6:9])
q_hat = q_Phi.quat_mult_left(q_check, out='Quaternion')
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - K @ h_jac) @ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_check, y_m, p_check, v_check, q_check):
# Do some intialization
P_k = p_cov_check
#print("P_k: ", P_k)
# 3.1 Compute Kalman Gain
#H_k = np.zeros([3,9])
#H_k[:,0:3] = np.eye(3)
H_k = h_jac
M_k = np.eye(3)
R_k = sensor_var * np.eye(3)
K_k = P_k @ H_k.T @ np.linalg.inv(H_k @ P_k @ H_k.T + R_k)
#print("K_k: ", K_k)
# 3.2 Compute error state
y_k = p_check
dx_k = K_k @ (y_m - y_k).T
print("dx_k: ", dx_k)
# 3.3 Correct predicted state
p_hat = p_check + dx_k[:3]
v_hat = v_check + dx_k[3:6]
q = Quaternion(axis_angle = dx_k[6:] )
q_hat = q.quat_mult_left(q_check)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - K_k @ H_k) @ P_k
return p_hat, v_hat, q_hat, p_cov_hat
################################################################################################
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
q_prev = Quaternion(
*q_est[k - 1, :]) # previous orientation as a quaternion object
q_curr = Quaternion(axis_angle=(imu_w.data[k - 1] *
delta_t)) # current IMU orientation
c_ns = q_prev.to_mat() # previous orientation as a matrix
f_ns = (c_ns @ imu_f.data[k - 1]) + g # calculate sum of forces
p_check = p_est[k - 1, :] + delta_t * v_est[k - 1, :] + 0.5 * (delta_t**
2) * f_ns
v_check = v_est[k - 1, :] + delta_t * f_ns
q_check = q_prev.quat_mult_left(q_curr)
# 1.1 Linearize the motion model and compute Jacobians
f_jac = np.eye(9) # motion model jacobian with respect to last state
f_jac[0:3, 3:6] = np.eye(3) * delta_t
f_jac[3:6, 6:9] = -skew_symmetric(c_ns @ imu_f.data[k - 1]) * delta_t
# 2. Propagate uncertainty
q_cov = np.zeros((6, 6)) # IMU noise covariance
q_cov[0:3, 0:3] = delta_t**2 * np.eye(3) * var_imu_f
q_cov[3:6, 3:6] = delta_t**2 * np.eye(3) * var_imu_w
p_cov_check = f_jac @ p_cov[k -
1, :, :] @ f_jac.T + l_jac @ q_cov @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
if imu_f.t[k] in gnss_t:
delta_t = imu_f.t[k] - imu_f.t[k - 1]
curr_t = imu_f.t[k]
f_k_1 = imu_f.data[k - 1]
w_k_1 = imu_w.data[k - 1]
p_k_1 = p_est[k - 1]
v_k_1 = v_est[k - 1]
q_k_1 = Quaternion(q_est[k - 1, 0], q_est[k - 1, 1], q_est[k - 1, 2],
q_est[k - 1, 3])
Cns = q_k_1.to_mat()
# 1. Update state with IMU inputs
p_check = p_k_1 + delta_t * v_k_1 + (delta_t**2) / 2 * (Cns @ f_k_1 + g)
v_check = v_k_1 + delta_t * (Cns @ f_k_1 + g)
q_check = q_k_1.quat_mult_left(Quaternion(axis_angle=(w_k_1 * delta_t)))
# 1.1 Linearize the motion model and compute Jacobians
F_k_1 = np.identity(9)
F_k_1[0:3, 3:6] = np.identity(3) * delta_t
F_k_1[3:6, 6:9] = -1 * skew_symmetric(Cns @ f_k_1) * delta_t
# 2. Propagate uncertainty
Q_k = np.identity(6)
Q_k[0:3, 0:3] = delta_t**2 * var_imu_f * np.identity(3)
Q_k[3:6, 3:6] = delta_t**2 * var_imu_w * np.identity(3)
p_cov_check = F_k_1 @ p_cov[k - 1] @ F_k_1.T + l_jac @ Q_k @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
gnss_idx = find_closest_measurement(curr_t, gnss.t, max(last_gnss_idx, 0))
lidar_idx = find_closest_measurement(curr_t, lidar.t,
# for our state in a loop.
################################################################################################
from termcolor import colored
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_check).to_mat()
p_check += delta_t * v_check + delta_t**2*(C_ns@imu_f.data[k] + np.array([0, 0, -9.8]))/2
v_check += delta_t*(C_li@imu_f.data[k] + np.array([0, 0, -9.8]))
q = Quaternion(euler=imu_w.data[k] * delta_t)
q_check = q.quat_mult_left(q_check)
# 1.1 Linearize the motion model and compute Jacobians
F_k = np.eye(9)
F_k[:3,3:6] = np.eye(3) * delta_t
F_k[3:6,6:] = -skew_symmetric(C_ns @ imu_f.data[k]) * delta_t
#L_k = np.zeros([9,6])
#L_k[3:6,:3] = np.eye(3)
#L_k[6:,3:] = np.eye(3)
L_k = l_jac
# 2. Propagate uncertainty
Q_k = np.zeros([6,6])
Q_k[:3,:3] = var_imu_f * delta_t**2 * np.eye(3)
Q_k[3:,3:] = var_imu_w * delta_t**2 * np.eye(3) #input noise convariance
