robot_pose_ekf源码解读

卡尔曼滤波理论

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

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 }+Ky\\ P=(I-KH)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 }+Ky\\ 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方法中执行了所有模型的更新操作。

PS:因水平能力有限，上述内容可能有理解不到位或不正确的地方。欢迎在下面的留言区给我留言。希望我写的东西也能帮到大家:)

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。