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]
def update_pose(self, K_gain, innovation, t_idx):
exp_args = Transform.calculate_twist(K_gain @ innovation.ravel(order="F"))
self.poses_ekf[:,:,t_idx] = self.poses_pred[:,:,t_idx] @ expm(exp_args)