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

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

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

ProcessIMU方法：

  1 template<typename EKFState_T>
  2 void MSF_Core<EKFState_T>::ProcessIMU(
  3     const msf_core::Vector3& linear_acceleration,
  4     const msf_core::Vector3& angular_velocity, const double& msg_stamp,
  5     size_t /*msg_seq*/) {
  6 
  7   if (!initialized_)
  8     return;
  9 
 10   msf_timing::DebugTimer timer_PropGetClosestState("PropGetClosestState");
 11   if (it_last_IMU == stateBuffer_.GetIteratorEnd()) {
 12     it_last_IMU = stateBuffer_.GetIteratorClosestBefore(msg_stamp);
 13   }
 14 
 15   shared_ptr<EKFState_T> lastState = it_last_IMU->second;
 16   timer_PropGetClosestState.Stop();
 17 
 18   msf_timing::DebugTimer timer_PropPrepare("PropPrepare");
 19   if (lastState->time == constants::INVALID_TIME) {
 20     MSF_WARN_STREAM_THROTTLE(
 21         2, __FUNCTION__<<"ImuCallback: closest state is invalid\n");
 22     return;  // Early abort.
 23   }
 24 
 25   shared_ptr<EKFState_T> currentState(new EKFState_T);
 26   currentState->time = msg_stamp;
 27 
 28   // Check if this IMU message is really after the last one (caused by restarting
 29   // a bag file).
 30   if (currentState->time - lastState->time < -0.01 && predictionMade_) {
 31     initialized_ = false;
 32     predictionMade_ = false;
 33     MSF_ERROR_STREAM(
 34         __FUNCTION__<<"latest IMU message was out of order by a too large amount, "
 35         "resetting EKF: last-state-time: " << msf_core::timehuman(lastState->time)
 36         << " "<< "current-imu-time: "<< msf_core::timehuman(currentState->time));
 37     return;
 38   }
 39 
 40   static int seq = 0;
 41   // Get inputs.
 42   currentState->a_m = linear_acceleration;
 43   currentState->w_m = angular_velocity;
 44   currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr());
 45   currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc());
 46 
 47 
 48   // Remove acc spikes (TODO (slynen): find a cleaner way to do this).
 49   static Eigen::Matrix<double, 3, 1> last_am =
 50       Eigen::Matrix<double, 3, 1>(0, 0, 0);
 51   if (currentState->a_m.norm() > 50)
 52     currentState->a_m = last_am;
 53   else {
 54     // Try to get the state before the current time.
 55     if (lastState->time == constants::INVALID_TIME) {
 56       MSF_WARN_STREAM(
 57           "Accelerometer readings had a spike, but no prior state was in the "
 58           "buffer to take cleaner measurements from");
 59       return;
 60     }
 61     last_am = lastState->a_m;
 62   }
 63   if (!predictionMade_) {
 64     if (lastState->time == constants::INVALID_TIME) {
 65       MSF_WARN_STREAM("Wanted to compare prediction time offset to last state, "
 66       "but no prior state was in the buffer to take cleaner measurements from");
 67       return;
 68     }
 69     if (fabs(currentState->time - lastState->time) > 0.1) {
 70       MSF_WARN_STREAM_THROTTLE(
 71           2, "large time-gap re-initializing to last state\n");
 72       typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime(
 73           lastState->time, currentState->time);
 74       time_P_propagated = currentState->time;
 75       return;  // // early abort // // (if timegap too big)
 76     }
 77   }
 78 
 79   if (lastState->time == constants::INVALID_TIME) {
 80     MSF_WARN_STREAM(
 81         "Wanted to propagate state, but no valid prior state could be found in "
 82         "the buffer");
 83     return;
 84   }
 85   timer_PropPrepare.Stop();
 86 
 87   msf_timing::DebugTimer timer_PropState("PropState");
 88   //propagate state and covariance
 89   PropagateState(lastState, currentState);
 90   timer_PropState.Stop();
 91 
 92   msf_timing::DebugTimer timer_PropInsertState("PropInsertState");
 93   it_last_IMU = stateBuffer_.Insert(currentState);
 94   timer_PropInsertState.Stop();
 95 
 96   msf_timing::DebugTimer timer_PropCov("PropCov");
 97   PropagatePOneStep();
 98   timer_PropCov.Stop();
 99   usercalc_.PublishStateAfterPropagation(currentState);
100 
101   // Making sure we have sufficient states to apply measurements to.
102   if (stateBuffer_.Size() > 3)
103     predictionMade_ = true;
104 
105   if (predictionMade_) {
106     // Check if we can apply some pending measurement.
107     HandlePendingMeasurements();
108   }
109   seq++;
110 
111 }

ProcessIMU