robot_pose_ekf源码解读

首飞爱玩机器人

已于 2022-02-21 09:30:19 修改

阅读量7.6k

点赞数 10

分类专栏： SLAM 文章标签： EKF SLAM

于 2019-10-22 22:59:45 首次发布

本文链接：https://blog.csdn.net/shoufei403/article/details/102655696

版权

SLAM 专栏收录该内容

10 篇文章 13 订阅

订阅专栏

卡尔曼滤波理论

下面几个网址对卡尔曼滤波算法有详细的描述：

Kalman Filter For Dummies
How a Kalman filter works, in pictures
SLAM中的EKF，UKF，PF原理简介
在这里插入图片描述

卡尔曼滤波与扩展卡尔曼滤波的重要区别：
卡尔曼滤波只能针对线性模型，包括线性运动模型、线性观测模型并且随机误差满足高斯分布。当处理的模型是非线性的，比如运动方程是非线性的，那么从上一次状态（用高斯分布描述）经过转换矩阵到当前状态就不再是高斯分布能描述的了。而到底用什么分布函数来描述都很难说的清。对于线性模型来说，上一次状态用高斯分布来描述，线性转换后的状态依然可以用高斯分布来描述。扩展卡尔曼滤波是实现了一种将非线性模型线性化的方法。除此之外，其它运算过程与卡尔曼滤波是一致的。扩展卡尔曼滤波的思路是:
1）在工作点附近，用泰勒展开式去线性近似。泰勒展开式的高阶项被忽略了所以得到下面的公式。
在这里插入图片描述
ps:图片来自https://www.cnblogs.com/gaoxiang12/p/5560360.html
这一点在代码上的实现可以参考来自robot_pose_ekf包中的一段代码。

namespace BFL
{
  using namespace MatrixWrapper;


  NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise)
    : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),
      df(6,6)
  {
    // initialize df matrix
    for (unsigned int i=1; i<=6; i++){
      for (unsigned int j=1; j<=6; j++){
        if (i==j) df(i,j) = 1;
        else df(i,j) = 0;
      }
    }
  }


  NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){}

  ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const  //更新预测步骤的状态量,这里实现了x_t = f(x_t-1)
  {
    ColumnVector state = ConditionalArgumentGet(0);
    ColumnVector vel  = ConditionalArgumentGet(1);
    state(1) += cos(state(6)) * vel(1);//x
    state(2) += sin(state(6)) * vel(1);//y
    state(6) += vel(2);//yaw
    return state + AdditiveNoiseMuGet();
  }

  Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const  //得到Jacobian矩阵
  {
    if (i==0)//derivative to the first conditional argument (x)
    {
      double vel_trans = ConditionalArgumentGet(1)(1);
      double yaw = ConditionalArgumentGet(0)(6);

      df(1,3)=-vel_trans*sin(yaw); //求x关于yaw的偏导
      df(2,3)= vel_trans*cos(yaw);//求y关于yaw的偏导

      return df;
    }
    else
    {
      if (i >= NumConditionalArgumentsGet())
      {
        cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";
        exit(-BFL_ERRMISUSE);
      }
      else{
        cerr << "The df is not implemented for the" <<i << "th conditional argument\n";
        exit(-BFL_ERRMISUSE);
      }
    }
  }

}//namespace BFL

2)将预测步骤和测量更新步骤的误差项用均值为0的高斯分布近似替代。

关于协方差矩阵的几点理解：
1)协方差用于描述两个变量之间线性相关的强度。
2)在机器人的应用中，一般都是用多维度的高斯分布来描述机器人的状态。其均值是一个状态列向量。如 $\left(\begin{array}{l}{x} \\ {y} \\ {\theta}\end{array}\right)$ 是用坐标x,y和偏航角来描述机器人的状态。而各维度的方差用协方差矩阵来表征。协方差矩阵统一描述不同维度间的协方差。它是一个对称矩阵，而且对角线是各个维度上的方差（表征对应维度样本数据的离散程度）。
3）当某维度的协方差为0时说明该维度上的数据一点都不离散，是完全不波动的数据。在后面计算卡尔曼增益 $K=P^{\prime} H^{T} S^{-1}$ 时，因为 $P^{\prime}$ 某维度等于0，则计算的该维度的卡尔曼增益 $K$ 也将为0。根据卡尔曼滤波算法公式 $\begin{array}{l}{x=x^{\prime}+K y} \\ {P=(I-K H) P^{\prime}}\end{array}$
更新的状态量与上一次的状态量保持一致，协方差也保持和上一状态的一致。由此可以看出此时卡尔曼滤波是不起作用的。
4）当观测模型协方差矩阵对角线中某一维度的元素非常大，如9999.0，则滤波器将会忽略掉该维度的测量数据。下面是推理步骤：
$\begin{array}{l}{R=[\infty]} \\ {S=H P^{\prime} H^{T}+R} \\ {S=\infty} \\ {K=P^{\prime} H^{T} S^{-1}} \\ {K=P^{\prime} H^{T}(\infty)^{-1}} \\ {K=0} \\ {x=x^{\prime}+K y} \\ {x=x^{\prime}}\end{array}$
最后会发现后验状态与先验状态保持一致，即完全忽略了测量数据。

理解协方差的参考网站：
详解协方差与协方差矩阵
 深入理解协方差矩阵
 浅谈协方差矩阵

BFL库

bfl是内置在ROS系统中的一个贝叶斯滤波器包，而robot_pose_ekf功能包是对bfl的ros封装。所以要分析robot_pose_ekf包必须对bfl有所了解。
源码下载地址https://github.com/ros-gbp/bfl-release.git
需要注意该仓库的upstream分支才有bfl库的源代码，master分支为robot_pose_ekf包的源代码。
下面的文章对bfl库的使用作了介绍。
C++中贝叶斯滤波器包bfl的使用(1)
C++中贝叶斯滤波器包bfl的使用(2)
C++中贝叶斯滤波器包bfl的使用(3)

BFL库的文件结构

在这里插入图片描述
examples文件夹中包含了几个使用案例。robot_pose_ekf中会使用到的非线性模型的建立方法在里面也有体现。tests文件夹中包含了测试用例子。
src文件夹中包含了bfl库的主要源码。
1）filter文件夹中实现了众多基于贝叶斯思想的滤波算法，其中就包括扩展卡尔曼滤波和直方图滤波。
在这里插入图片描述
filter.cpp文件实现了滤波器的虚基类，统一了各滤波器操作的接口。kalmanfilter.cpp文件的KalmanFilter类继承了filter,cpp中的Filter类，实现了卡尔曼滤波算法的大部分运算。extendedkalmanfilter.cpp文件中的ExtendedKalmanFilter类继承了KalmanFilter类，并在SysUpdate和MeasUpdate方法中分别实现了运动模型和观测模型的相关运算。不管实例化一个卡尔曼滤波还是扩展卡尔曼滤波都是使用ExtendedKalmanFilter类来定义，只是实例化时传入的模型分了线性模型和非线性模型。

2）pdf文件夹实现了多种概率密度分布函数，其中就包括了卡尔曼滤波中用到的高斯分布函数。下图显示了相关类的继承关系。
在这里插入图片描述

3）model文件夹中主要是对pdf文件夹中的概率分布函数进行包装，抽象出线性/非线性预测模型和线性/非线性观测模型，并提供了操作的接口。

4）wrappers文件夹中实现了矩阵的多种运算操作，避免了对其他库的依赖。

robot_pose_ekf

1）robot_pose_ekf节点可以接收odom、IMU、vo(视觉里程计)和GPS的数据。每类数据的接收均在相应接收话题的回调函数里。

  void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)

  void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)

  void OdomEstimationNode::voCallback(const VoConstPtr& vo)

  void OdomEstimationNode::gpsCallback(const GpsConstPtr& gps)

2）节点中定义了一个定时器，用于周期性的执行卡尔曼滤波。下面的函数就是整个滤波循环。

  // filter loop
  void OdomEstimationNode::spin(const ros::TimerEvent& e)

3）节点的执行流程图
在这里插入图片描述
注意，确定先验位姿所用的传感器是可选的。
4）在OdomEstimation类的构造函数里实例化了预测模型和4种传感器的测量模型。其中只有预测模型是非线性的。针对该非线性模型，在nonlinearanalyticconditionalgaussianodo.cpp文件中的ExpectedValueGet方法实现了扩展卡尔曼算法中的 $x^{\prime}=f(x)$ ，即状态预测更新。dfGet方法求得了线性化所需的Jacobian矩阵。在该类的update方法中执行了所有模型的更新操作。