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