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 "); 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 "); 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 }
ProcessExternallyPropagatedState方法：
1 template<typename EKFState_T> 2 void MSF_Core<EKFState_T>::ProcessExternallyPropagatedState( 3 const msf_core::Vector3& linear_acceleration, 4 const msf_core::Vector3& angular_velocity, const msf_core::Vector3& p, 5 const msf_core::Vector3& v, const msf_core::Quaternion& q, 6 bool is_already_propagated, const double& msg_stamp, size_t /*msg_seq*/) { 7 8 if (!initialized_) 9 return; 10 11 // fast method to get last_IMU is broken 12 // TODO(slynen): fix iterator setting for state callback 13 14 // // Get the closest state and check validity. 15 // if (it_last_IMU == stateBuffer_.getIteratorEnd()) { 16 // it_last_IMU = stateBuffer_.getIteratorClosestBefore(msg_stamp); 17 // assert(!(it_last_IMU == stateBuffer_.getIteratorEnd())); 18 // } 19 20 // TODO(slynen): not broken, revert back when it_last_IMU->second from above is fixed 21 shared_ptr<EKFState_T> lastState = stateBuffer_.GetLast();//it_last_IMU->second; 22 if (lastState->time == constants::INVALID_TIME) { 23 MSF_WARN_STREAM_THROTTLE(2, "StateCallback: closest state is invalid "); 24 return; // Early abort. 25 } 26 27 // Create a new state. 28 shared_ptr<EKFState_T> currentState(new EKFState_T); 29 currentState->time = msg_stamp; 30 31 // Get inputs. 32 currentState->a_m = linear_acceleration; 33 currentState->w_m = angular_velocity; 34 currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr()); 35 currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc()); 36 37 // Remove acc spikes (TODO (slynen): Find a cleaner way to do this). 38 static Eigen::Matrix<double, 3, 1> last_am = 39 Eigen::Matrix<double, 3, 1>(0, 0, 0); 40 if (currentState->a_m.norm() > 50) 41 currentState->a_m = last_am; 42 else 43 last_am = currentState->a_m; 44 45 if (!predictionMade_) { 46 if (fabs(currentState->time - lastState->time) > 5) { 47 typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime( 48 lastState->time, currentState->time); 49 MSF_WARN_STREAM_THROTTLE( 50 2, "large time-gap re-initializing to last state: " 51 << msf_core::timehuman(tmp->time)); 52 return; // Early abort (if timegap too big). 53 } 54 } 55 56 bool isnumeric = CheckForNumeric( 57 currentState->template Get<StateDefinition_T::p>(), 58 "before prediction p"); 59 60 // State propagation is made externally, so we read the actual state. 61 if (is_already_propagated && isnumeric) { 62 currentState->template Get<StateDefinition_T::p>() = p; 63 currentState->template Get<StateDefinition_T::v>() = v; 64 currentState->template Get<StateDefinition_T::q>() = q; 65 66 // Zero props: copy non propagation states from last state. 67 boost::fusion::for_each( 68 currentState->statevars, 69 msf_tmp::CopyNonPropagationStates<EKFState_T>(*lastState)); 70 71 } else if (!is_already_propagated || !isnumeric) { 72 // Otherwise let's do the state prop. here. 73 PropagateState(lastState, currentState); 74 } 75 76 // Clean reset of state and measurement buffer, before we start propagation. 77 if (!predictionMade_) { 78 79 // Make sure we keep the covariance for the first state. 80 currentState->P = stateBuffer_.GetLast()->P; 81 time_P_propagated = currentState->time; 82 83 stateBuffer_.Clear(); 84 MeasurementBuffer_.Clear(); 85 while (!queueFutureMeasurements_.empty()) { 86 queueFutureMeasurements_.pop(); 87 } 88 } 89 90 stateBuffer_.Insert(currentState); 91 PropagatePOneStep(); 92 predictionMade_ = true; 93 usercalc_.PublishStateAfterPropagation(currentState); 94 95 isnumeric = CheckForNumeric( 96 currentState->template Get<StateDefinition_T::p>(), "prediction p"); 97 isnumeric = CheckForNumeric(currentState->P, "prediction done P"); 98 99 // Check if we can apply some pending measurement. 100 HandlePendingMeasurements(); 101 }
AddMeasurement方法：
1 template<typename EKFState_T> 2 void MSF_Core<EKFState_T>::AddMeasurement( 3 shared_ptr<MSF_MeasurementBase<EKFState_T> > measurement) { 4 5 // Return if not initialized of no imu data available. 6 if (!initialized_ || !predictionMade_) 7 return; 8 9 // Check if the measurement is in the future where we don't have imu 10 // measurements yet. 11 if (measurement->time > stateBuffer_.GetLast()->time) { 12 queueFutureMeasurements_.push(measurement); 13 return; 14 15 } 16 // Check if there is still a state in the buffer for this message (too old). 17 if (measurement->time < stateBuffer_.GetFirst()->time) { 18 MSF_WARN_STREAM( 19 "You tried to give me a measurement which is too far in the past. Are " 20 "you sure your clocks are synced and delays compensated correctly? " 21 "[measurement: "<<timehuman(measurement->time)<<" (s) first state in " 22 "buffer: "<<timehuman(stateBuffer_.GetFirst()->time)<<" (s)]"); 23 return; // Reject measurements too far in the past. 24 } 25 26 // Add this measurement to the buffer and get an iterator to it. 27 typename measurementBufferT::iterator_T it_meas = 28 MeasurementBuffer_.Insert(measurement); 29 // Get an iterator the the end of the measurement buffer. 30 typename measurementBufferT::iterator_T it_meas_end = MeasurementBuffer_ 31 .GetIteratorEnd(); 32 33 // No propagation if no update is applied. 34 typename StateBuffer_T::iterator_T it_curr = stateBuffer_.GetIteratorEnd(); 35 36 bool appliedOne = false; 37 38 isfuzzyState_ = false; 39 40 // Now go through all the measurements and apply them one by one. 41 for (; it_meas != it_meas_end; ++it_meas) { 42 43 if (it_meas->second->time <= 0) // Valid? 44 continue; 45 msf_timing::DebugTimer timer_meas_get_state("Get state for measurement"); 46 // Propagates covariance to state. 47 shared_ptr<EKFState_T> state = GetClosestState(it_meas->second->time); 48 timer_meas_get_state.Stop(); 49 if (state->time <= 0) { 50 MSF_ERROR_STREAM_THROTTLE( 51 1, __FUNCTION__<< " GetClosestState returned an invalid state"); 52 continue; 53 } 54 55 msf_timing::DebugTimer timer_meas_apply("Apply measurement"); 56 // Calls back core::ApplyCorrection(), which sets time_P_propagated to meas 57 // time. 58 it_meas->second->Apply(state, *this); 59 timer_meas_apply.Stop(); 60 // Make sure to propagate to next measurement or up to now if no more 61 // measurements. Propagate from current state. 62 it_curr = stateBuffer_.GetIteratorAtValue(state); 63 64 typename StateBuffer_T::iterator_T it_end; 65 typename measurementBufferT::iterator_T it_nextmeas = it_meas; 66 ++it_nextmeas; // The next measurement in the list. 67 // That was the last measurement, so propagate state to now. 68 if (it_nextmeas == it_meas_end) { 69 it_end = stateBuffer_.GetIteratorEnd(); 70 } else { 71 it_end = stateBuffer_.GetIteratorClosestAfter(it_nextmeas->second->time); 72 if (it_end != stateBuffer_.GetIteratorEnd()) { 73 // Propagate to state closest after the measurement so we can interpolate. 74 ++it_end; 75 } 76 } 77 78 typename StateBuffer_T::iterator_T it_next = it_curr; 79 ++it_next; 80 81 msf_timing::DebugTimer timer_prop_state_after_meas( 82 "Repropagate state to now"); 83 // Propagate to selected state. 84 for (; it_curr != it_end && it_next != it_end && 85 it_curr->second->time != constants::INVALID_TIME && 86 it_next->second->time != constants::INVALID_TIME; ++it_curr, 87 ++it_next) { 88 if (it_curr->second == it_next->second) { 89 MSF_ERROR_STREAM(__FUNCTION__<< " propagation : it_curr points to same " 90 "state as it_next. This must not happen."); 91 continue; 92 } 93 // Break loop if EKF reset in the meantime. 94 if (!initialized_ || !predictionMade_) 95 return; 96 PropagateState(it_curr->second, it_next->second); 97 } 98 timer_prop_state_after_meas.Stop(); 99 appliedOne = true; 100 } 101 102 if (!appliedOne) { 103 MSF_ERROR_STREAM("No measurement was applied, this should not happen."); 104 return; 105 } 106 107 // Now publish the best current estimate. 108 shared_ptr<EKFState_T>& latestState = stateBuffer_.GetLast(); 109 110 PropPToState(latestState); // Get the latest covariance. 111 112 usercalc_.PublishStateAfterUpdate(latestState); 113 }
注意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>。
1 template<typename T, typename RMAT_T, typename EKFState_T> 2 class MSF_Measurement : public MSF_MeasurementBase<EKFState_T>
1 typedef msf_core::MSF_Measurement<geometry_msgs::PoseWithCovarianceStamped, 2 Eigen::Matrix<double, nMeasurements, nMeasurements>, msf_updates::EKFState> PoseMeasurementBase;
struct PoseMeasurement : public PoseMeasurementBase
查看PoseMeasurement类Apply方法的代码：
1 virtual void Apply(shared_ptr<EKFState_T> state_nonconst_new, 2 msf_core::MSF_Core<EKFState_T>& core) { 3 4 if (isabsolute_) { // Does this measurement refer to an absolute measurement, 5 // or is is just relative to the last measurement. 6 // Get a const ref, so we can read core states 7 const EKFState_T& state = *state_nonconst_new; 8 // init variables 9 Eigen::Matrix<double, nMeasurements, 10 msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new; 11 Eigen::Matrix<double, nMeasurements, 1> r_old; 12 13 CalculateH(state_nonconst_new, H_new); 14 15 // Get rotation matrices. 16 Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>() 17 18 .conjugate().toRotationMatrix(); 19 Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>() 20 .conjugate().toRotationMatrix(); 21 22 // Construct residuals. 23 // Position. 24 r_old.block<3, 1>(0, 0) = z_p_ 25 - (C_wv.transpose() 26 * (-state.Get<StatePwvIdx>() 27 + state.Get<StateDefinition_T::p>() 28 + C_q.transpose() * state.Get<StatePicIdx>())) 29 * state.Get<StateLIdx>(); 30 31 // Attitude. 32 Eigen::Quaternion<double> q_err; 33 q_err = (state.Get<StateQwvIdx>() 34 * state.Get<StateDefinition_T::q>() 35 * state.Get<StateQicIdx>()).conjugate() * z_q_; 36 r_old.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2; 37 // Vision world yaw drift. 38 q_err = state.Get<StateQwvIdx>(); 39 40 r_old(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y()) 41 / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z())); 42 43 if (!CheckForNumeric(r_old, "r_old")) { 44 MSF_ERROR_STREAM("r_old: "<<r_old); 45 MSF_WARN_STREAM( 46 "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose()); 47 } 48 if (!CheckForNumeric(H_new, "H_old")) { 49 MSF_ERROR_STREAM("H_old: "<<H_new); 50 MSF_WARN_STREAM( 51 "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose()); 52 } 53 if (!CheckForNumeric(R_, "R_")) { 54 MSF_ERROR_STREAM("R_: "<<R_); 55 MSF_WARN_STREAM( 56 "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose()); 57 } 58 59 // Call update step in base class. 60 this->CalculateAndApplyCorrection(state_nonconst_new, core, H_new, r_old, 61 R_); 62 } else { 63 // Init variables: Get previous measurement. 64 shared_ptr < msf_core::MSF_MeasurementBase<EKFState_T> > prevmeas_base = 65 core.GetPreviousMeasurement(this->time, this->sensorID_); 66 67 if (prevmeas_base->time == msf_core::constants::INVALID_TIME) { 68 MSF_WARN_STREAM( 69 "The previous measurement is invalid. Could not apply measurement! " "time:"<<this->time<<" sensorID: "<<this->sensorID_); 70 return; 71 } 72 73 // Make this a pose measurement. 74 shared_ptr<PoseMeasurement> prevmeas = dynamic_pointer_cast 75 < PoseMeasurement > (prevmeas_base); 76 if (!prevmeas) { 77 MSF_WARN_STREAM( 78 "The dynamic cast of the previous measurement has failed. " 79 "Could not apply measurement"); 80 return; 81 } 82 83 // Get state at previous measurement. 84 shared_ptr<EKFState_T> state_nonconst_old = core.GetClosestState( 85 prevmeas->time); 86 87 if (state_nonconst_old->time == msf_core::constants::INVALID_TIME) { 88 MSF_WARN_STREAM( 89 "The state at the previous measurement is invalid. Could " 90 "not apply measurement"); 91 return; 92 } 93 94 // Get a const ref, so we can read core states. 95 const EKFState_T& state_new = *state_nonconst_new; 96 const EKFState_T& state_old = *state_nonconst_old; 97 98 Eigen::Matrix<double, nMeasurements, 99 msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new, 100 H_old; 101 Eigen::Matrix<double, nMeasurements, 1> r_new, r_old; 102 103 CalculateH(state_nonconst_old, H_old); 104 105 H_old *= -1; 106 107 CalculateH(state_nonconst_new, H_new); 108 109 //TODO (slynen): check that both measurements have the same states fixed! 110 Eigen::Matrix<double, 3, 3> C_wv_old, C_wv_new; 111 Eigen::Matrix<double, 3, 3> C_q_old, C_q_new; 112 113 C_wv_new = state_new.Get<StateQwvIdx>().conjugate().toRotationMatrix(); 114 C_q_new = state_new.Get<StateDefinition_T::q>().conjugate() 115 .toRotationMatrix(); 116 117 C_wv_old = state_old.Get<StateQwvIdx>().conjugate().toRotationMatrix(); 118 C_q_old = state_old.Get<StateDefinition_T::q>().conjugate() 119 .toRotationMatrix(); 120 121 // Construct residuals. 122 // Position: 123 Eigen::Matrix<double, 3, 1> diffprobpos = (C_wv_new.transpose() 124 * (-state_new.Get<StatePwvIdx>() + state_new.Get<StateDefinition_T::p>() 125 + C_q_new.transpose() * state_new.Get<StatePicIdx>())) 126 * state_new.Get<StateLIdx>() - (C_wv_old.transpose() 127 * (-state_old.Get<StatePwvIdx>() + state_old.Get<StateDefinition_T::p>() 128 + C_q_old.transpose() * state_old.Get<StatePicIdx>())) 129 * state_old.Get<StateLIdx>(); 130 131 132 Eigen::Matrix<double, 3, 1> diffmeaspos = z_p_ - prevmeas->z_p_; 133 134 r_new.block<3, 1>(0, 0) = diffmeaspos - diffprobpos; 135 136 // Attitude: 137 Eigen::Quaternion<double> diffprobatt = (state_new.Get<StateQwvIdx>() 138 * state_new.Get<StateDefinition_T::q>() 139 * state_new.Get<StateQicIdx>()).conjugate() 140 * (state_old.Get<StateQwvIdx>() 141 * state_old.Get<StateDefinition_T::q>() 142 * state_old.Get<StateQicIdx>()); 143 144 Eigen::Quaternion<double> diffmeasatt = z_q_.conjugate() * prevmeas->z_q_; 145 146 Eigen::Quaternion<double> q_err; 147 q_err = diffprobatt.conjugate() * diffmeasatt; 148 149 r_new.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2; 150 // Vision world yaw drift. 151 q_err = state_new.Get<StateQwvIdx>(); 152 153 r_new(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y()) 154 / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z())); 155 156 if (!CheckForNumeric(r_old, "r_old")) { 157 MSF_ERROR_STREAM("r_old: "<<r_old); 158 MSF_WARN_STREAM( 159 "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose()); 160 } 161 if (!CheckForNumeric(H_new, "H_old")) { 162 MSF_ERROR_STREAM("H_old: "<<H_new); 163 MSF_WARN_STREAM( 164 "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose()); 165 } 166 if (!CheckForNumeric(R_, "R_")) { 167 MSF_ERROR_STREAM("R_: "<<R_); 168 MSF_WARN_STREAM( 169 "state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose()); 170 } 171 172 // Call update step in base class. 173 this->CalculateAndApplyCorrectionRelative(state_nonconst_old, 174 state_nonconst_new, core, H_old, 175 H_new, r_new, R_); 176 177 } 178 } 179 };
里面有2个方法CalculateAndApplyCorrection和CalculateAndApplyCorrectionRelative，这2个方法在父类MSF_MeasurementBase<EKFState_T>中实现。
这2个方法实现了EKF方法的更新步骤。