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(