src/inekf.cpp
// Correct State: Right-Invariant Observation

Correct State: Right-Invariant Observation
Correct State : Right - Invariant Observation
void InEKF::CorrectRightInvariant(const Eigen::MatrixXd& Z, 
                                  const Eigen::MatrixXd& H, 
                                  const Eigen::MatrixXd& N) {
  const auto& X = state_.getX();
  Eigen::VectorXd Theta = state_.getTheta();
  Eigen::MatrixXd P = state_.getP();
  const int dimX = state_.dimX();
  const int dimTheta = state_.dimTheta();
  const int dimP = state_.dimP();
  Theta = Eigen::Matrix<double,6,1>::Zero();
  P.block<6,6>(dimP-dimTheta,dimP-dimTheta) = 0.0001*Eigen::Matrix<double,6,6>::Identity();
  P.block(0,dimP-dimTheta,dimP-dimTheta,dimTheta).setZero();
  P.block(dimP-dimTheta,0,dimTheta,dimP-dimTheta).setZero();
  if (error_type_==ErrorType::LeftInvariant) {
    Eigen::MatrixXd Adj = Eigen::MatrixXd::Identity(dimP,dimP);
    Adj.block(0,0,dimP-dimTheta,dimP-dimTheta) = Adjoint_SEK3(X); 
    P.noalias() = (Adj * P * Adj.transpose()).eval(); 
  }
  const Eigen::MatrixXd PHT = P * H.transpose();
  const Eigen::MatrixXd S = H * PHT + N;
  Eigen::MatrixXd Sinv;
  if (S.rows() <= 3) {
    Sinv = S.inverse();
  }
  else {
    ldlt_.compute(S);
    const int dimS = S.rows();
    Sinv = ldlt_.solve(Eigen::MatrixXd::Identity(dimS, dimS));
  }
  const Eigen::MatrixXd K = PHT * Sinv;
  const Eigen::VectorXd delta = K*Z;
  const Eigen::MatrixXd dX = Exp_SEK3(delta.segment(0,delta.rows()-dimTheta));
  const Eigen::VectorXd dTheta = delta.segment(delta.rows()-dimTheta, dimTheta);
  const Eigen::MatrixXd X_new = dX*X; 
  const Eigen::VectorXd Theta_new = Theta + dTheta;
  state_.setX(X_new); 
  state_.setTheta(Theta_new);
  const Eigen::MatrixXd IKH = Eigen::MatrixXd::Identity(dimP,dimP) - K*H;
  Eigen::MatrixXd P_new = IKH * P * IKH.transpose() + K*N*K.transpose(); 
  if (error_type_==ErrorType::LeftInvariant) {
    Eigen::MatrixXd AdjInv = Eigen::MatrixXd::Identity(dimP,dimP);
    AdjInv.block(0,0,dimP-dimTheta,dimP-dimTheta) = Adjoint_SEK3(state_.calcXinv()); 
    P_new = (AdjInv * P_new * AdjInv.transpose()).eval();
  }
  state_.setP(P_new); 
}
void InEKF :: CorrectRightInvariant ( const Eigen :: MatrixXd & Z , const Eigen :: MatrixXd & H , const Eigen :: MatrixXd & N ) { const auto & X = state_ . getX ( ) ; Eigen :: VectorXd Theta = state_ . getTheta ( ) ; Eigen :: MatrixXd P = state_ . getP ( ) ; const int dimX = state_ . dimX ( ) ; const int dimTheta = state_ . dimTheta ( ) ; const int dimP = state_ . dimP ( ) ; Theta = Eigen :: Matrix < double , 6 , 1 > :: Zero ( ) ; P . block < 6 , 6 > ( dimP - dimTheta , dimP - dimTheta ) = 0.0001 * Eigen :: Matrix < double , 6 , 6 > :: Identity ( ) ; P . block ( 0 , dimP - dimTheta , dimP - dimTheta , dimTheta ) . setZero ( ) ; P . block ( dimP - dimTheta , 0 , dimTheta , dimP - dimTheta ) . setZero ( ) ; if ( error_type_ == ErrorType :: LeftInvariant ) { Eigen :: MatrixXd Adj = Eigen :: MatrixXd :: Identity ( dimP , dimP ) ; Adj . block ( 0 , 0 , dimP - dimTheta , dimP - dimTheta ) = Adjoint_SEK3 ( X ) ; P . noalias ( ) = ( Adj * P * Adj . transpose ( ) ) . eval ( ) ; } const Eigen :: MatrixXd PHT = P * H . transpose ( ) ; const Eigen :: MatrixXd S = H * PHT + N ; Eigen :: MatrixXd Sinv ; if ( S . rows ( ) <= 3 ) { Sinv = S . inverse ( ) ; } else { ldlt_ . compute ( S ) ; const int dimS = S . rows ( ) ; Sinv = ldlt_ . solve ( Eigen :: MatrixXd :: Identity ( dimS , dimS ) ) ; } const Eigen :: MatrixXd K = PHT * Sinv ; const Eigen :: VectorXd delta = K * Z ; const Eigen :: MatrixXd dX = Exp_SEK3 ( delta . segment ( 0 , delta . rows ( ) - dimTheta ) ) ; const Eigen :: VectorXd dTheta = delta . segment ( delta . rows ( ) - dimTheta , dimTheta ) ; const Eigen :: MatrixXd X_new = dX * X ; const Eigen :: VectorXd Theta_new = Theta + dTheta ; state_ . setX ( X_new ) ; state_ . setTheta ( Theta_new ) ; const Eigen :: MatrixXd IKH = Eigen :: MatrixXd :: Identity ( dimP , dimP ) - K * H ; Eigen :: MatrixXd P_new = IKH * P * IKH . transpose ( ) + K * N * K . transpose ( ) ; if ( error_type_ == ErrorType :: LeftInvariant ) { Eigen :: MatrixXd AdjInv = Eigen :: MatrixXd :: Identity ( dimP , dimP ) ; AdjInv . block ( 0 , 0 , dimP - dimTheta , dimP - dimTheta ) = Adjoint_SEK3 ( state_ . calcXinv ( ) ) ; P_new = ( AdjInv * P_new * AdjInv . transpose ( ) ) . eval ( ) ; } state_ . setP ( P_new ) ; }
Correct State: Right-Invariant Observation
Correct State : Right - Invariant Observation
// Get current state estimate
 // Remove bias
 // std::cout << "P:\n" << P << std::endl;
 // std::cout << state_ << std::endl;
 // Map from left invariant to right invariant error temporarily
 // Compute Kalman Gain
 // Compute state correction vector
 // Update state
 // Right-Invariant Update
 // Set new state  
 // Update Covariance
 // Joseph update form
 // Map from right invariant back to left invariant error
 // Set new covariance
