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

Correct State: Left-Invariant Observation
Correct State : Left - Invariant Observation
void InEKF::CorrectLeftInvariant(const Eigen::MatrixXd& Z, 
                                 const Eigen::MatrixXd& H, 
                                 const Eigen::MatrixXd& N) {
  const auto& X = state_.getX();
  const auto& Theta = state_.getTheta();
  Eigen::MatrixXd P = state_.getP();
  int dimX = state_.dimX();
  int dimTheta = state_.dimTheta();
  int dimP = state_.dimP();
  if (error_type_==ErrorType::RightInvariant) {
    Eigen::MatrixXd AdjInv = Eigen::MatrixXd::Identity(dimP,dimP);
    AdjInv.block(0,0,dimP-dimTheta,dimP-dimTheta) = Adjoint_SEK3(state_.calcXinv()); 
    P = (AdjInv * P * AdjInv.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 = X*dX; 
  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::RightInvariant) {
    Eigen::MatrixXd Adj = Eigen::MatrixXd::Identity(dimP,dimP);
    Adj.block(0,0,dimP-dimTheta,dimP-dimTheta) = Adjoint_SEK3(X_new); 
    P_new = (Adj * P_new * Adj.transpose()).eval(); 
  }
  state_.setP(P_new); 
}
void InEKF :: CorrectLeftInvariant ( const Eigen :: MatrixXd & Z , const Eigen :: MatrixXd & H , const Eigen :: MatrixXd & N ) { const auto & X = state_ . getX ( ) ; const auto & Theta = state_ . getTheta ( ) ; Eigen :: MatrixXd P = state_ . getP ( ) ; int dimX = state_ . dimX ( ) ; int dimTheta = state_ . dimTheta ( ) ; int dimP = state_ . dimP ( ) ; if ( error_type_ == ErrorType :: RightInvariant ) { Eigen :: MatrixXd AdjInv = Eigen :: MatrixXd :: Identity ( dimP , dimP ) ; AdjInv . block ( 0 , 0 , dimP - dimTheta , dimP - dimTheta ) = Adjoint_SEK3 ( state_ . calcXinv ( ) ) ; P = ( AdjInv * P * AdjInv . 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 = X * dX ; 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 :: RightInvariant ) { Eigen :: MatrixXd Adj = Eigen :: MatrixXd :: Identity ( dimP , dimP ) ; Adj . block ( 0 , 0 , dimP - dimTheta , dimP - dimTheta ) = Adjoint_SEK3 ( X_new ) ; P_new = ( Adj * P_new * Adj . transpose ( ) ) . eval ( ) ; } state_ . setP ( P_new ) ; }
Correct State: Left-Invariant Observation
Correct State : Left - Invariant Observation
// Get current state estimate
 // Map from right invariant to left invariant error temporarily
 // Compute Kalman Gain
 // Compute state correction vector
 // Update state
 // Left-Invariant Update
 // Set new state
 // Update Covariance
 // Joseph update form
 // Map from left invariant back to right invariant error
 // Set new covariance
