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

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\n");
 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 }

ProcessExternallyPropagatedState

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

注意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 };

Apply

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

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

posted @ 2018-03-07 16:04 太一吾鱼水阅读(1439) 评论(0) 编辑收藏举报

刷新页面返回顶部

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

公告