摘要

bfl是内置在ROS系统中的一个贝叶斯滤波器包，被使用在robot_pose_ekf中。本文是将该包单独提出来，操作一次整个包的教程（英文教程地址）。这个教程由3部分组成，第一部分，也就是本文，预测模型以及测量更新模型都是线性的。第二部分预测模型为非线性。第三部分预测模型和测量更新模型皆为非线性。

关于卡尔曼滤波器本身网上已经有很多大牛写的很棒的教程，我就不再赘述了。本文中的名词尽量与《概率机器人》中统一。本文中使用的软件包在这里下载。由于本人水平有限，欢迎邮件交流，我的邮箱是：[email protected]
　　
接下来让我们直接开始教程吧，let’s waaaaaaaaagh！！！

15分钟内构建你的第一个卡尔曼滤波器

准备.cpp文件
线性预测模型
线性测量更新
先验分布
构造滤波器

提示

机器人仿真
更新滤波器以及新的估计
结果

本文会一步步推导出一个拥有线性预测和线性测量更新的卡尔曼滤波器模型。

这个例子中，将会用卡尔曼滤波器去估计机器人的位置。机器人会在一个拥有一面墙的自由空间中移动。这个例子中，机器人只能前后移动，不能转弯。通过一个距离传感器，机器人可以得知它与墙的距离，如图所示：
　　图1-1

准备.cpp文件

在文件夹BFL/examples/linear_kalman/中你会看到一个cpp文件test_linear_kalman.cpp，这一部分是将要使用的卡尔曼滤波器头文件：

  #include <filter/extendendkalmanfilter.h>
  #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
  #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
  #include <pdf/linearanalyticsystemmodel_gaussianuncertainty.h>
  #include <pdf/linearanalyticmeasurementmodel_gaussianuncertainty.h>

这一部分是机器人仿真模型文件：

  #include "../mobile_robot.h"

这一部分是读取的一部分参数（比如机器人的初始位置估计）：

  #include "../mobile_robot_wall_cts.h"

最后是我们使用的命名空间：

  using namespace MatrixWrapper;
  using namespace BFL;
  using namespace std;

线性预测模型

现在开始建立能够预知每一步机器人位置的线性预测模型。因为机器人无法转弯，所以机器人位置的预测是基于机器人速度的线性等式。因为模型是线性的，BFL包已经提供了直接可以使用的方法。
　　
现在已知的参数有：机器人之前的位置 $x_{k-1}$ 以及此时的速度量 $u_{k-1}$ (包含线速度v，角速度（本例中为0））。

希望得到的参数是：机器人目前的位置 $x_{k}$ ，线性模型表示如下：

$x_{k}$ = A $x_{k-1}$ + B $u_{k-1}$

A和B矩阵如下给定：

  Matrix A(2,2);
  A(1,1) = 1.0;
  A(1,2) = 0.0;
  A(2,1) = 0.0;
  A(2,2) = 1.0;

  Matrix B(2,2);
  B(1,1) = cos(0.8);
  B(1,2) = 0.0;
  B(2,1) = sin(0.8);
  B(2,2) = 0.0;

将两个矩阵组合成一个向量vector：

  vector<Matrix> AB(2);
  AB[0] = A;
  AB[1] = B;

之后我们用均值向量μ以及一个协方差矩阵cov生成高斯分布以代表预测位置的不确定性。均值由MU_SYSTEM_NOISE_X和MU_SYSTEM_NOISE_Y确定：

  ColumnVector sysNoise_Mu(2);
  sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
  sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;

将协方差矩阵选为对角矩阵，对角线上为MU_SYSTEM_NOISE_X和MU_SYSTEM_NOISE_Y：

  SymmetricMatrix sysNoise_Cov(2);
  sysNoise_Cov = 0.0;
  sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
  sysNoise_Cov(1,2) = 0.0;
  sysNoise_Cov(2,1) = 0.0;
  sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;

均值与协方差矩阵一起定义二维高斯分布：

  Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);

创造一个线性条件概率密度函数（pdf）用以代表在给定当前机器人位置情况下，机器人预测位置的概率。这个概率密度函数由两个参数构成：AB代表线性模型本身，高斯分布则代表系统模型的额外不确定度：

  LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);

终于我们通过系统的概率密度函数创造出整个预测系统的模型：

  LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);

线性测量更新

现在为了纠正机器人移动预测的位置，基于机器人距离墙距离的测量值，建立测量更新模型。本例中使用线性测量更新模型，BFL包中已经包装好了。

首先查看将测量值与机器人位置联系起来的数学公式：

$z_{k+1}$ = H $x_{k+1}$

我们建立矩阵H如下：

  Matrix H(1,2);
  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
  H = 0.0;
  H(1,1) = wall_ct * RICO_WALL;
  H(1,2) = 0 - wall_ct;

其中，wall_ct是一个帮助常数，RICO_WALl是墙的斜率。然后我们以传感器本身的不确定性来建立一个高斯分布，这个高斯分布同样包含均值μ以及协方差矩阵cov：

  ColumnVector measNoise_Mu(1);
  measNoise_Mu(1) = MU_MEAS_NOISE; //噪音均值
  SymmetricMatrix measNoise_Cov(1);
  measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;  //噪音协方差矩阵（σ^2）
  Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);

现在可以创建测量可能的概率密度模型，这个概率密度模型由表示测量系统本身的矩阵H以及代表测量不确定性的高斯分布构成。最后根据这个概率密度模型我们可以构建测量更新模型：

  LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
    LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);

先验分布

现在建立代表初始估计与不确定度的先验分布，同样包含了均值以及协方差如下所示：

  ColumnVector prior_Mu(2);
  prior_Mu(1) = PRIOR_MU_X;
  prior_Mu(2) = PRIOR_MU_Y;
  SymmetricMatrix prior_Cov(2);
  prior_Cov(1,1) = PRIOR_COV_X;
  prior_Cov(1,2) = 0.0;
  prior_Cov(2,1) = 0.0;
  prior_Cov(2,2) = PRIOR_COV_Y;
  Gaussian prior(prior_Mu, prior_Cov);

构造滤波器

本例中希望估计机器人的位置，BFL为此提供了ExtendedKalmanFilter类，这个类需要的唯一一个参数是先验分布：

  ExtendedKalmanFilter filter(&prior_cont);

提示

值得一提的是扩展卡尔曼滤波器也可以去处理非线性预测与（或）非线性测量更新。当刚接触BFL包的时候，为了建立一个简单的卡尔曼滤波器，可能首先想到使用类KalmanFilter（KalmanFilter.h）去定义。然而KalmanFilter类表示所有卡尔曼滤波器家族（扩展卡尔曼滤波器EKF，迭代扩展卡尔曼滤波器IEKF）。系统通过这个基类去更新后验密度。然而对应不同的卡尔曼滤波器需要不同的参数，这也是为什么该类中全是纯虚函数。因此，本例中满足我们需求的类是ExtendedKalmanFilter。

机器人仿真

本文中的仿真机器人是在开放世界有一堵墙，机器人在这个开放世界中移动。机器人的相关参数被定义在mobile_robot_wall_cts.h中（例如初始位置，仿真噪声等）。机器人仿真这样声明：

  MobileRobot mobile_robot;

仿真器有三个方法：

Move( input )
这个方法让机器人以特定输入速度移动，持续一秒。
Measure()
这个方法返回目前机器人所在位置与墙距离的测量值
GetState()
这个方法返回机器人的真正位置，为了与估计结果进行比较

每个时间步长内机器人会接受到输入速度，当他到达新位置后，会返回测量值（与墙的距离）：

  mobile_robot.Move( input );
  ColumnVector measurement = mobile_robot.Measure();

更新滤波器以及新的估计

每个时间步长内，通过提供预测与测量更新模型，都可以得到一个新的估计。如果同时提供预测与测量更新模型去更新滤波器的话，滤波器如下更新：

  filter->Update(&sys_model,input,&meas_model, measurement);

如果只通过预测模型去更行滤波器的话，如下所示：

  Pdf<ColumnVector> * posterior = filter->PostGet();

想得到所有预测与测量更新模型的后验（概率）或者只想得到期望值或者协方差矩阵可以如下操作：

  Pdf<ColumnVector> * posterior = filter->PostGet();
  posterior->ExpectedValueGet();
  posterior->CovarianceGet();

结果

上图为不使用测量信息，下图为使用测量信息：
　　此处输入图片的描述
　　

C++中贝叶斯滤波器包bfl的使用(1)-线性卡尔曼滤波器