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]: