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方法的更新步骤。