Apollo6.0融合模块代码阅读笔记(一）

本文用于记录本人在学习Apollo6.0融合模块过程中的一些笔记，日后再次阅读Apollo代码时能快速恢复记忆。欢迎大家留言讨论，本人水平有限，若有理解错误的地方，还请指出，谢谢啦！

文章目录

• Apollo6.0融合模块代码阅读笔记(一）
• 前言
• 一、卡尔曼滤波的几个公式及计算过程
• 二、卡尔曼滤波之预测
• 二、卡尔曼滤波之校正
• 三、Kalman_filter其他函数
• 总结

---

前言

本文只涉及Apollo6.0卡尔曼滤波部分的源码阅读，motion_fusion/kalman_filter等。

---

一、卡尔曼滤波的几个公式及计算过程

卡尔曼滤波器的主要计算过程可以用以下五个公式代表：

1.预测

x k ^ ′ = A ∗ x ^ k − 1 + B u k \hat{x_k}'=A*\hat{x}_{k-1}+Bu_k xk​^​′=A∗x^k−1​+Buk​
P k ′ = A ∗ P k − 1 ∗ A T + Q P_k'=A*P_{k-1}*A^T+Q Pk′​=A∗Pk−1​∗AT+Q

2.校正

K k = P k ′ H T ( H P k ′ H T + R ) − 1 K_k=P'_kH^T(HP'_kH^T+R)^{-1} Kk​=Pk′​HT(HPk′​HT+R)−1
x ^ k = x ^ k ′ + K k ( z k − H x ^ k ′ ) \hat{x}_k=\hat{x}_k'+K_k(z_k-H\hat{x}_k') x^k​=x^k′​+Kk​(zk​−Hx^k′​)

3.更新误差协方差矩阵

P k = ( I − K k H ) P k ′ P_k=(I-K_kH)P_k' Pk​=(I−Kk​H)Pk′​
注意：Apollo使用了非最优卡尔曼增益的估计误差协方差矩阵更新公式
P k = ( I − K k H ) P k ′ ∗ ( I − K k H ) T + K ∗ R ∗ K T P_k=(I-K_kH)P_k'*(I-K_kH)^T+K*R*K^T Pk​=(I−Kk​H)Pk′​∗(I−Kk​H)T+K∗R∗KT

4.Apollo卡尔曼滤波器

Apollo卡尔曼滤波器的六个状态变量分别为anchor_point的横坐标和纵坐标、车辆的横向速度和纵向速度以及车辆的横向加速度及纵向加速度。

二、卡尔曼滤波之预测

1.源码注释

Kalman_filter.cc中Predict代码如下：

//transform_matrix状态转移矩阵A，env_uncertainty_matrix预测噪声的协方差矩阵Q
bool KalmanFilter::Predict(const Eigen::MatrixXd &transform_matrix,
                           const Eigen::MatrixXd &env_uncertainty_matrix) {
//检查是否初始化及各矩阵维度是否对应一致等。
  if (!init_) {
    AERROR << "Predict: Kalman Filter initialize not successfully";
    return false;
  }
  if (transform_matrix.rows() != states_num_) {
    AERROR << "the rows of transform matrix should be equal to state_num";
    return false;
  }
  if (transform_matrix.cols() != states_num_) {
    AERROR << "the cols of transform matrix should be equal to state_num";
    return false;
  }
  if (env_uncertainty_matrix.rows() != states_num_) {
    AERROR << "the rows of env uncertainty should be equal to state_num";
    return false;
  }
  if (env_uncertainty_matrix.cols() != states_num_) {
    AERROR << "the cols of env uncertainty should be equal to state_num";
    return false;
  }
  //transform_matrix状态转移矩阵A
  transform_matrix_ = transform_matrix;
  //env_uncertainty_matrix预测噪声的协方差矩阵Q
  env_uncertainty_ = env_uncertainty_matrix;
  //预测：X = A*X
  global_states_ = transform_matrix_ * global_states_;
  //先验误差P'的更新：P' = A*P*A.transpose()+Q
  global_uncertainty_ =
      transform_matrix_ * global_uncertainty_ * transform_matrix_.transpose() +
      env_uncertainty_;
  return true;
}

2.Predict()笔记

Predict()函数，实现对状态变量的预测及先验误差的更新。

二、卡尔曼滤波之校正

1.源码注释

//cur_observation观测值Z，cur_observation_uncertainty观测噪声的协方差矩阵R
bool KalmanFilter::Correct(const Eigen::VectorXd &cur_observation,
                           const Eigen::MatrixXd &cur_observation_uncertainty) {
  if (!init_) {
    AERROR << "Correct: Kalman Filter initialize not successfully";
    return false;
  }
  if (cur_observation.rows() != states_num_) {
    AERROR << "the rows of current observation should be equal to state_num";
    return false;
  }
  if (cur_observation_uncertainty.rows() != states_num_) {
    AERROR << "the rows of current observation uncertainty "
              "should be equal to state_num";
    return false;
  }
  if (cur_observation_uncertainty.cols() != states_num_) {
    AERROR << "the cols of current observation uncertainty "
              "should be equal to state_num";
    return false;
  }

  // 观测值Z
  cur_observation_ = cur_observation;
  // 观测噪声的协方差矩阵R
  cur_observation_uncertainty_ = cur_observation_uncertainty;
  //卡尔曼增益K= P' * H.transpose() */( H * P' * H.transpose() + R )
  kalman_gain_ = global_uncertainty_ * c_matrix_.transpose() *
                 (c_matrix_ * global_uncertainty_ * c_matrix_.transpose() +
                  cur_observation_uncertainty_)
                     .inverse();
  // 状态变量X = X' + K * ( Z - H * X')
  global_states_ = global_states_ + kalman_gain_ * (cur_observation_ -
                                                    c_matrix_ * global_states_);
  Eigen::MatrixXd tmp_identity;
  tmp_identity.setIdentity(states_num_, states_num_);
  // P = (I - K * H) * P' * (I - K * H).transpose() + K * R * K.transpose()
  global_uncertainty_ =
      (tmp_identity - kalman_gain_ * c_matrix_) * global_uncertainty_ *
          (tmp_identity - kalman_gain_ * c_matrix_).transpose() +
      kalman_gain_ * cur_observation_uncertainty_ * kalman_gain_.transpose();
  return true;
}

2.Correct()总结

Correct()函数实现了对卡尔曼增益的求解，状态变量的校正以及对误差的协方差矩阵P的更新。

三、Kalman_filter其他函数

1.SetControlMatrix() 源码注释

bool KalmanFilter::SetControlMatrix(const Eigen::MatrixXd &control_matrix) {
//检查初始化及矩阵维度
  if (!init_) {
    AERROR << "SetControlMatrix: Kalman Filter initialize not successfully";
    return false;
  }
  if (control_matrix.rows() != states_num_ ||
      control_matrix.cols() != states_num_) {
    AERROR << "the rows/cols of control matrix should be equal to state_num";
    return false;
  }
  // 设置状态转移矩阵Ｈ
  c_matrix_ = control_matrix;
  return true;
}

2.SetGainBreakdownThresh() 源码注释

bool KalmanFilter::SetGainBreakdownThresh(const std::vector<bool> &break_down,
                                          const float threshold) {
  if (static_cast<int>(break_down.size()) != states_num_) {
    return false;
  }
  //设置KalmanFilter的gain_break_down_、gain_break_down_threshold_
  for (int i = 0; i < states_num_; i++) {
    if (break_down[i]) {
      gain_break_down_(i) = 1;
    }
  }
  gain_break_down_threshold_ = threshold;
  return true;
}

3.SetValueBreakdownThresh() 源码注释

bool KalmanFilter::SetValueBreakdownThresh(const std::vector<bool> &break_down,
                                           const float threshold) {
  if (static_cast<int>(break_down.size()) != states_num_) {
    return false;
  }
  // 设置KalmanFilter的value_break_down_、value_break_down_threshold_ 
  for (int i = 0; i < states_num_; i++) {
    if (break_down[i]) {
      value_break_down_(i) = 1;
    }
  }
  value_break_down_threshold_ = threshold;
  return true;
}

4.CorrectionBreakdown() 源码注释

void KalmanFilter::CorrectionBreakdown() {
  //prior_global_states_是上一帧的global_states,states_gain是前后两帧校正值的差值
  Eigen::VectorXd states_gain = global_states_ - prior_global_states_;
  //breakdown_diff = (global_states_ - prior_global_states_)./(gain_break_down)
  //获取delta_ax和delta_ay
  Eigen::VectorXd breakdown_diff = states_gain.cwiseProduct(gain_break_down_);
  global_states_ -= breakdown_diff;
  //如果加速度增益大于2，对breakdown_diff进行调整
  if (breakdown_diff.norm() > gain_break_down_threshold_) {
    breakdown_diff.normalize();
    breakdown_diff *= gain_break_down_threshold_;
  }
  global_states_ += breakdown_diff;

  Eigen::VectorXd temp;
  temp.setOnes(states_num_, 1);
  //如果速度值小于0.05，则对速度进行修正
  if ((global_states_.cwiseProduct(value_break_down_)).norm() <
      value_break_down_threshold_) {
  //将vx和vy设置成0
    global_states_ = global_states_.cwiseProduct(temp - value_break_down_);
  }
  //更新校正结果
  //加速度处理的是加速度增益，而速度处理的是速度值
  prior_global_states_ = global_states_;
}

总结

kalman_filter中定义了卡尔曼滤波的主要计算流程，在kalman_motion_fuison中被逐步调用，实现障碍物的motion_fusion。