def predict_pose(self,t_idx): if t_idx == 0: self.poses_pred[:,:,t_idx] = Transform.calcualte_pose(np.eye(3), np.zeros(3)) else: u = self.imu.get_linear_angular_velocity(t_idx) twist = Transform.calculate_twist(u) self.poses_pred[:,:,t_idx] = self.poses_pred[:,:,t_idx-1] @ expm(self.imu.delta_t * twist) return self.poses_pred[:,:,t_idx]