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