多传感器融合MSF算法源码阅读(二)

无人驾驶算法学习（六）：多传感器融合MSF算法
多传感器融合MSF算法源码阅读(一)

1. 触发回调函数

imu模块主要其预测p,v,q的作用
ethzasl_msf/msf_core/include/msf_core/msf_IMUHandler_ROS.h查看
IMUHandler_ROS::IMUCallback,用来传播imu状态量(a,w)

void IMUCallback(const sensor_msgs::ImuConstPtr & msg) 
{
    static int lastseq = constants::INVALID_SEQUENCE;
    if (static_cast(msg->header.seq) != lastseq + 1
        && lastseq != constants::INVALID_SEQUENCE) {
      MSF_WARN_STREAM(
          "msf_core: imu message drop curr seq:" << msg->header.seq
              << " expected: " << lastseq + 1);
    }
    lastseq = msg->header.seq;

    msf_core::Vector3 linacc;
    linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
        ->linear_acceleration.z;

    msf_core::Vector3 angvel;
    angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
        ->angular_velocity.z;

    this->ProcessIMU(linacc, angvel, msg->header.stamp.toSec(),
                      msg->header.seq);
  }

ethzasl_msf/msf_core/include/msf_core/msf_IMUHandler_ROS.h查看
IMUHandler_ROS::StateCallback，传入的参数sensor_fusion_comm::ExtEkfConstPtr,传播整个状态量(p,v,q,a,w)

  void StateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg) {
    static_cast&>(this->manager_)
        .SetHLControllerStateBuffer(*msg);

    // Get the imu values.
    msf_core::Vector3 linacc;
    linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
        ->linear_acceleration.z;

    msf_core::Vector3 angvel;
    angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
        ->angular_velocity.z;

    int32_t flag = msg->flag;
    // Make sure we tell the HL to ignore if data playback is on.
    if (this->manager_.GetDataPlaybackStatus())
      flag = sensor_fusion_comm::ExtEkf::ignore_state;

    bool isnumeric = true;
    if (flag == sensor_fusion_comm::ExtEkf::current_state) {
      isnumeric = CheckForNumeric(
          Eigen::Map >(msg->state.data()),
          "before prediction p,v,q");
    }

    // Get the propagated states.
    msf_core::Vector3 p, v;
    msf_core::Quaternion q;

    p = Eigen::Matrix(msg->state[0], msg->state[1],
                                    msg->state[2]);
    v = Eigen::Matrix(msg->state[3], msg->state[4],
                                    msg->state[5]);
    q = Eigen::Quaternion(msg->state[6], msg->state[7], msg->state[8],
                                  msg->state[9]);
    q.normalize();

    bool is_already_propagated = false;
    if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) {
      is_already_propagated = true;
    }

    this->ProcessState(linacc, angvel, p, v, q, is_already_propagated,
                        msg->header.stamp.toSec(), msg->header.seq);
  }

2.状态预测

ethzasl_msf/msf_core/include/msf_core/include/msf_core/implementation/msf_core_inl.h查看
IMUHandler_ROS类的父类IMUHandler的ProcessIMU

template
void MSF_Core::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 lastState = it_last_IMU->second;
  timer_PropGetClosestState.Stop();

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

  shared_ptr 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 = 0;
  // 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 last_am =
      Eigen::Matrix(0, 0, 0);
  if (currentState->a_m.norm() > 50)
    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(
          2, "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() > 3)
    predictionMade_ = true;

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

ProcessIMU()中调用了PropagateState()函数:
ethzasl_msf/msf_core/include/msf_core/include/msf_core/implementation/msf_core_inl.h查看

template
void MSF_Core::PropagateState(shared_ptr& state_old,
                                          shared_ptr& state_new) {

  double dt = state_new->time - state_old->time;

  // 将最新状态置为零
  boost::fusion::for_each(state_new->statevars, msf_tmp::ResetState());

  // 零传播：复制常量用于非传播状态。
  boost::fusion::for_each(
      state_new->statevars,
      msf_tmp::CopyNonPropagationStates(*state_old));

  Eigen::Matrix dv;
  const Vector3 ew = state_new->w_m
      - state_new->template Get();
  const Vector3 ewold = state_old->w_m
      - state_old->template Get();
  const Vector3 ea = state_new->a_m
      - state_new->template Get();
  const Vector3 eaold = state_old->a_m
      - state_old->template Get();
  const Matrix4 Omega = OmegaMatJPL(ew);
  const Matrix4 OmegaOld = OmegaMatJPL(ewold);
  Matrix4 OmegaMean = OmegaMatJPL((ew + ewold) / 2);

  // Zero order quaternion integration.
  // cur_state.q_ = (Eigen::Matrix::Identity() +
  // 0.5*Omega*dt)*StateBuffer_[(unsigned char)(idx_state_-1)].q_.coeffs();

  // First order quaternion integration, this is kind of costly and may not add
  // a lot to the quality of propagation...
  int div = 1;
  Matrix4 MatExp;
  MatExp.setIdentity();
  OmegaMean *= 0.5 * dt;
  for (int i = 1; i < 5; i++) {  // Can be made fourth order or less to save cycles.
    div *= i;
    MatExp = MatExp + OmegaMean / div;
    OmegaMean *= OmegaMean;
  }

  // 第一个四元数积分矩阵
  const Matrix4 quat_int =
      MatExp + 1.0 / 48.0 * (Omega * OmegaOld - OmegaOld * Omega) * dt * dt;

  // 第一次四元数积分
  state_new->template Get().coeffs() = quat_int *
      state_old->template Get().coeffs();
      state_new->template Get().normalize();

  dv = (state_new->template Get().toRotationMatrix() *
       ea + state_old->template Get().toRotationMatrix() *
       eaold) / 2;
  state_new->template Get() =
      state_old->template Get()
      + (dv - constants::GRAVITY) * dt;
  state_new->template Get() =
      state_old->template Get()
      + ((state_new->template Get()
          + state_old->template Get()) / 2 * dt);
}

• 算法流程图：
  

3.预测状态协方差

ethzasl_msf/msf_core/include/msf_core/implementation/msf_core_inl.h查看
PredictProcessCovariance()函数
注意:协方差是状态量的,msf使用的运动模型是误差状态运动模型的Fc,是关于误差状态导数的,状态量的状态转移矩阵需要做矩阵积分

template
void MSF_Core::PredictProcessCovariance(
    shared_ptr& state_old, shared_ptr& state_new) {

  double dt = state_new->time - state_old->time;

  if (dt <= 0) {
    MSF_WARN_STREAM_THROTTLE(
        1, "Requested cov prop between two states that where "<w_m
      - state_new->template Get();
  const Vector3 ea = state_new->a_m
      - state_new->template Get();

  const Matrix3 a_sk = Skew(ea);
  const Matrix3 w_sk = Skew(ew);
  const Matrix3 eye3 = Eigen::Matrix::Identity();

  const Matrix3 C_eq = state_new->template Get().toRotationMatrix();

  const double dt_p2_2 = dt * dt * 0.5;
  const double dt_p3_6 = dt_p2_2 * dt / 3.0;
  const double dt_p4_24 = dt_p3_6 * dt * 0.25;
  const double dt_p5_120 = dt_p4_24 * dt * 0.2;

  const Matrix3 Ca3 = C_eq * a_sk;
  const Matrix3 A = Ca3
      * (-dt_p2_2 * eye3 + dt_p3_6 * w_sk - dt_p4_24 * w_sk * w_sk);
  const Matrix3 B = Ca3
      * (dt_p3_6 * eye3 - dt_p4_24 * w_sk + dt_p5_120 * w_sk * w_sk);
  const Matrix3 D = -A;
  const Matrix3 E = eye3 - dt * w_sk + dt_p2_2 * w_sk * w_sk;
  const Matrix3 F = -dt * eye3 + dt_p2_2 * w_sk - dt_p3_6 * (w_sk * w_sk);
  const Matrix3 C = Ca3 * F;

  // Discrete error state propagation Matrix Fd according to:
  // Stephan Weiss and Roland Siegwart.
  // Real-Time Metric State Estimation for Modular Vision-Inertial Systems.
  // IEEE International Conference on Robotics and Automation. Shanghai, China, 2011
  typename EKFState_T::F_type& Fd = state_old->Fd;

  enum {
    idxstartcorr_p = msf_tmp::GetStartIndexInCorrection::value,
    idxstartcorr_v = msf_tmp::GetStartIndexInCorrection::value,
    idxstartcorr_q = msf_tmp::GetStartIndexInCorrection::value,
    idxstartcorr_b_w = msf_tmp::GetStartIndexInCorrection::value,
    idxstartcorr_b_a = msf_tmp::GetStartIndexInCorrection::value
  };

  Fd.template block<3, 3>(idxstartcorr_p, idxstartcorr_v) = dt * eye3;
  Fd.template block<3, 3>(idxstartcorr_p, idxstartcorr_q) = A;
  Fd.template block<3, 3>(idxstartcorr_p, idxstartcorr_b_w) = B;
  Fd.template block<3, 3>(idxstartcorr_p, idxstartcorr_b_a) = -C_eq * dt_p2_2;

  Fd.template block<3, 3>(idxstartcorr_v, idxstartcorr_q) = C;
  Fd.template block<3, 3>(idxstartcorr_v, idxstartcorr_b_w) = D;
  Fd.template block<3, 3>(idxstartcorr_v, idxstartcorr_b_a) = -C_eq * dt;

  Fd.template block<3, 3>(idxstartcorr_q, idxstartcorr_q) = E;
  Fd.template block<3, 3>(idxstartcorr_q, idxstartcorr_b_w) = F;

  typename EKFState_T::Q_type& Qd = state_old->Qd;

  CalcQCore(
      dt, state_new->template Get(), ew, ea, nav, nbav,
      nwv, nbwv, Qd);

  // Call user Q calc to fill in the blocks of auxiliary states.
  // TODO optim: make state Q-blocks map respective parts of Q using Eigen Map,
  // avoids copy.
  usercalc_.CalculateQAuxiliaryStates(*state_new, dt);

  // Now copy the userdefined blocks to Qd.
  boost::fusion::for_each(
      state_new->statevars,
      msf_tmp::CopyQBlocksFromAuxiliaryStatesToQ(Qd));

  // TODO (slynen) Optim: Multiplication of F blockwise, using the fact that aux
  // states have no entries outside their block.
  state_new->P = Fd * state_old->P * Fd.transpose() + Qd;

  // Set time for best cov prop to now.
  time_P_propagated = state_new->time;

}

• 算法流程图：