Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement

1.MSF_Core的三个函数：ProcessIMU、ProcessExternallyPropagatedState和AddMeasurement

MSF_Core维护了状态队列和观测值队列，这里需要结合论文思考这个状态队列的作用。

ProcessIMU方法：

 template<typename EKFState_T>
 void MSF_Core<EKFState_T>::ProcessIMU(
     const msf_core::Vector3& linear_acceleration,
     const msf_core::Vector3& angular_velocity, const double& msg_stamp,
     size_t /*msg_seq*/) {

   if (!initialized_)
     return;

   msf_timing::DebugTimer timer_PropGetClosestState("PropGetClosestState");
   if (it_last_IMU == stateBuffer_.GetIteratorEnd()) {
     it_last_IMU = stateBuffer_.GetIteratorClosestBefore(msg_stamp);
   }

   shared_ptr<EKFState_T> lastState = it_last_IMU->second;
   timer_PropGetClosestState.Stop();

   msf_timing::DebugTimer timer_PropPrepare("PropPrepare");
   if (lastState->time == constants::INVALID_TIME) {
     MSF_WARN_STREAM_THROTTLE(
         , __FUNCTION__<<"ImuCallback: closest state is invalid\n");
     return;  // Early abort.
   }

   shared_ptr<EKFState_T> currentState(new EKFState_T);
   currentState->time = msg_stamp;

   // Check if this IMU message is really after the last one (caused by restarting
   // a bag file).
   if (currentState->time - lastState->time < -0.01 && predictionMade_) {
     initialized_ = false;
     predictionMade_ = false;
     MSF_ERROR_STREAM(
         __FUNCTION__<<"latest IMU message was out of order by a too large amount, "
         "resetting EKF: last-state-time: " << msf_core::timehuman(lastState->time)
         << " "<< "current-imu-time: "<< msf_core::timehuman(currentState->time));
     return;
   }

   static int seq = ;
   // Get inputs.
   currentState->a_m = linear_acceleration;
   currentState->w_m = angular_velocity;
   currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr());
   currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc());

   // Remove acc spikes (TODO (slynen): find a cleaner way to do this).
   static Eigen::Matrix<double, , > last_am =
       Eigen::Matrix<double, , >(, , );
   if (currentState->a_m.norm() > )
     currentState->a_m = last_am;
   else {
     // Try to get the state before the current time.
     if (lastState->time == constants::INVALID_TIME) {
       MSF_WARN_STREAM(
           "Accelerometer readings had a spike, but no prior state was in the "
           "buffer to take cleaner measurements from");
       return;
     }
     last_am = lastState->a_m;
   }
   if (!predictionMade_) {
     if (lastState->time == constants::INVALID_TIME) {
       MSF_WARN_STREAM("Wanted to compare prediction time offset to last state, "
       "but no prior state was in the buffer to take cleaner measurements from");
       return;
     }
     if (fabs(currentState->time - lastState->time) > 0.1) {
       MSF_WARN_STREAM_THROTTLE(
           , "large time-gap re-initializing to last state\n");
       typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime(
           lastState->time, currentState->time);
       time_P_propagated = currentState->time;
       return;  // // early abort // // (if timegap too big)
     }
   }

   if (lastState->time == constants::INVALID_TIME) {
     MSF_WARN_STREAM(
         "Wanted to propagate state, but no valid prior state could be found in "
         "the buffer");
     return;
   }
   timer_PropPrepare.Stop();

   msf_timing::DebugTimer timer_PropState("PropState");
   //propagate state and covariance
   PropagateState(lastState, currentState);
   timer_PropState.Stop();

   msf_timing::DebugTimer timer_PropInsertState("PropInsertState");
   it_last_IMU = stateBuffer_.Insert(currentState);
   timer_PropInsertState.Stop();

   msf_timing::DebugTimer timer_PropCov("PropCov");
   PropagatePOneStep();
   timer_PropCov.Stop();
   usercalc_.PublishStateAfterPropagation(currentState);

   // Making sure we have sufficient states to apply measurements to.
   if (stateBuffer_.Size() > )
     predictionMade_ = true;

   if (predictionMade_) {
     // Check if we can apply some pending measurement.
     HandlePendingMeasurements();
   }
   seq++;

 }

ProcessIMU

ProcessExternallyPropagatedState方法：

 template<typename EKFState_T>
 void MSF_Core<EKFState_T>::ProcessExternallyPropagatedState(
     const msf_core::Vector3& linear_acceleration,
     const msf_core::Vector3& angular_velocity, const msf_core::Vector3& p,
     const msf_core::Vector3& v, const msf_core::Quaternion& q,
     bool is_already_propagated, const double& msg_stamp, size_t /*msg_seq*/) {

   if (!initialized_)
     return;

   // fast method to get last_IMU is broken
   // TODO(slynen): fix iterator setting for state callback

 //  // Get the closest state and check validity.
 //  if (it_last_IMU == stateBuffer_.getIteratorEnd()) {
 //    it_last_IMU = stateBuffer_.getIteratorClosestBefore(msg_stamp);
 //    assert(!(it_last_IMU == stateBuffer_.getIteratorEnd()));
 //  }

   // TODO(slynen): not broken, revert back when it_last_IMU->second from above is fixed
   shared_ptr<EKFState_T> lastState = stateBuffer_.GetLast();//it_last_IMU->second;
   if (lastState->time == constants::INVALID_TIME) {
     MSF_WARN_STREAM_THROTTLE(, "StateCallback: closest state is invalid\n");
     return;  // Early abort.
   }

   // Create a new state.
   shared_ptr<EKFState_T> currentState(new EKFState_T);
   currentState->time = msg_stamp;

   // Get inputs.
   currentState->a_m = linear_acceleration;
   currentState->w_m = angular_velocity;
   currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr());
   currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc());

   // Remove acc spikes (TODO (slynen): Find a cleaner way to do this).
   static Eigen::Matrix<double, , > last_am =
       Eigen::Matrix<double, , >(, , );
   if (currentState->a_m.norm() > )
     currentState->a_m = last_am;
   else
     last_am = currentState->a_m;

   if (!predictionMade_) {
     if (fabs(currentState->time - lastState->time) > ) {
       typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime(
           lastState->time, currentState->time);
       MSF_WARN_STREAM_THROTTLE(
           , "large time-gap re-initializing to last state: "
           << msf_core::timehuman(tmp->time));
       return;  // Early abort (if timegap too big).
     }
   }

   bool isnumeric = CheckForNumeric(
       currentState->template Get<StateDefinition_T::p>(),
       "before prediction p");

   // State propagation is made externally, so we read the actual state.
   if (is_already_propagated && isnumeric) {
     currentState->template Get<StateDefinition_T::p>() = p;
     currentState->template Get<StateDefinition_T::v>() = v;
     currentState->template Get<StateDefinition_T::q>() = q;

     // Zero props: copy non propagation states from last state.
     boost::fusion::for_each(
         currentState->statevars,
         msf_tmp::CopyNonPropagationStates<EKFState_T>(*lastState));

   } else if (!is_already_propagated || !isnumeric) {
     // Otherwise let's do the state prop. here.
     PropagateState(lastState, currentState);
   }

   // Clean reset of state and measurement buffer, before we start propagation.
   if (!predictionMade_) {

     // Make sure we keep the covariance for the first state.
     currentState->P = stateBuffer_.GetLast()->P;
     time_P_propagated = currentState->time;

     stateBuffer_.Clear();
     MeasurementBuffer_.Clear();
     while (!queueFutureMeasurements_.empty()) {
       queueFutureMeasurements_.pop();
     }
   }

   stateBuffer_.Insert(currentState);
   PropagatePOneStep();
   predictionMade_ = true;
   usercalc_.PublishStateAfterPropagation(currentState);

   isnumeric = CheckForNumeric(
       currentState->template Get<StateDefinition_T::p>(), "prediction p");
   isnumeric = CheckForNumeric(currentState->P, "prediction done P");

   // Check if we can apply some pending measurement.
   HandlePendingMeasurements();
 }

ProcessExternallyPropagatedState

AddMeasurement方法：

 template<typename EKFState_T>
 void MSF_Core<EKFState_T>::AddMeasurement(
     shared_ptr<MSF_MeasurementBase<EKFState_T> > measurement) {

   // Return if not initialized of no imu data available.
   if (!initialized_ || !predictionMade_)
     return;

   // Check if the measurement is in the future where we don't have imu
   // measurements yet.
   if (measurement->time > stateBuffer_.GetLast()->time) {
     queueFutureMeasurements_.push(measurement);
     return;

   }
   // Check if there is still a state in the buffer for this message (too old).
   if (measurement->time < stateBuffer_.GetFirst()->time) {
     MSF_WARN_STREAM(
         "You tried to give me a measurement which is too far in the past. Are "
         "you sure your clocks are synced and delays compensated correctly? "
         "[measurement: "<<timehuman(measurement->time)<<" (s) first state in "
             "buffer: "<<timehuman(stateBuffer_.GetFirst()->time)<<" (s)]");
     return;  // Reject measurements too far in the past.
   }

   // Add this measurement to the buffer and get an iterator to it.
   typename measurementBufferT::iterator_T it_meas =
       MeasurementBuffer_.Insert(measurement);
   // Get an iterator the the end of the measurement buffer.
   typename measurementBufferT::iterator_T it_meas_end = MeasurementBuffer_
       .GetIteratorEnd();

   // No propagation if no update is applied.
   typename StateBuffer_T::iterator_T it_curr = stateBuffer_.GetIteratorEnd();

   bool appliedOne = false;

   isfuzzyState_ = false;

   // Now go through all the measurements and apply them one by one.
   for (; it_meas != it_meas_end; ++it_meas) {

     if (it_meas->second->time <= )  // Valid?
       continue;
     msf_timing::DebugTimer timer_meas_get_state("Get state for measurement");
     // Propagates covariance to state.
     shared_ptr<EKFState_T> state = GetClosestState(it_meas->second->time);
     timer_meas_get_state.Stop();
     if (state->time <= ) {
       MSF_ERROR_STREAM_THROTTLE(
           , __FUNCTION__<< " GetClosestState returned an invalid state");
       continue;
     }

     msf_timing::DebugTimer timer_meas_apply("Apply measurement");
     // Calls back core::ApplyCorrection(), which sets time_P_propagated to meas
     // time.
     it_meas->second->Apply(state, *this);
     timer_meas_apply.Stop();
     // Make sure to propagate to next measurement or up to now if no more
     // measurements. Propagate from current state.
     it_curr = stateBuffer_.GetIteratorAtValue(state);

     typename StateBuffer_T::iterator_T it_end;
     typename measurementBufferT::iterator_T it_nextmeas = it_meas;
     ++it_nextmeas;  // The next measurement in the list.
     // That was the last measurement, so propagate state to now.
     if (it_nextmeas == it_meas_end) {
       it_end = stateBuffer_.GetIteratorEnd();
     } else {
       it_end = stateBuffer_.GetIteratorClosestAfter(it_nextmeas->second->time);
       if (it_end != stateBuffer_.GetIteratorEnd()) {
         // Propagate to state closest after the measurement so we can interpolate.
         ++it_end;
       }
     }

     typename StateBuffer_T::iterator_T it_next = it_curr;
     ++it_next;

     msf_timing::DebugTimer timer_prop_state_after_meas(
         "Repropagate state to now");
     // Propagate to selected state.
     for (; it_curr != it_end && it_next != it_end &&
            it_curr->second->time != constants::INVALID_TIME &&
            it_next->second->time != constants::INVALID_TIME; ++it_curr,
            ++it_next) {
       if (it_curr->second == it_next->second) {
         MSF_ERROR_STREAM(__FUNCTION__<< " propagation : it_curr points to same "
         "state as it_next. This must not happen.");
         continue;
       }
       // Break loop if EKF reset in the meantime.
       if (!initialized_ || !predictionMade_)
         return;
       PropagateState(it_curr->second, it_next->second);
     }
     timer_prop_state_after_meas.Stop();
     appliedOne = true;
   }

   if (!appliedOne) {
     MSF_ERROR_STREAM("No measurement was applied, this should not happen.");
     return;
   }

   // Now publish the best current estimate.
   shared_ptr<EKFState_T>& latestState = stateBuffer_.GetLast();

   PropPToState(latestState);  // Get the latest covariance.

   usercalc_.PublishStateAfterUpdate(latestState);
 }

AddMeasurement

注意AddMeasurement方法中的 it_meas->second->Apply(state, *this); 这句代码，调用了PoseMeasurement的Apply方法。注意Apply方法要求*this参数，将MSF_Core作为参数传入，这在Apply执行过程中有一个调用core.ApplyCorrection()的步骤。

2.PoseMeasurement继承自PoseMeasurementBase，即msf_core::MSF_Measurement，这个又继承自MSF_MeasurementBase<EKFState_T>。

 template<typename T, typename RMAT_T, typename EKFState_T>
 class MSF_Measurement : public MSF_MeasurementBase<EKFState_T>

 typedef msf_core::MSF_Measurement<geometry_msgs::PoseWithCovarianceStamped,
     Eigen::Matrix<double, nMeasurements, nMeasurements>, msf_updates::EKFState> PoseMeasurementBase;

struct PoseMeasurement : public PoseMeasurementBase

查看PoseMeasurement类Apply方法的代码：

 virtual void Apply(shared_ptr<EKFState_T> state_nonconst_new,
                      msf_core::MSF_Core<EKFState_T>& core) {

     if (isabsolute_) {  // Does this measurement refer to an absolute measurement,
       // or is is just relative to the last measurement.
       // Get a const ref, so we can read core states
       const EKFState_T& state = *state_nonconst_new;
       // init variables
       Eigen::Matrix<double, nMeasurements,
           msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new;
       Eigen::Matrix<double, nMeasurements, > r_old;

       CalculateH(state_nonconst_new, H_new);

       // Get rotation matrices.
       Eigen::Matrix<double, , > C_wv = state.Get<StateQwvIdx>()

           .conjugate().toRotationMatrix();
       Eigen::Matrix<double, , > C_q = state.Get<StateDefinition_T::q>()
           .conjugate().toRotationMatrix();

       // Construct residuals.
       // Position.
       r_old.block<, >(, ) = z_p_
           - (C_wv.transpose()
               * (-state.Get<StatePwvIdx>()
                   + state.Get<StateDefinition_T::p>()
                   + C_q.transpose() * state.Get<StatePicIdx>()))
               * state.Get<StateLIdx>();

       // Attitude.
       Eigen::Quaternion<double> q_err;
       q_err = (state.Get<StateQwvIdx>()
           * state.Get<StateDefinition_T::q>()
           * state.Get<StateQicIdx>()).conjugate() * z_q_;
       r_old.block<, >(, ) = q_err.vec() / q_err.w() * ;
       // Vision world yaw drift.
       q_err = state.Get<StateQwvIdx>();

       r_old(, ) = - * (q_err.w() * q_err.z() + q_err.x() * q_err.y())
           / ( -  * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));

       if (!CheckForNumeric(r_old, "r_old")) {
         MSF_ERROR_STREAM("r_old: "<<r_old);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
       }
       if (!CheckForNumeric(H_new, "H_old")) {
         MSF_ERROR_STREAM("H_old: "<<H_new);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
       }
       if (!CheckForNumeric(R_, "R_")) {
         MSF_ERROR_STREAM("R_: "<<R_);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
       }

       // Call update step in base class.
       this->CalculateAndApplyCorrection(state_nonconst_new, core, H_new, r_old,
                                         R_);
     } else {
       // Init variables: Get previous measurement.
       shared_ptr < msf_core::MSF_MeasurementBase<EKFState_T> > prevmeas_base =
           core.GetPreviousMeasurement(this->time, this->sensorID_);

       if (prevmeas_base->time == msf_core::constants::INVALID_TIME) {
         MSF_WARN_STREAM(
             "The previous measurement is invalid. Could not apply measurement! " "time:"<<this->time<<" sensorID: "<<this->sensorID_);
         return;
       }

       // Make this a pose measurement.
       shared_ptr<PoseMeasurement> prevmeas = dynamic_pointer_cast
           < PoseMeasurement > (prevmeas_base);
       if (!prevmeas) {
         MSF_WARN_STREAM(
             "The dynamic cast of the previous measurement has failed. "
             "Could not apply measurement");
         return;
       }

       // Get state at previous measurement.
       shared_ptr<EKFState_T> state_nonconst_old = core.GetClosestState(
           prevmeas->time);

       if (state_nonconst_old->time == msf_core::constants::INVALID_TIME) {
         MSF_WARN_STREAM(
             "The state at the previous measurement is invalid. Could "
             "not apply measurement");
         return;
       }

       // Get a const ref, so we can read core states.
       const EKFState_T& state_new = *state_nonconst_new;
       const EKFState_T& state_old = *state_nonconst_old;

       Eigen::Matrix<double, nMeasurements,
           msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new,
           H_old;
       Eigen::Matrix<double, nMeasurements, > r_new, r_old;

       CalculateH(state_nonconst_old, H_old);

       H_old *= -;

       CalculateH(state_nonconst_new, H_new);

       //TODO (slynen): check that both measurements have the same states fixed!
       Eigen::Matrix<double, , > C_wv_old, C_wv_new;
       Eigen::Matrix<double, , > C_q_old, C_q_new;

       C_wv_new = state_new.Get<StateQwvIdx>().conjugate().toRotationMatrix();
       C_q_new = state_new.Get<StateDefinition_T::q>().conjugate()
           .toRotationMatrix();

       C_wv_old = state_old.Get<StateQwvIdx>().conjugate().toRotationMatrix();
       C_q_old = state_old.Get<StateDefinition_T::q>().conjugate()
           .toRotationMatrix();

       // Construct residuals.
       // Position:
       Eigen::Matrix<double, , > diffprobpos = (C_wv_new.transpose()
           * (-state_new.Get<StatePwvIdx>() + state_new.Get<StateDefinition_T::p>()
               + C_q_new.transpose() * state_new.Get<StatePicIdx>()))
           * state_new.Get<StateLIdx>() - (C_wv_old.transpose()
           * (-state_old.Get<StatePwvIdx>() + state_old.Get<StateDefinition_T::p>()
               + C_q_old.transpose() * state_old.Get<StatePicIdx>()))
               * state_old.Get<StateLIdx>();

       Eigen::Matrix<double, , > diffmeaspos = z_p_ - prevmeas->z_p_;

       r_new.block<, >(, ) = diffmeaspos - diffprobpos;

       // Attitude:
       Eigen::Quaternion<double> diffprobatt = (state_new.Get<StateQwvIdx>()
           * state_new.Get<StateDefinition_T::q>()
           * state_new.Get<StateQicIdx>()).conjugate()
           * (state_old.Get<StateQwvIdx>()
               * state_old.Get<StateDefinition_T::q>()
               * state_old.Get<StateQicIdx>());

       Eigen::Quaternion<double> diffmeasatt = z_q_.conjugate() * prevmeas->z_q_;

       Eigen::Quaternion<double> q_err;
       q_err = diffprobatt.conjugate() * diffmeasatt;

       r_new.block<, >(, ) = q_err.vec() / q_err.w() * ;
       // Vision world yaw drift.
       q_err = state_new.Get<StateQwvIdx>();

       r_new(, ) = - * (q_err.w() * q_err.z() + q_err.x() * q_err.y())
           / ( -  * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));

       if (!CheckForNumeric(r_old, "r_old")) {
         MSF_ERROR_STREAM("r_old: "<<r_old);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose());
       }
       if (!CheckForNumeric(H_new, "H_old")) {
         MSF_ERROR_STREAM("H_old: "<<H_new);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose());
       }
       if (!CheckForNumeric(R_, "R_")) {
         MSF_ERROR_STREAM("R_: "<<R_);
         MSF_WARN_STREAM(
             "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose());
       }

       // Call update step in base class.
       this->CalculateAndApplyCorrectionRelative(state_nonconst_old,
                                                 state_nonconst_new, core, H_old,
                                                 H_new, r_new, R_);

     }
   }
 };

Apply

里面有2个方法CalculateAndApplyCorrection和CalculateAndApplyCorrectionRelative，这2个方法在父类MSF_MeasurementBase<EKFState_T>中实现。

这2个方法实现了EKF方法的更新步骤。