C++中贝叶斯滤波器包bfl的使用(2)-预测模型非线性的扩展卡尔曼滤波器

摘要

线性卡尔曼滤波器的教程已经翻译完。可是在实际生产生活中，线性状态转移和测量是很少见的，此时就需要扩展卡尔曼滤波器（EKF）登场了。同时在robot_pose_ekf包中使用的也是EKF。

接下来只需要额外的10分钟，就能够为非线性系统模型建立扩展卡尔曼滤波器。这个例子中，移动机器人在有一面墙的空间内移动。机器人拥有测量与墙距离的传感器，我们通过一个卡尔曼滤波器估计机器人的位置和方向。Let’s waaaaaaaaagh！！！！

---

---

准备.CPP文件

在文件夹BFL/examples/nonlinear_kalman/中可以找到test_nonlinear_kalman.cpp。这个文件中的头文件和上一个教程中的相同，只是多包含了一个我们将自己编写的头文件：

  #include "nonlinearanalyticconditionalgaussianmobile.h"

---

非线性预测模型

与上一个教程的预测模型相比，本教程中的预测模型不仅包含机器人方向而且非线性。

仍然将系统噪声视为高斯分布，我们以均值μ和协方差矩阵cov构建高斯分布以代表对机器人位置和方向预测的不确定性。均值以及协方差矩阵的值均定义在mobie_robot_wall_cts.h，表达式如下：

  //预测噪音均值mu
  ColumnVector sys_noise_Mu(STATE_SIZE);
  sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
  sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
  sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
  //预测噪音协方差矩阵cov，为对角矩阵
  SymmetricMatrix sys_noise_Cov(STATE_SIZE);
  sys_noise_Cov = 0.0;
  sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
  sys_noise_Cov(1,2) = 0.0;
  sys_noise_Cov(1,3) = 0.0;
  sys_noise_Cov(2,1) = 0.0;
  sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
  sys_noise_Cov(2,3) = 0.0;
  sys_noise_Cov(3,1) = 0.0;
  sys_noise_Cov(3,2) = 0.0;
  sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
  //构建一个三维高斯分布以代表预测模型的不确定性
  Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);

现在假设已经知道机器人目前的位置和方向，接下来需要做的就是构建机器人位置预测的非线性概率密度函数。BFL本身并没有为我们提供非线性概率密度函数，所以我们只能自己来。

在本例中，现在将要创建的类起个名字：NonLinearAnalyticConditionalGaussianMobile。BFL里提供了拥有高斯不确定度的解析概率密度模型AnalyticConditionalGaussianAdditiveNoise，我们可以从中继承一些公式。毕竟我们的模型也是解析的并且增加了高斯噪音。

为了生成我们自己的类，让我们创建一个头文件：nonlinearanalyticconditionalgaussianmobile.h。在这个头文件中首先include将要继承类的头文件：

  #include 

这个类将会在BFL命名空间下生成，当然需要构造函数和析构函数。构造函数中希望可以有代表预测模型噪音的高斯分布：

  namespace BFL
  {
   class NonLinearAnalyticConditionalGaussianMobile : 
    public AnalyticConditionalGaussianAdditiveNoise
   {
    public:
     NonLinearAnalyticConditionalGaussianMobile(const Gaussian& additiveNoise);
     virtual ~NonLinearAnalyticConditionalGaussianMobile();
   };
  }

点开analyticconditionalgaussian_additivenoise.h，发现至少有两种方法我们必须要继承：

• ColumnVector ExpectedValueGet()
  返回条件概率密度的期望值，期望值由过程方程决定
• Matrix dfGet(unsigned int i)
  求第i个条件参数求偏导数。本例中只对状态（第一个条件参数）求偏导数。

不仅如此，这两个方法还是我们唯二需要重新声明的函数，因此在头文件的public部分加入下面2行：

  virtual MatrixWrapper::ColumnVector    ExpectedValueGet() 	const;
  virtual MatrixWrapper::Matrix          dfGet(unsigned int i)  const;

好样的！头文件已经准备完毕了，回到nonlinearanalyticconditionalgaussianmobile.cpp中去写表达式吧。

条件概率密度的条件参数，状态与输入都是条件概率密度的私有变量。

让我们看看ExpectedValueGet()，为了计算条件概率密度函数的期望值，我们需要知道当前的状态与输入量：

  ColumnVector state = ConditionalArgumentGet(0);
  ColumnVector vel  = ConditionalArgumentGet(1);
  //计算并返回期望值
  state(1) += cos(state(3)) * vel(1);
  state(2) += sin(state(3)) * vel(1);
  state(3) += vel(2);
  return state + AdditiveNoiseMuGet();

现在只剩下dfGet(unsigned int i)，我们只需要对第一个条件变量（也就是状态参数）求导：

  //首先得到现在的条件参数，与上面的类似
  ColumnVector state = ConditionalArgumentGet(0);
  ColumnVector vel = ConditionalArgumentGet(1);
  // 3×3矩阵求导
  Matrix df(3,3);
  df(1,1)=1;
  df(1,2)=0;
  df(1,3)=-vel(1)*sin(state(3));
  df(2,1)=0;
  df(2,2)=1;
  df(2,3)=vel(1)*cos(state(3));
  df(3,1)=0;
  df(3,2)=0;
  df(3,3)=1;
  return df;

上面完成了我们自己的预测模型，现在我们可以创建非线性条件密度概率函数的实例以及其预测模型了：

  NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);
  AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);

---

线性测量更新模型

现在建立测量更新模型。与上个例子，因为状态量多了一个机器人方向，所以测量更新模型多了额外的0，其他步骤与之前的例子一样，如下所示：

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

---

先验分布

与上个例子相比，先验分布增加一个方向变量：

  ColumnVector prior_Mu(STATE_SIZE);
  prior_Mu(1) = PRIOR_MU_X;
  prior_Mu(2) = PRIOR_MU_Y;
  prior_Mu(3) = PRIOR_MU_THETA;
  //协方差矩阵是对角矩阵，
  SymmetricMatrix prior_Cov(STATE_SIZE);
  prior_Cov(1,1) = PRIOR_COV_X;
  prior_Cov(1,2) = 0.0;
  prior_Cov(1,3) = 0.0;
  prior_Cov(2,1) = 0.0;
  prior_Cov(2,2) = PRIOR_COV_Y;
  prior_Cov(2,3) = 0.0;
  prior_Cov(3,1) = 0.0;
  prior_Cov(3,2) = 0.0;
  prior_Cov(3,3) = PRIOR_COV_THETA;
  //均值与协方差矩阵构成一个三维的高斯分布
  Gaussian prior(prior_Mu, prior_Cov);

---

构造滤波器、机器人仿真和更新滤波器以及新的估计

本例中使用扩展卡尔曼滤波器估计移动机器人的位置，因此与上个例子完全一样：

  ExtendedKalmanFilter filter(&prior_cont);

机器人仿真和更新滤波器以及新的估计与上个例子完全一样

---

结果

上图为不使用测量信息，下图为使用测量信息：