src/inekf.cpp
// Observation of absolute position - GPS (Left-Invariant Measurement)

Observation of absolute position - GPS (Left-Invariant Measurement)
Observation of absolute position - GPS ( Left - Invariant Measurement )
void InEKF::CorrectPosition(const Eigen::Vector3d& measured_position, 
                            const Eigen::Matrix3d& covariance, 
                            const Eigen::Vector3d& indices) {
}
void InEKF :: CorrectPosition ( const Eigen :: Vector3d & measured_position , const Eigen :: Matrix3d & covariance , const Eigen :: Vector3d & indices ) { }
Observation of absolute position - GPS (Left-Invariant Measurement)
Observation of absolute position - GPS ( Left - Invariant Measurement )
// Eigen::VectorXd Y, b;
 // Eigen::MatrixXd H, N, PI;
 // // Fill out observation data
 // int dimX = state_.dimX();
 // int dimTheta = state_.dimTheta();
 // int dimP = state_.dimP();
 // // Fill out Y
 // Y.conservativeResize(dimX, Eigen::NoChange);
 // Y.segment(0,dimX) = Eigen::VectorXd::Zero(dimX);
 // Y.segment<3>(0) = measured_position;
 // Y(4) = 1;       
 // // Fill out b
 // b.conservativeResize(dimX, Eigen::NoChange);
 // b.segment(0,dimX) = Eigen::VectorXd::Zero(dimX);
 // b(4) = 1;       
 // // Fill out H
 // H.conservativeResize(3, dimP);
 // H.block(0,0,3,dimP) = Eigen::MatrixXd::Zero(3,dimP);
 // H.block<3,3>(0,6) = Eigen::Matrix3d::Identity(); 
 // // Fill out N
 // N.conservativeResize(3, 3);
 // N = covariance;
 // // Fill out PI      
 // PI.conservativeResize(3, dimX);
 // PI.block(0,0,3,dimX) = Eigen::MatrixXd::Zero(3,dimX);
 // PI.block(0,0,3,3) = Eigen::Matrix3d::Identity();
 // // Modify measurement based on chosen indices
 // const double HIGH_UNCERTAINTY = 1e6;
 // Eigen::Vector3d p = state_.getPosition();
 // if (!indices(0)) { 
 //   Y(0) = p(0);
 //   N(0,0) = HIGH_UNCERTAINTY;
 //   N(0,1) = 0;
 //   N(0,2) = 0;
 //   N(1,0) = 0;
 //   N(2,0) = 0;
 //   } 
 // if (!indices(1)) { 
 //   Y(1) = p(1);
 //   N(1,0) = 0;
 //   N(1,1) = HIGH_UNCERTAINTY;
 //   N(1,2) = 0;
 //   N(0,1) = 0;
 //   N(2,1) = 0;
 //   } 
 // if (!indices(2)) { 
 //   Y(2) = p(2);
 //   N(2,0) = 0;
 //   N(2,1) = 0;
 //   N(2,2) = HIGH_UNCERTAINTY;
 //   N(0,2) = 0;
 //   N(1,2) = 0;
 //   } 
 // // Correct state using stacked observation
 // Observation obs(Y,b,H,N,PI);
 // if (!obs.empty()) {
 //   this->CorrectLeftInvariant(obs);
 //   // cout << obs << endl;
 // }
