#### 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 nominal state with IMU inputs Rotation_Mat = Quaternion(*q_est[k - 1]).to_mat() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t**2) * ( Rotation_Mat.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[k - 1] + delta_t * (Rotation_Mat.dot(imu_f.data[k - 1]) - g) q_est[k] = Quaternion(euler=delta_t * imu_w.data[k - 1]).quat_mult( q_est[k - 1]) # 1.1 Linearize Motion Model and compute Jacobians F = np.eye(9) imu = imu_f.data[k - 1].reshape((3, 1)) F[0:3, 3:6] = delta_t * np.eye(3) F[3:6, 6:9] = Rotation_Mat.dot(-skew_symmetric(imu)) * delta_t # 2. Propagate uncertainty Q = np.eye(6) Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3] Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
Cns @ imu_f.data[k - 1] + g) #Update position v_est[k] = v_est[k - 1] + delta_t * (Cns @ imu_f.data[k - 1] + g ) #Update velocity # q_est[k] = Quaternion(euler = imu_w.data[k-1]*delta_t).quat_mult_right(q_est[k-1]) q_est[k] = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right( q_est[k - 1]) #Update orientation # 1.1 Linearize the motion model and compute Jacobians F = np.eye( 9 ) # Linearizing motion model jacobian. It has 9 states and thus there are 9 differentiables F[0:3, 3:6] = delta_t * np.eye( 3 ) # Based on the position jacobian when differentiated w.r.t velocities # F[3:6,6:9] = -skew_symmetric(Cns@imu_f.data[k-1])*delta_t # This representation is wrong F[3:6, 6:] = -(Cns.dot(skew_symmetric(imu_f.data[k - 1].reshape( (3, 1))))) * delta_t #relation between velocity and orientation Q = np.eye( 6 ) #IMU has 6 inputs of specific force and rotational velocity respectively Q[0:3, 0:3] *= delta_t**2 * var_imu_f #Assignning specific force variance Q[3:6, 3:6] *= delta_t**2 * var_imu_w #Assigning rotational velocity variance # 2. Propagate uncertainty/predicted error covariance p_cov[k] = F @ p_cov[ k - 1] @ F.T + l_jac @ Q @ l_jac.T #(9*9).(9*9).(9*9) + (9*6).(6*6).(6*9) eventually yields a 1*9*9 vector # print(np.shape(p_cov)) # 3. Check availability of GNSS and LIDAR measurements # Check 1: If both lidar and gnss data are available and their counters are less than the maximum number of measurements available
#### 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() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + ( (delta_t**2) / 2) * (C_ns.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[k - 1] + (C_ns.dot(imu_f.data[k - 1]) + g) * delta_t q_est[k] = 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_k_1 = np.eye(9) F_k_1[:3, 3:6] = delta_t * np.eye(3) F_k_1[3:6, 6:9] = -(C_ns @ skew_symmetric(imu_f.data[k - 1].reshape( (3, 1)))) * delta_t Q_k_1 = (delta_t**2) * np.diag( [var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w]) # 2. Propagate uncertainty p_cov[k] = F_k_1.dot(p_cov[k - 1]).dot(F_k_1.T) + l_jac.dot(Q_k_1).dot( l_jac.T) # 3. Check availability of GNSS and LIDAR measurements if lidar_i < lidar.t.shape[0] and lidar.t[lidar_i] <= imu_f.t[k - 1]:
#### 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 # - Update Position, Velocity, Orientation respectively. C_ns = Quaternion(*q_est[k - 1]).to_mat() c_term = C_ns.dot(imu_f.data[k - 1]) + g # print("Q_est={},\n mat={},\n iumu_f[k-1]={}".format(Q, Q.to_mat(),imu_f.data[k - 1])) # print("c_term = {}".format(c_term)) p_check = p_est[k - 1] + delta_t * v_est[k - 1] + (0.5 * (delta_t**2)) * c_term v_check = v_est[k - 1] + delta_t * c_term # q_check = Quaternion(*q_est[k-1]).quat_mult_right(Quaternion(axis_angle=(imu_w.data[k-1] * delta_t))) theta = angle_normalize(imu_w.data[k - 1] * delta_t) q_check = Quaternion(euler=theta).quat_mult_left(Quaternion(*q_est[k - 1])) q_check = Quaternion( euler=angle_normalize(Quaternion(*q_check).to_euler())).to_numpy() # 1.1 Linearize the motion model and compute Jacobians F_km1 = np.zeros((9, 9))
# 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, len( imu_f.data[:, 0])): # start at 1 b/c we have initial prediction from gt 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]
#### 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] #print(*q_est[k-1]) rotation_matrix = Quaternion(*q_est[k - 1]).to_mat() # 1. Update state with IMU inputs p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (delta_t**2 / 2) * ( rotation_matrix.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[ k - 1] + delta_t * (rotation_matrix.dot(imu_f.data[k - 1]) + g) q_est[k] = 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 # 2. Propagate uncertainty F = np.identity(9) Q = np.identity(6) F[:3, 3:6] = delta_t * np.identity(3) F[3:6, 6:] = -(rotation_matrix.dot( skew_symmetric(imu_f.data[k - 1].reshape((3, 1))))) Q[:, :3] *= delta_t**2 * var_imu_f
def main(): path = 'data/pt1_data.pkl' gt, imu_a, imu_w, lidar, gnss = getData(path) C_li, t_i_li = calibrationRotMatrix() #transform the lidar data to inertial frame lidar.data = (C_li @ lidar.data.T).T + t_i_li #lidar index lidar_idx = 0 #gnss index gnss_idx = 0 ##pre_computed matrix I = np.identity(3) #pertubation matrix L_jac = np.zeros((18, 12)) L_jac[3:15, :] = np.identity(12) # save the estimated result p_est = np.zeros([imu_a.data.shape[0], 3]) # position estimates v_est = np.zeros([imu_a.data.shape[0], 3]) # velocity estimates q_est = np.zeros([imu_a.data.shape[0], 4]) # orientation estimates as quaternions a_bias_est = np.zeros([imu_a.data.shape[0], 3]) #acc bias estimation w_bias_est = np.zeros([imu_a.data.shape[0], 3]) #w bias estimation g_est = np.zeros([imu_a.data.shape[0], 3]) p_cov = np.zeros([imu_a.data.shape[0], 18, 18]) # covariance matrices at each timestep # intial state p_est[0] = gt.p[0] v_est[0] = gt.v[0] q_est[0] = Quaternion(euler=gt.r[0]).to_numpy() #bias are initially set to 0 g = np.array([0, 0, -9.81]) # gravity g_est[0] = g p_cov[0] = np.zeros(18) # covariance of estimate #start the motion prediction for k in range(1, imu_a.data.shape[0]): # compute dt delta_t = imu_a.t[k] - imu_a.t[k - 1] # computed transform matrix to the common/navigayion frame rotation_matrix = Quaternion(*q_est[k - 1]).to_mat() # step 1 motion propogation using nonlinear function for normial state p_est[k] = p_est[k - 1] + v_est[k - 1] * delta_t + ( 0.5 * delta_t**2) * (rotation_matrix.dot( imu_a.data[k - 1] - a_bias_est[k - 1]) + g_est[k - 1]) v_est[k] = v_est[k - 1] + delta_t * (rotation_matrix.dot( imu_a.data[k - 1] - a_bias_est[k - 1]) + g_est[k - 1]) q_est[k] = Quaternion( axis_angle=(imu_w.data[k - 1] - w_bias_est[k - 1]) * delta_t).quat_mult_right(q_est[k - 1]) a_bias_est[k] = a_bias_est[k - 1] w_bias_est[k] = w_bias_est[k - 1] g_est[k] = g_est[k - 1] #step 2 covariance propogation F_m = np.identity(18) F_m[0:3, 3:6] = delta_t * I F_m[3:6, 6:9] = -(delta_t * rotation_matrix.dot( skew_symmetric(imu_a.data[k - 1] - a_bias_est[k - 1]))) F_m[3:6, 9:12] = -(delta_t * rotation_matrix) F_m[3:6, 15:18] = delta_t * I F_m[6:9, 12:15] = -(delta_t * rotation_matrix) Q = np.zeros((12, 12)) Q[0:3, 0:3] = (var_imu_f * delta_t**2) * I Q[3:6, 3:6] = (var_imu_w * delta_t**2) * I Q[6:9, 6:9] = (var_bias_f * delta_t**2) * I Q[9:12, 9:12] = (var_bias_w * delta_t**2) * I p_cov[k] = F_m.dot(p_cov[k - 1]).dot(F_m.T) + L_jac.dot(Q).dot(L_jac.T) #step3 measurement correction if lidar_idx < lidar.data.shape[0] and imu_a.t[ k - 1] == lidar.t[lidar_idx]: p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[ k], p_cov[k] = measurement_update(var_lidar, p_cov[k], lidar.data[lidar_idx], p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[k]) lidar_idx += 1 if gnss_idx < gnss.data.shape[0] and imu_a.t[k - 1] == gnss.t[gnss_idx]: p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[ k], p_cov[k] = measurement_update(var_gnss, p_cov[k], gnss.data[gnss_idx], p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[k]) gnss_idx += 1 #convert from quaternion to euler angle euler_est = np.zeros((q_est.shape[0], 3)) for i in range(len(q_est)): euler_est[i] = Quaternion(*q_est[i]).to_euler() #convert to degree euler_est = 180 / np.pi * euler_est plotResult(gt.p, gt.v, gt.r, p_est, v_est, euler_est, a_bias_est, w_bias_est, g_est, imu_a.t, gt._t) # evaluate rmse of pos rmse_px, rmse_py, rmse_pz = rmse(gt.p, p_est) print(rmse_px, rmse_py, rmse_pz) # evalue rmse of velocity rmse_vx, rmse_vy, rmse_vz = rmse(gt.v, v_est) print(rmse_vx, rmse_vy, rmse_vz)
#### 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 nominal state with IMU inputs C_n = Quaternion(*q_est[k - 1]).to_mat() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t**2) * ( C_n.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[k - 1] + delta_t * (C_n.dot(imu_f.data[k - 1]) - g) q_est[k] = Quaternion(euler=delta_t * imu_w.data[k - 1]).quat_mult( q_est[k - 1]) # 1.1 Linearize Motion Model and compute Jacobians F = np.eye(9) imu = imu_f.data[k - 1].reshape((3, 1)) F[0:3, 3:6] = delta_t * np.eye(3) F[3:6, 6:9] = C_n.dot(-skew_symmetric(imu)) * delta_t # 2. Propagate uncertainty Q = np.eye(6) Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3] Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
def get_imu_data(self, stby=700, offset_quat=True, gravity_bias_est=False): """ Loads, cleans and assigns IMU data to object attributes. Args: stby (int): Number of initial IMU readings (standby readings) before first movement. Default: 700. offset_quat (bool): If True, the quaternion readings will be offset such that initial heading is zero. Default: True. gravity_bias_est (bool): If True, the initial bias estimation, including estimating gravity component in the z-direction, will be done by averaging across the first ´stby´ number of IMU readings. Default: False """ imu = pd.read_csv( self.imu_file, usecols=[ "field.header.stamp", "field.orientation.x", "field.orientation.y", "field.orientation.z", "field.orientation.w", "field.angular_velocity.x", "field.angular_velocity.y", "field.angular_velocity.z", "field.linear_acceleration.x", "field.linear_acceleration.y", "field.linear_acceleration.z" ]) imu.columns = [ "timestamp", "q_x", "q_y", "q_z", "q_w", "om_x", "om_y", "om_z", "a_x", "a_y", "a_z" ] # Assign arrays for timestamp, linear acceleration and rotational velocity self.imu_t = np.round( imu.timestamp.values / 1e9, 3) # converting to seconds with three decimal places self.imu_f = imu[["a_x", "a_y", "a_z"]].values self.imu_w = imu[["om_x", "om_y", "om_z"]].values self.imu_q = imu[["q_w", "q_x", "q_y", "q_z"]].values self.n = self.imu_f.shape[0] # number of IMU readings if offset_quat: # Transform quaternions to Euler angles phi = np.ndarray((self.n, 3)) for i in range(self.n): phi[i, :] = Quaternion( *self.imu_q[i, :]).normalize().to_euler() # Shift yaw angle such that the inital yaw equals zero inityaw = np.mean(phi[:stby, 2]) # Estimated initial yaw tf_yaw = wraptopi(phi[:, 2] - inityaw) # Transformed yaw # Transform Euler angles back to quaternions phi[:, 2] = tf_yaw for i in range(self.n): self.imu_q[i, :] = Quaternion( euler=phi[i, :]).normalize().to_numpy() if gravity_bias_est: imu_f_trans = np.ndarray((stby, 3)) for i in range(stby): C_ns = Quaternion(*self.imu_q[i, :]).normalize().to_mat() imu_f_trans[i] = C_ns.dot(self.imu_f[i, :]) self.g = np.mean(imu_f_trans, 0) # bias + gravity else: self.g = np.array([0, 0, 9.80665]) # gravity
p_last = p_est[k - 1] # last position v_last = v_est[k - 1] # last velocity q_last = q_est[k - 1] # last orientation f_last = imu_f.data[k - 1] # last force calculated by IMU w_last = imu_w.data[k - 1] # last rotational rate calculated bu IMU p_cov_last = p_cov[k - 1] # last covariance matrix # 1. Update state with IMU inputs # The quaternion will keep track of the orientation of our sensor frame S with respect to navigation frame N Cns = Quaternion(*q_last).to_mat() # 1.1 Linearize the motion model and compute Jacobians p_predict = p_last + delta_t * v_last + ( (delta_t**2) / 2) * (Cns.dot(f_last) + g) v_predict = v_last + delta_t * (Cns.dot(f_last) + g) q_predict = Quaternion(axis_angle=w_last * delta_t).quat_mult_right(q_last) # 2. Propagate uncertainty F_last = np.eye(9) # F Jacobian matrix; L is constant F_last[:3, 3:6] = np.eye(3) * delta_t F_last[3:6, 6:9] = -skew_symmetric(Cns @ f_last) * delta_t # f, w location (Covariance of measurement noise) Q_last = np.eye(6) Q_last[:3, :3] *= (delta_t**2) * var_imu_f Q_last[-3:, -3:] *= (delta_t**2) * var_imu_w # We may perform this propagation step multiple times before a measurement arrives from either GNSS receiver or the LIDAR sensor p_cov_predict = F_last @ p_cov_last @ F_last.T + l_jac @ Q_last @ l_jac.T
#### 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 rotation_matrix = Quaternion(*q_est[k - 1]).to_mat() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (delta_t**2 / 2) * ( rotation_matrix.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[ k - 1] + delta_t * (rotation_matrix.dot(imu_f.data[k - 1]) + g) q_est[k] = Quaternion(euler=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) imu = imu_f.data[k - 1].reshape((3, 1)) F[0:3, 3:6] = delta_t * np.eye(3) F[3:6, 6:9] = rotation_matrix.dot(-skew_symmetric(imu)) * delta_t # 2. Propagate uncertainty Q = np.eye(6) Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3] Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
# 1. Update state with IMU inputs C_ns = Quaternion(*q_est[k - 1]).to_mat() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + ( (delta_t**2) / 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_est[k] = 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 # 2. Propagate uncertainty F_jac = np.eye(9) F_jac[:3, 3:6] = np.eye(3) * delta_t F_jac[3:6, 6:] = -(C_ns.dot(skew_symmetric(imu_f.data[k - 1].reshape((3, 1))))) #F_jac[3:6, 6:] = -skew_symmetric(C_ns @ imu_f.data[k-1]) * delta_t Q_jac = np.eye(6) Q_jac[:3, :3] = var_imu_f * delta_t**2 * np.eye(3) Q_jac[3:, 3:] = var_imu_w * delta_t**2 * np.eye(3) p_cov[k] = F_jac @ p_cov[k - 1] @ F_jac.T + l_jac @ Q_jac @ l_jac.T # 3. Check availability of GNSS and LIDAR measurements # Check that we didn't reach end of LIDAR data and that current LIDAR timestamp is close to that of IMU if lidar_i < lidar.t.shape[0] and lidar.t[lidar_i] <= imu_f.t[k]: 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 gnss.t[gnss_i] <= imu_f.t[k]:
return p_hat, v_hat, q_hat, p_cov_hat #### 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 rotation_matrix = Quaternion(*q_est[k-1]).to_mat() # 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)*(rotation_matrix.dot(imu_f.data[k-1]) + g) v_est[k] = v_est[k-1] + delta_t*(rotation_matrix.dot(imu_f.data[k-1]) + g) q_est[k] = Quaternion(axis_angle=imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1]) # 2. Propagate uncertainty F = np.identity(9) Q = np.identity(6) F[:3, 3:6] = delta_t * np.identity(3) # F[3:6, 6:] = -skew_symmetric(rotation_matrix.dot(imu_f.data[k-1])) * delta_t F[3:6, 6:] = -(rotation_matrix.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1))))) # Q[:3, :3] = var_imu_f * delta_t**2 * np.identity(3) # Q[3:, 3:] = var_imu_w * delta_t**2 * np.identity(3) Q[:, :3] *= delta_t**2 * var_imu_f Q[:, -3:] *= delta_t**2 * var_imu_w p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)
#### 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]).normalize().to_mat() 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=delta_t * imu_w.data[k - 1]).normalize().quat_mult(q_est[k - 1]) q_check = Quaternion(*q_check).normalize().to_numpy() # 1.1 Linearize Motion Model Fk = np.eye(9) #1 Fk[0:3, 3:6] = np.eye(3) * delta_t #2 Fk[3:6, 6:9] = -skew_symmetric(C_ns.dot( imu_f.data[k - 1])) * delta_t #3print(Fk) Qk = np.zeros((6, 6)) Qk[0:3, 0:3] = (delta_t**2) * var_imu_f * np.identity(3) Qk[3:6, 3:6] = (delta_t**2) * var_imu_w * np.identity(3) # 2. Propagate uncertainty
): # 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() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + delta_t**2 / 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_est[k] = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right( q_est[k - 1]) q_est[k] = Quaternion(*q_est[k]).normalize().to_numpy() # 1.1 Linearize the motion model and compute Jacobians f_jac = np.eye(9) f_jac[0:3, 3:6] = np.eye(3) * delta_t f_jac[3:6, 6:9] = -c_ns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t # 2. Propagate uncertainty p_cov[k] = f_jac @ p_cov[k - 1] @ f_jac.T + l_jac @ (Q * delta_t * delta_t) @ l_jac.T # 3. Check availability of GNSS and LIDAR measurements # if 0: if gnss_i < len(gnss.t): if abs(gnss.t[gnss_i] - imu_f.t[k]) <= 1e3: y_k = gnss.data[gnss_i] p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update( var_gnss, p_cov[k], y_k, p_est[k], v_est[k], q_est[k]) gnss_i += 1 if 0: # if lidar_i < len(lidar.t):
#### 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() p_check = p_est[k - 1] + v_est[k - 1] * delta_t + 0.5 * ( C_ns.dot(imu_f.data[k - 1]) + g) * delta_t**2 #proveri je l ide plus ili minus g v_check = v_est[k - 1] + (C_ns.dot(imu_f.data[k - 1]) + g) * delta_t tempq = Quaternion(euler=imu_w.data[k - 1] * delta_t) q_check = Quaternion(*q_est[k - 1]).quat_mult_right(tempq) # 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.dot( skew_symmetric(imu_f.data[k - 1].reshape( (3, 1))))) * delta_t Q = np.eye(6)
# 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 Cns = Quaternion(*q_est[k-1]).to_mat() # orientation quaternion at k-1 p_check = p_est[k-1] + delta_t * v_est[k-1] + ((delta_t**2)/2) * (Cns @ imu_f.data[k-1] - g) v_check = v_est[k-1] + delta_t * (Cns @ imu_f.data[k-1] - g) q_check = Quaternion(axis_angle = (imu_w.data[k-1])*delta_t).quat_mult(q_est[k-1]) # orientation at k # 1.1 Linearize Motion Model F_k_1 = np.eye(9) #1 F_k_1[0:3,3:6] = np.eye(3) * delta_t #2 F_k_1[3:6,6:9] = -Cns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t L_k_1 = l_jac Q_k = np.eye(6) Q_k[0:3, 0:3] = Q_k[0:3, 0:3]*var_imu_f Q_k[3:6, 3:6] = Q_k[3:6, 3:6]*var_imu_w Q_k = (delta_t**2)*Q_k # 2. Propagate uncertainty p_cov_check = F_k_1@p_cov[k-1]@F_k_1.T + L_k_1@Q_k@L_k_1.T # 3. Check availability of GNSS and LIDAR measurements GNSSavailable = is_avl(gnss.t,imu_f.t[k]) #To check if GNSS reading is available Lidaravailable = is_avl(lidar.t,imu_f.t[k]) # To check if LIDAR reading is available
return p_check, v_check, q_check, p_cov_check #### 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 Rotation_Mat = Quaternion(*q_est[k-1]).to_mat() aux1=Rotation_Mat.dot(imu_f.data[k-1]) + g p_est[k] = p_est[k-1] + delta_t * v_est[k-1] + delta_t * delta_t * aux1/ 2 v_est[k] = v_est[k-1] + delta_t * aux1 q_est[k] = Quaternion(euler = delta_t * imu_w.data[k-1]).quat_mult(q_est[k-1]) Rotation_Mat = Quaternion(*q_est[k - 1]).to_mat() p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t ** 2) * (Rotation_Mat.dot(imu_f.data[k - 1]) + g) v_est[k] = v_est[k - 1] + delta_t * (Rotation_Mat.dot(imu_f.data[k - 1]) + g) q_est[k] = Quaternion(euler = delta_t * imu_w.data[k - 1]).quat_mult(q_est[k - 1]) # 1.1 Linearize the motion model and compute Jacobians F = np.eye(9) imu = imu_f.data[k - 1].reshape((3, 1)) F[0:3,3:6]=np.eye(3)*delta_t
return p_hat, v_hat, q_hat, p_cov_hat #### 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 rotation_matrix = Quaternion(*q_est[k-1]).to_mat() p_est[k] = p_est[k-1] + delta_t * v_est[k-1] + (delta_t**2 / 2)*(rotation_matrix.dot(imu_f.data[k-1]) + g) v_est[k] = v_est[k-1] + delta_t*(rotation_matrix.dot(imu_f.data[k-1]) + g) q_est[k] = (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.identity(9) F[:3, 3:6] = delta_t * np.identity(3) F[3:6, 6:] = -(rotation_matrix @ (skew_symmetric(imu_f.data[k-1].reshape((3,1))))) * delta_t Q = np.identity(6) Q[:, :3] *= delta_t**2 * var_imu_f Q[:, -3:] *= delta_t**2 * var_imu_w p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T) if lidar_i < lidar.t.shape[0] and abs(lidar.t[lidar_i] - imu_f.t[k]) < 0.02: p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_lidar, p_cov[k], lidar.data[lidar_i].T, p_est[k], v_est[k], q_est[k]) lidar_i += 1
return p_hat, v_hat, q_hat, p_cov_hat #### 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] Cns = Quaternion(*q_est[k - 1]).to_mat() # 1. Update state with IMU inputs p_check = p_est[k - 1] + v_est[k - 1] * delta_t + ( Cns.dot(imu_f.data[k - 1]) + g) * (delta_t**2) * 0.5 v_check = v_est[k - 1] + (Cns.dot(imu_f.data[k - 1]) + g) * delta_t q_check = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right( q_est[k - 1]) # 1.1 Linearize the motion model and compute Jacobians rotation_part = np.eye(3) rotation_part = -Cns.dot(skew_symmetric(imu_f.data[k - 1].reshape( 3, 1))) * delta_t F = np.eye(9) F[0:3, 3:6] = delta_t * np.eye(3) F[3:6, 6:9] = rotation_part
# start taking in the sensor data and creating estimates # for our state in a loop. ################################################################################ # start at 1 b/c we have initial prediction from gt for k in range(1, imu_f.data.shape[0]): delta_t = imu_f.t[k] - imu_f.t[k - 1] # 1. Update state with IMU inputs #the ortation matrix of the previous quaternion (3x3) C_ns = Quaternion(*q_est[k-1]).to_mat() #a. Position 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)) #b. Velocity v_check = v_est[k-1] + (delta_t*((C_ns.dot(imu_f.data[k-1]))+g)) #c. Orientation q_check = Quaternion(\ euler =(imu_w.data[k-1]*delta_t)).quat_mult_right(q_est[k-1]) # 1.1 Linearize the motion model and compute Jacobians #a. F_(k-1) Jacobian size of (9x9) 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] = -delta_t*skew_symmetric (C_ns.dot(imu_f.data[k-1])) #b. L_(k-1) Jacobian (9x6)
cns = Quaternion(*q_est[k - 1]).to_mat() temp = np.dot(cns, imu_f.data[k - 1]) p_est[k] = p_est[k - 1] + v_est[k - 1] * delta_t + (temp - g) * delta_t**2 / 2 v_est[k] = v_est[k - 1] + (temp - g) * delta_t q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] * delta_t).quat_mult( q_est[k - 1]) # 1.1 Linearize Motion Model Q = np.eye(6) Q[:3, :3] = var_imu_f * delta_t**2 * I3 Q[3:, 3:] = var_imu_w * delta_t**2 * I3 F = np.eye(9) F[:3, 3:6] = I3 * delta_t #F[3:6, 6:] = -skew_symmetric(temp) * delta_t F[3:6, 6:] = -cns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t p_cov[k] = F @ p_cov[k - 1] @ F.T + l_jac @ Q @ l_jac.T # 2. Propagate uncertainty # 3. Check availability of GNSS and LIDAR measurements lidar_found = False while lidar_index < lidar.t.shape[0]: if abs(lidar.t[lidar_index] - imu_f.t[k]) < threshold: lidar_found = True break elif lidar.t[lidar_index] > imu_f.t[k] + threshold: break lidar_index += 1 if lidar_found:
#### 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] # 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)))