robot_localization中EKF源码介绍

本文深入解析了扩展卡尔曼滤波(EKF)的核心算法,包括量测更新和时间更新两个关键步骤。详细介绍了如何通过矩阵运算实现状态估计的更新,并讨论了在实际应用过程中可能遇到的问题及解决方法。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

robot_localization中EKF源码介绍

/*
 * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc.
 * Copyright (c) 2017, Locus Robotics, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
//其参考的导航坐标系为东北天坐标系
#include <robot_localization/ekf.hpp>
#include <robot_localization/filter_common.hpp>
#include <angles/angles.h>
#include <Eigen/Dense>
#include <rclcpp/duration.hpp>
#include <vector>

namespace robot_localization
{
Ekf::Ekf()
: FilterBase()
{}

Ekf::~Ekf() {}
//扩展卡尔曼滤波的量测更新
void Ekf::correct(const Measurement & measurement)
{
  FB_DEBUG(
    "---------------------- Ekf::correct ----------------------\n" <<
      "State is:\n" <<
      state_ <<
      "\n"
      "Topic is:\n" <<
      measurement.topic_name_ <<
      "\n"
      "Measurement is:\n" <<
      measurement.measurement_ <<
      "\n"
      "Measurement topic name is:\n" <<
      measurement.topic_name_ <<
      "\n\n"
      "Measurement covariance is:\n" <<
      measurement.covariance_ << "\n");

  // We don't want to update everything, so we need to build matrices that only
  // update the measured parts of our state vector. Throughout prediction and
  // correction, we attempt to maximize efficiency in Eigen.

  // First, determine how many state vector values we're updating
  std::vector<size_t> update_indices;
  for (size_t i = 0; i < measurement.update_vector_.size(); ++i) {
    if (measurement.update_vector_[i]) {
      // Handle nan and inf values in measurements
      if (std::isnan(measurement.measurement_(i))) {
        FB_DEBUG(
          "Value at index " << i <<
            " was nan. Excluding from update.\n");
      } else if (std::isinf(measurement.measurement_(i))) {
        FB_DEBUG(
          "Value at index " << i <<
            " was inf. Excluding from update.\n");
      } else {
        update_indices.push_back(i);
      }
    }
  }

  FB_DEBUG("Update indices are:\n" << update_indices << "\n");

  size_t update_size = update_indices.size();
  //设置状态量的向量和相关矩阵 Eigen::VectorXd是表示的是库Eigen的向量,而MatrixXd为其的矩阵
  // Now set up the relevant matrices
  Eigen::VectorXd state_subset(update_size);       // x (in most literature)
  Eigen::VectorXd measurement_subset(update_size);  // z
  Eigen::MatrixXd measurement_covariance_subset(update_size, update_size);  // R
  Eigen::MatrixXd state_to_measurement_subset(update_size, state_.rows());  // H 测量矩阵
  Eigen::MatrixXd kalman_gain_subset(state_.rows(), update_size);          // K 
  Eigen::VectorXd innovation_subset(update_size);  // z - Hx 观测误差
  //将所有矩阵、向量初始化为0
  state_subset.setZero();
  measurement_subset.setZero();
  measurement_covariance_subset.setZero();
  state_to_measurement_subset.setZero();
  kalman_gain_subset.setZero();
  innovation_subset.setZero();

  // Now build the sub-matrices from the full-sized matrices
  for (size_t i = 0; i < update_size; ++i) {
    measurement_subset(i) = measurement.measurement_(update_indices[i]);
    state_subset(i) = state_(update_indices[i]);

    for (size_t j = 0; j < update_size; ++j) {
      measurement_covariance_subset(i, j) =
        measurement.covariance_(update_indices[i], update_indices[j]);
    }

    // Handle negative (read: bad) covariances in the measurement. Rather
    // than exclude the measurement or make up a covariance, just take
    // the absolute value.
    //设置量测协方差矩阵
    //检查量测协方差矩阵是否为正,若不为正,则通过绝对值函数fabs将其改为正,防止矩阵奇异
    if (measurement_covariance_subset(i, i) < 0) {
      FB_DEBUG(
        "WARNING: Negative covariance for index " <<
          i << " of measurement (value is" <<
          measurement_covariance_subset(i, i) <<
          "). Using absolute value...\n");

      measurement_covariance_subset(i, i) =
        ::fabs(measurement_covariance_subset(i, i));
    }

    // If the measurement variance for a given variable is very
    // near 0 (as in e-50 or so) and the variance for that
    // variable in the covariance matrix is also near zero, then
    // the Kalman gain computation will blow up. Really, no
    // measurement can be completely without error, so add a small
    // amount in that case.
    if (measurement_covariance_subset(i, i) < 1e-9) {
      FB_DEBUG(
        "WARNING: measurement had very small error covariance for index " <<
          update_indices[i] <<
          ". Adding some noise to maintain filter stability.\n");

      measurement_covariance_subset(i, i) = 1e-9;
    }
  }

  // The state-to-measurement function, h, will now be a measurement_size x
  // full_state_size matrix, with ones in the (i, i) locations of the values to
  // be updated
  //设置量测矩阵H的值
  for (size_t i = 0; i < update_size; ++i) {
    state_to_measurement_subset(i, update_indices[i]) = 1;
  }

  FB_DEBUG(
    "Current state subset is:\n" <<
      state_subset << "\nMeasurement subset is:\n" <<
      measurement_subset << "\nMeasurement covariance subset is:\n" <<
      measurement_covariance_subset <<
      "\nState-to-measurement subset is:\n" <<
      state_to_measurement_subset << "\n");

  // (1) Compute the Kalman gain: K = (PH') / (HPH' + R) 计算卡尔曼增益
  //transpose()转置函数 inverse()求逆函数 noalias()为防止左右赋值时出现混淆
  Eigen::MatrixXd pht =
    estimate_error_covariance_ * state_to_measurement_subset.transpose();
  Eigen::MatrixXd hphr_inverse =
    (state_to_measurement_subset * pht + measurement_covariance_subset)
    .inverse();
  kalman_gain_subset.noalias() = pht * hphr_inverse;

  innovation_subset = (measurement_subset - state_subset);

  // Wrap angles in the innovation
  for (size_t i = 0; i < update_size; ++i) {
    if (update_indices[i] == StateMemberRoll ||
      update_indices[i] == StateMemberPitch ||
      update_indices[i] == StateMemberYaw)
    {
      innovation_subset(i) = ::angles::normalize_angle(innovation_subset(i));//调用ROS中angles的normalize_angle,将角度装为弧度范围为【-PI,PI】
    }
  }

  // (2) Check Mahalanobis distance between mapped measurement and state. 检查状态值与测量值之间的马氏距离:Mahalanobis距离是数据所在的空间的协方差的度量
  if (checkMahalanobisThreshold(
      innovation_subset, hphr_inverse,
      measurement.mahalanobis_thresh_))
  {
    // (3) Apply the gain to the difference between the state and measurement: x 利用当前观测与观测结果获得最终融合结果
    // x= x + K(z - Hx)
    state_.noalias() += kalman_gain_subset * innovation_subset;

    // (4) Update the estimate error covariance using the Joseph form: P=(I -KH)P(I - KH)' + KRK'
    //获得最终噪声的协方差矩阵
    Eigen::MatrixXd gain_residual = identity_;//单位矩阵
    gain_residual.noalias() -= kalman_gain_subset * state_to_measurement_subset;
    estimate_error_covariance_ =
      gain_residual * estimate_error_covariance_ * gain_residual.transpose();//(I -KH)P(I - KH)'
    estimate_error_covariance_.noalias() += kalman_gain_subset *
      measurement_covariance_subset *
      kalman_gain_subset.transpose();//(I -KH)P(I - KH)' + KRK'

    // Handle wrapping of angles
    wrapStateAngles();

    FB_DEBUG(
      "Kalman gain subset is:\n" <<
        kalman_gain_subset << "\nInnovation is:\n" <<
        innovation_subset << "\nCorrected full state is:\n" <<
        state_ << "\nCorrected full estimate error covariance is:\n" <<
        estimate_error_covariance_ <<
        "\n\n---------------------- /Ekf::correct ----------------------\n");
  }
}
//下面是扩展卡尔曼滤波的时间更新
void Ekf::predict(
  const rclcpp::Time & reference_time,
  const rclcpp::Duration & delta)
{
  const double delta_sec = filter_utilities::toSec(delta);//设置离散时间间隔

  FB_DEBUG(
    "---------------------- Ekf::predict ----------------------\n" <<
      "delta is " << filter_utilities::toSec(delta) << "\n" <<
      "state is " << state_ << "\n");

  double roll = state_(StateMemberRoll);
  double pitch = state_(StateMemberPitch);
  double yaw = state_(StateMemberYaw);
  double x_vel = state_(StateMemberVx);
  double y_vel = state_(StateMemberVy);
  double z_vel = state_(StateMemberVz);
  double pitch_vel = state_(StateMemberVpitch);
  double yaw_vel = state_(StateMemberVyaw);
  double x_acc = state_(StateMemberAx);
  double y_acc = state_(StateMemberAy);
  double z_acc = state_(StateMemberAz);

  // We'll need these trig calculations a lot.
  double sp = ::sin(pitch);
  double cp = ::cos(pitch);
  double cpi = 1.0 / cp;
  double tp = sp * cpi;

  double sr = ::sin(roll);
  double cr = ::cos(roll);

  double sy = ::sin(yaw);
  double cy = ::cos(yaw);

  prepareControl(reference_time, delta);
  //X是在世界坐标系下,X的一阶导和二阶导是在载体坐标系下,因此需要根据k-1时刻的姿态转换到世界坐标系下
  // Prepare the transfer function
  transfer_function_(StateMemberX, StateMemberVx) = cy * cp * delta_sec;
  transfer_function_(StateMemberX, StateMemberVy) =
    (cy * sp * sr - sy * cr) * delta_sec;
  transfer_function_(StateMemberX, StateMemberVz) =
    (cy * sp * cr + sy * sr) * delta_sec;
  transfer_function_(StateMemberX, StateMemberAx) =
    0.5 * transfer_function_(StateMemberX, StateMemberVx) * delta_sec;
  transfer_function_(StateMemberX, StateMemberAy) =
    0.5 * transfer_function_(StateMemberX, StateMemberVy) * delta_sec;
  transfer_function_(StateMemberX, StateMemberAz) =
    0.5 * transfer_function_(StateMemberX, StateMemberVz) * delta_sec;
  transfer_function_(StateMemberY, StateMemberVx) = sy * cp * delta_sec;
  transfer_function_(StateMemberY, StateMemberVy) =
    (sy * sp * sr + cy * cr) * delta_sec;
  transfer_function_(StateMemberY, StateMemberVz) =
    (sy * sp * cr - cy * sr) * delta_sec;
  transfer_function_(StateMemberY, StateMemberAx) =
    0.5 * transfer_function_(StateMemberY, StateMemberVx) * delta_sec;
  transfer_function_(StateMemberY, StateMemberAy) =
    0.5 * transfer_function_(StateMemberY, StateMemberVy) * delta_sec;
  transfer_function_(StateMemberY, StateMemberAz) =
    0.5 * transfer_function_(StateMemberY, StateMemberVz) * delta_sec;
  transfer_function_(StateMemberZ, StateMemberVx) = -sp * delta_sec;
  transfer_function_(StateMemberZ, StateMemberVy) = cp * sr * delta_sec;
  transfer_function_(StateMemberZ, StateMemberVz) = cp * cr * delta_sec;
  transfer_function_(StateMemberZ, StateMemberAx) =
    0.5 * transfer_function_(StateMemberZ, StateMemberVx) * delta_sec;
  transfer_function_(StateMemberZ, StateMemberAy) =
    0.5 * transfer_function_(StateMemberZ, StateMemberVy) * delta_sec;
  transfer_function_(StateMemberZ, StateMemberAz) =
    0.5 * transfer_function_(StateMemberZ, StateMemberVz) * delta_sec;
  transfer_function_(StateMemberRoll, StateMemberVroll) = delta_sec;
  transfer_function_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta_sec;
  transfer_function_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta_sec;
  transfer_function_(StateMemberPitch, StateMemberVpitch) = cr * delta_sec;
  transfer_function_(StateMemberPitch, StateMemberVyaw) = -sr * delta_sec;
  transfer_function_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta_sec;
  transfer_function_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta_sec;
  transfer_function_(StateMemberVx, StateMemberAx) = delta_sec;
  transfer_function_(StateMemberVy, StateMemberAy) = delta_sec;
  transfer_function_(StateMemberVz, StateMemberAz) = delta_sec;

  // Prepare the transfer function Jacobian. This function is analytically
  // derived from the transfer function.
  //求解雅克比矩阵
  double x_coeff = 0.0;
  double y_coeff = 0.0;
  double z_coeff = 0.0;
  double one_half_at_squared = 0.5 * delta_sec * delta_sec;

  y_coeff = cy * sp * cr + sy * sr;
  z_coeff = -cy * sp * sr + sy * cr;
  double dFx_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;
  double dFR_dR = 1.0 + (cr * tp * pitch_vel - sr * tp * yaw_vel) * delta_sec;

  x_coeff = -cy * sp;
  y_coeff = cy * cp * sr;
  z_coeff = cy * cp * cr;
  double dFx_dP =
    (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
    one_half_at_squared;
  double dFR_dP =
    (cpi * cpi * sr * pitch_vel + cpi * cpi * cr * yaw_vel) * delta_sec;

  x_coeff = -sy * cp;
  y_coeff = -sy * sp * sr - cy * cr;
  z_coeff = -sy * sp * cr + cy * sr;
  double dFx_dY =
    (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
    one_half_at_squared;

  y_coeff = sy * sp * cr - cy * sr;
  z_coeff = -sy * sp * sr - cy * cr;
  double dFy_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;
  double dFP_dR = (-sr * pitch_vel - cr * yaw_vel) * delta_sec;

  x_coeff = -sy * sp;
  y_coeff = sy * cp * sr;
  z_coeff = sy * cp * cr;
  double dFy_dP =
    (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
    one_half_at_squared;

  x_coeff = cy * cp;
  y_coeff = cy * sp * sr - sy * cr;
  z_coeff = cy * sp * cr + sy * sr;
  double dFy_dY =
    (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
    one_half_at_squared;

  y_coeff = cp * cr;
  z_coeff = -cp * sr;
  double dFz_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;
  double dFY_dR = (cr * cpi * pitch_vel - sr * cpi * yaw_vel) * delta_sec;

  x_coeff = -cp;
  y_coeff = -sp * sr;
  z_coeff = -sp * cr;
  double dFz_dP =
    (x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +
    (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
    one_half_at_squared;
  double dFY_dP =
    (sr * tp * cpi * pitch_vel + cr * tp * cpi * yaw_vel) * delta_sec;

  // Much of the transfer function Jacobian is identical to the transfer
  // function 计算雅克比矩阵,其为线性化的状态转移矩阵
  transfer_function_jacobian_ = transfer_function_;
  transfer_function_jacobian_(StateMemberX, StateMemberRoll) = dFx_dR;
  transfer_function_jacobian_(StateMemberX, StateMemberPitch) = dFx_dP;
  transfer_function_jacobian_(StateMemberX, StateMemberYaw) = dFx_dY;
  transfer_function_jacobian_(StateMemberY, StateMemberRoll) = dFy_dR;
  transfer_function_jacobian_(StateMemberY, StateMemberPitch) = dFy_dP;
  transfer_function_jacobian_(StateMemberY, StateMemberYaw) = dFy_dY;
  transfer_function_jacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;
  transfer_function_jacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;
  transfer_function_jacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;
  transfer_function_jacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;
  transfer_function_jacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;
  transfer_function_jacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
  transfer_function_jacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;

  FB_DEBUG(
    "Transfer function is:\n" <<
      transfer_function_ << "\nTransfer function Jacobian is:\n" <<
      transfer_function_jacobian_ << "\nProcess noise covariance is:\n" <<
      process_noise_covariance_ << "\nCurrent state is:\n" <<
      state_ << "\n");
  //过程噪声协方差矩阵Q位于filter_base.cpp
  Eigen::MatrixXd * process_noise_covariance = &process_noise_covariance_;

  if (use_dynamic_process_noise_covariance_) {
    computeDynamicProcessNoiseCovariance(state_);
    process_noise_covariance = &dynamic_process_noise_covariance_;
  }

  // (1) Apply control terms, which are actually accelerations预测状态向量的状态转移矩阵
  state_(StateMemberVroll) +=
    control_acceleration_(ControlMemberVroll) * delta_sec;
  state_(StateMemberVpitch) +=
    control_acceleration_(ControlMemberVpitch) * delta_sec;
  state_(StateMemberVyaw) +=
    control_acceleration_(ControlMemberVyaw) * delta_sec;

  state_(StateMemberAx) = (control_update_vector_[ControlMemberVx] ?
    control_acceleration_(ControlMemberVx) :
    state_(StateMemberAx));
  state_(StateMemberAy) = (control_update_vector_[ControlMemberVy] ?
    control_acceleration_(ControlMemberVy) :
    state_(StateMemberAy));
  state_(StateMemberAz) = (control_update_vector_[ControlMemberVz] ?
    control_acceleration_(ControlMemberVz) :
    state_(StateMemberAz));

  // (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))
  state_ = transfer_function_ * state_;

  // Handle wrapping
  wrapStateAngles();

  FB_DEBUG(
    "Predicted state is:\n" <<
      state_ << "\nCurrent estimate error covariance is:\n" <<
      estimate_error_covariance_ << "\n");

  // (3) Project the error forward: P = J * P * J' + Q 预测其噪声协方差矩阵
  estimate_error_covariance_ =
    (transfer_function_jacobian_ * estimate_error_covariance_ *
    transfer_function_jacobian_.transpose());
  estimate_error_covariance_.noalias() +=
    delta_sec * (*process_noise_covariance);

  FB_DEBUG(
    "Predicted estimate error covariance is:\n" <<
      estimate_error_covariance_ <<
      "\n\n--------------------- /Ekf::predict ----------------------\n");
}

}  // namespace robot_localization

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

小猴同学编代码

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值