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_f = C_ns @ imu_f.data[k-1]
theta = angle_normalize(imu_w.data[k-1]*delta_t)
p_check = p_est[k-1] + delta_t*v_est[k-1] + 0.5*(delta_t**2)*(C_f + g)
v_check = v_est[k-1] + delta_t*(C_f + g)
q_check = Quaternion(axis_angle=theta).quat_mult_right(q_est[k-1])
# 1.1 Linearize the motion model and compute Jacobians
F_k_1 = np.eye(9)
F_k_1[0:3,3:6] = delta_t*np.eye(3)
F_k_1[3:6,6:9] = -(skew_symmetric(C_ns@imu_f.data[k-1].reshape((3,1))))*delta_t
Q = np.eye(6)
Q[0:3,0:3] = var_imu_f*np.eye(3)
Q[3:6,3:6] = var_imu_w*np.eye(3)
Q = (delta_t**2)*Q
# 2. Propagate uncertainty
p_cov_check = (F_k_1 @ p_cov[k-1] @ F_k_1.T) + (l_jac @ Q @ l_jac.T)
# 3. Check availability of GNSS and LIDAR measurements
if (gnss_available(gnss_i) is True and lidar_available(lidar_i) is False):
y_k = gnss.data[gnss_i]
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_gnss, p_cov_check, y_k, p_check, v_check, q_check)
gnss_i = gnss_i + 1
p_check = p + delta_t * v + 0.5 * delta_t**2 * (C_ns @ imu_f.data[k - 1] +
g)
#print('p_check:', p_check, 'p_check shape:', np.shape(p_check))
v_check = v + delta_t * (C_ns @ imu_f.data[k - 1] + g)
#print('v_check:', v_check, 'v_check shape:', np.shape(v_check))
q_check = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_left(q)
#print('q_check:', q_check, 'q_check shape:', np.shape(q_check))
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
F[0:3, 3:6] = np.eye(3) * delta_t
F[3:6, 6:9] = -C_ns @ skew_symmetric(imu_f.data[k - 1]) * delta_t
#print("F matrix:", F, "shape:", np.shape(F))
# 2. Propagate uncertainty
L = l_jac
Q = delta_t**2 * np.diag(
[var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w])
P_cov_check = F @ p_cov[k - 1] @ F.T + L @ Q @ L.T
#print("P_cov_check:", P_cov_check, "P_cov_check shape:", np.shape(P_cov_check))
# 3. Check availability of GNSS and LIDAR measurements
time_stamp = imu_f.t[k]
gnss_time_set = gnss.t
lidar_time_set = lidar.t
if time_stamp in gnss_time_set:
# 1.1 Linearize the motion model and compute Jacobians
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (delta_t**2 / 2) * (
rot_mat @ imu_f.data[k - 1] + g) # Position
v_est[k] = v_est[k - 1] + delta_t * (rot_mat @ imu_f.data[k - 1] + g
) # Velocity
q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_right(q_est[k - 1]) # Orientation
#q_est[k] = Quaternion(euler=imu_w.data[k-1]*delta_t).quat_mult_right(q_est[k-1]) # Orientation
# 2. Propagate uncertainty
F = np.identity(9)
Q = np.identity(6)
F[:3, 3:6] = delta_t * np.identity(3)
#F[3:6, 6:] = -(rot_mat @ skew_symmetric(imu_f.data[k-1].reshape((3,1))))
F[3:6, 6:] = -(rot_mat @ skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1)))) * delta_t # don't miss delta_t
Q[:, :3] *= delta_t**2 * var_imu_f
Q[:, -3:] *= delta_t**2 * var_imu_w
p_cov[k] = F @ p_cov[k - 1] @ F.T + l_jac @ Q @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
if lidar_i < lidar.t.shape[0] and abs(lidar.t[lidar_i] -
imu_f.t[k - 1]) < 0.01:
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(
var_lidar, p_cov[k], lidar.data[lidar_i], p_est[k], v_est[k],
q_est[k])
lidar_i += 1
if gnss_i < gnss.t.shape[0] and abs(gnss.t[gnss_i] -
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# pre-definitions
c_ns = Quaternion(*q_est[k - 1]).to_mat()
# 1. Update state with IMU inputs
p_check = p_est[k - 1] + (delta_t * v_est[k - 1]) + (delta_t**2 / 2) * (
c_ns.dot(imu_f.data[k - 1]) + g)
v_check = v_est[k - 1] + (delta_t * (c_ns.dot(imu_f.data[k - 1]) + g))
q_check = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_right(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
F[0:3, 3:6] = np.eye(3) * delta_t
F[3:6, 6:9] = -(skew_symmetric(c_ns.dot(imu_f.data[k - 1])) * delta_t)
# 2. Propagate uncertainty
Q = np.diag([
var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w
]) * delta_t**2
p_cov_check = (F.dot(p_cov[k - 1].dot(F.T))) + (l_jac.dot(Q.dot(l_jac.T)))
# 3. Check availability of GNSS and LIDAR measurements
if gnss_i < 55 and gnss.t[gnss_i] == imu_f.t[k - 1]:
p_check, v_check, q_check, p_cov_check = measurement_update(
var_gnss, p_cov_check, gnss.data[gnss_i], p_check, v_check,
q_check)
gnss_i = gnss_i + 1
if lidar_i < 512 == imu_f.t[k - 1] and lidar.t[lidar_i]:
p_check, v_check, q_check, p_cov_check = measurement_update(
