Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ){ Matrix6d covTinv = Matrix6d::Zero(); Matrix6d adjTinv; adjTinv = adjoint_se3( inverse_se3(T) ); covTinv = adjTinv * covT * adjTinv.transpose(); return covTinv; }
// 3. System update void Ekf::systemUpdate(double dt){ // update state state_ = fSystem(state_,dt); // Then calculate F Matrix6d F; F = FSystem(state_,dt); // Then update covariance cov_ = F*cov_*F.transpose() + Q_; cov_ = zeroOutBiasXYThetaCov(cov_); }
void GaussianSamplingPose3D::update( const Vector6d& mean, const Matrix6d& llt ) { poseMean = mean; poseCov = llt * llt.transpose(); poseLT = llt; }