def predict_cov_combined(self, t_idx):
u = self.pose_tracker.imu.get_linear_angular_velocity(t_idx)
F = expm(-self.pose_tracker.imu.delta_t * Transform.adjoint_6d(u))
self.cov_combine[:6,:6] = F @ self.cov_combine[:6,:6] @ F.T # cov_TT
self.cov_combine[:6,:6] += self.pose_tracker.imu.W # cov_TT add noise
self.cov_combine[:6,6:] = F @ self.cov_combine[:6,6:] # cov_TM
self.cov_combine[6:,:6] = self.cov_combine[6:,:6] @ F.T # cov_MT
def predict_covariance(self,t_idx):
u = self.imu.get_linear_angular_velocity(t_idx)
exp_arg = - self.imu.delta_t * Transform.adjoint_6d(u)
self.cov = expm(exp_arg) @ self.cov @ expm(exp_arg).T + self.imu.W
