#### 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 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
#### 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 #calculating rotation matrix quat = Quaternion(q_est[k - 1, 0], q_est[k - 1, 1], q_est[k - 1, 2], q_est[k - 1, 3]) Cns = quat.to_mat() temp = Cns.dot(imu_f.data[k - 1]) t3 = np.add(temp, g) t2 = delta_t * v_est[k - 1] t1 = (0.5 * delta_t**2) * t3 p_check = np.add(p_est[k - 1], np.add(t2, t1)) #updating p v_check = np.add(v_est[k - 1], delta_t * t3) #updating v q_euler = delta_t * imu_w.data[k - 1] q_check = Quaternion(euler=q_euler).quat_mult_right(q_est[k - 1]) #updating q # 1.1 Linearize the motion model and compute Jacobians F = np.identity(9) F[0:3, 3:6] = delta_t * np.eye(3)
################################################################################################ # 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] p_check = p_est[k-1] v_check = v_est[k-1] q_check = q_est[k-1] p_cov_check = p_cov[k-1] # 1. Update state with IMU inputs # convert q_check numpy array to q_check_quat Quaternion object to call to_mat q_check_quat = Quaternion(w=q_check[0], x=q_check[1], y=q_check[2], z=q_check[3], axis_angle=None, euler=None) C_ns = q_check_quat.to_mat() p_check = p_check + delta_t * v_check + (0.5 * delta_t**2)*(C_ns.dot(imu_f.data[k - 1]) + g) v_check = v_check + delta_t * (C_ns.dot(imu_f.data[k - 1]) + g) q_check = Quaternion(euler=imu_w.data[k-1]*delta_t).quat_mult_right(q_check) # 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] = -delta_t * skew_symmetric(C_ns.dot(imu_f.data[k - 1])) #IMU process/motion model has accelerometer(a_x,a_y,a_z) and gyroscope(w_x,w_y,w_z) 6 degrees of uncorrelated uncertainty
# Now that everything is set up, we can start taking in the sensor data and creating estimates # for our state in a loop. ################################################################################################ last_gnss_idx = 0 last_lidar_idx = 0 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] 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)
def ekf(self): """ Main loop of the extended Kalman filter sensor fusion procedure. """ lid_i, lid_state = self.initialize_lidar() t = self.imu_t[0] # initialize time step st = round(time.time()) # initialize runtime print("Initiating Kalman filter loop...") for k in range(1, self.n): # Progress report if k % 100 == 0: tm_new = round(time.time()) tm_el = tm_new - st tm_rm = (self.n - k) * tm_el // k print( "Iteration {0}/{1}: Time elapsed {2}m{3}sec - Est. time remaining {4}m{5}sec" .format(k, self.n, tm_el // 60, tm_el % 60, tm_rm // 60, tm_rm % 60)) delta_t = (self.imu_t[k] - self.imu_t[k - 1]) # Assign previous state p_km = self.p_est[k - 1].reshape(3, 1) v_km = self.v_est[k - 1].reshape(3, 1) q = self.imu_q[k, :].reshape(4, 1) del_u_km = self.del_u_est[k - 1].reshape(6, 1) quat = Quaternion(*q).normalize() C_ns = quat.to_mat() # Assign control inputs and calibrate with estimated sensor error f_km = self.imu_f[k - 1].reshape(3, 1) - del_u_km[:3] w_km = self.imu_w[k - 1].reshape(3, 1) - del_u_km[3:] # Update state p_check = p_km + delta_t * v_km + delta_t**2 / 2 * ( C_ns.dot(f_km) - self.g.reshape(3, 1)) v_check = v_km + delta_t * (C_ns.dot(f_km) - self.g.reshape(3, 1)) q_check = q del_u_check = del_u_km # Linearize motion model F, L = SLAM.state_space_model(f_km, C_ns, delta_t) # Propagate uncertainty p_cov_km = self.p_cov[k - 1, :, :] p_cov_check = F.dot(p_cov_km).dot(F.T) + L.dot(self.Q_imu).dot(L.T) # Check availability of LIDAR measurements if lid_i != len(self.lid_r) and t >= self.lid_t[lid_i]: if self.algorithm == "icp": y_k = SLAM.icp_state(self.gf, self.lid_r[lid_i], lid_state) if self.algorithm == "feature": y_k = SLAM.feature_state(self.lid_r[lid_i - 1], self.lid_r[lid_i], lid_state) new_state = SLAM.measurement_update(y_k, p_check, v_check, q_check, del_u_check, p_cov_check, self.R_lid) p_check, v_check, q_check, p_cov_check, del_u_check, lid_state = new_state if self.algorithm == "feature": x_hat = p_check[0, 0] y_hat = p_check[1, 0] yaw_hat = Quaternion(*q_check).normalize().to_euler()[2, 0] update_map(self.gf, self.lid_r[lid_i], x_hat, y_hat, yaw_hat) lid_i += 1 # Store into state and uncertainty arrays self.p_est[k, :] = p_check.reshape(3, ) self.v_est[k, :] = v_check.reshape(3, ) self.q_est[k, :] = q_check.reshape(4, ) self.del_u_est[k, :] = del_u_check.reshape(6, ) self.p_cov[k, :, :] = p_cov_check # Increment time t += delta_t # Report finished progress tm_el = round(time.time()) - st print("Finished iterating after {0}m{1}sec".format( tm_el // 60, tm_el % 60))