src/inekf.cpp
// Corrects state using magnetometer measurements (Right Invariant)

Corrects state using magnetometer measurements (Right Invariant)
Corrects state using magnetometer measurements ( Right Invariant )
void InEKF::CorrectMagnetometer(const Eigen::Vector3d& measured_magnetic_field, const Eigen::Matrix3d& covariance) {
}
void InEKF :: CorrectMagnetometer ( const Eigen :: Vector3d & measured_magnetic_field , const Eigen :: Matrix3d & covariance ) { }
Corrects state using magnetometer measurements (Right Invariant)
Corrects state using magnetometer measurements ( Right Invariant )
// Eigen::VectorXd Y, b;
 // Eigen::MatrixXd H, N, PI;
 // // Get Rotation Estimate
 // Eigen::Matrix3d R = state_.getRotation();
 // // 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_magnetic_field;
 // // Fill out b
 // b.conservativeResize(dimX, Eigen::NoChange);
 // b.segment(0,dimX) = Eigen::VectorXd::Zero(dimX);
 // b.segment<3>(0) = magnetic_field_;
 // // Fill out H
 // H.conservativeResize(3, dimP);
 // H.block(0,0,3,dimP) = Eigen::MatrixXd::Zero(3,dimP);
 // H.block<3,3>(0,0) = skew(magnetic_field_); 
 // // Fill out N
 // N.conservativeResize(3, 3);
 // N = R * covariance * R.transpose();
 // // 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();
 // // Correct state using stacked observation
 // Observation obs(Y,b,H,N,PI);
 // if (!obs.empty()) {
 //     this->CorrectRightInvariant(obs);
 //     // cout << obs << endl;
 // }
