src/inekf.cpp
// Observation of absolute z-position of contact points (Left-Invariant Measurement)

Observation of absolute z-position of contact points (Left-Invariant Measurement)
Observation of absolute z - position of contact points ( Left - Invariant Measurement )
void InEKF::CorrectContactPosition(const int id, 
                                   const Eigen::Vector3d& measured_contact_position, 
                                   const Eigen::Matrix3d& covariance, 
                                   const Eigen::Vector3d& indices) {
  Eigen::VectorXd Z_full, Z;
  Eigen::MatrixXd PI, H_full, N_full, H, N;
  map<int,int>::iterator it_estimated = estimated_contact_positions_.find(id);
  if (it_estimated!=estimated_contact_positions_.end()) { 
    int startIndex;
    for (int i=0; i<3; ++i) {
      if (indices(i) != 0) {
        startIndex = PI.rows();
        PI.conservativeResize(startIndex+1, 3);  
        PI.template block<1,3>(startIndex,0).setZero();
        PI.coeffRef(startIndex,i) = 1;
      }  
    }
    if (PI.rows()==0) {
      return;
    }
    const int dimX = state_.dimX();
    const int dimTheta = state_.dimTheta();
    const int dimP = state_.dimP();
    const auto& d = state_.getVector(it_estimated->second);
    H_full = Eigen::MatrixXd::Zero(3,dimP);
    H_full.block<3,3>(0,0) = -skew(d);
    H_full.block<3,3>(0,3*it_estimated->second-6) = Eigen::Matrix3d::Identity();
    H.noalias() = PI*H_full;
    N_full = covariance;   
    N.noalias() = PI*N_full*PI.transpose();
    Z_full = measured_contact_position - d; 
    Z.noalias() = PI*Z_full;
    this->CorrectRightInvariant(Z,H,N);
  }
}
void InEKF :: CorrectContactPosition ( const int id , const Eigen :: Vector3d & measured_contact_position , const Eigen :: Matrix3d & covariance , const Eigen :: Vector3d & indices ) { Eigen :: VectorXd Z_full , Z ; Eigen :: MatrixXd PI , H_full , N_full , H , N ; map < int , int > :: iterator it_estimated = estimated_contact_positions_ . find ( id ) ; if ( it_estimated != estimated_contact_positions_ . end ( ) ) { int startIndex ; for ( int i = 0 ; i < 3 ; ++ i ) { if ( indices ( i ) != 0 ) { startIndex = PI . rows ( ) ; PI . conservativeResize ( startIndex + 1 , 3 ) ; PI . template block < 1 , 3 > ( startIndex , 0 ) . setZero ( ) ; PI . coeffRef ( startIndex , i ) = 1 ; } } if ( PI . rows ( ) == 0 ) { return ; } const int dimX = state_ . dimX ( ) ; const int dimTheta = state_ . dimTheta ( ) ; const int dimP = state_ . dimP ( ) ; const auto & d = state_ . getVector ( it_estimated -> second ) ; H_full = Eigen :: MatrixXd :: Zero ( 3 , dimP ) ; H_full . block < 3 , 3 > ( 0 , 0 ) = - skew ( d ) ; H_full . block < 3 , 3 > ( 0 , 3 * it_estimated -> second - 6 ) = Eigen :: Matrix3d :: Identity ( ) ; H . noalias ( ) = PI * H_full ; N_full = covariance ; N . noalias ( ) = PI * N_full * PI . transpose ( ) ; Z_full = measured_contact_position - d ; Z . noalias ( ) = PI * Z_full ; this -> CorrectRightInvariant ( Z , H , N ) ; } }
Observation of absolute z-position of contact points (Left-Invariant Measurement)
Observation of absolute z - position of contact points ( Left - Invariant Measurement )
// See if we can find id estimated_contact_positions
 // Fill out PI
 // Fill out observation data
 // Get contact position
 // Fill out H
 // Fill out N
 // Fill out Z
 // Correct
