深度学习可学习的滤波器组件滤波器模型

转载

mob6454cc6441b6 2024-07-08 09:53:28

文章标签 深度学习可学习的滤波器组件 C++ ROS 粒子滤波器预测模型 文章分类 深度学习人工智能

摘要

在完成上面两个教程后，这个教程会一步步的教你建立一个预测模型和测量更新模型皆非线性的滤波器。仿真条件和第二个例子中相同。

连续写3个果然有点累，waaaaaagh不动了。

再额外花15分钟建立你的粒子滤波器

准备.cpp文件
非线性预测模型

提示

非线性测量更新模型
非连续初始分布
构造滤波器
结论

准备.cpp文件

在文件夹BFL/examples/nonlinear_particle/中将会找到源文件test_nonlinear_particle.cpp，其头文件与第一个和第二个例子相同除了其中需要包含合适的滤波器文件以及这一节中我们将要建立的非线性测量更新模型：

#include <filter/bootstrapfilter.h>
  #include "nonlinearSystemPdf.h"
  #include "nonlinearMeasurementPdf.h"

非线性预测模型

我们当然可以直接把上个例子中的非线性预测模型直接拿来用，但是这里想要将预测模型推广至更一般的情况。本例中的非线性预测模型并不会局限于高斯分布噪声。
我们仍然假设预测模型的噪声符合高斯分布（就是因为高斯分布简单，你也没必要局限于高斯分布），具体是什么意思之前都说明过了，这里直接上代码：

//均值
  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;
  //协方差矩阵
  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并没有为我们提供这样的非线性条件pdf，所以我们需要自己编写。本例中我们给它起名：NonlinearSystemPdf。BFL为我们提供了最一般情况下的条件概率密度函数ConditionalPdf，我们可以从该类中继承函数。

为了生成我们自己的类，先从头文件开始吧nonlinearSystemPdf.h：

//需要继承的类的头文件
  #include <pdf/conditionalpdf.h>
  //构造函数和析构函数，构造函数需要一个代表预测模型噪音的高斯分布
  namespace BFL
  {
   class NonLinearSystemPdf: 
     public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector>
   {
    public:
     NonlinearSystemPdf( const Gaussian& additiveNoise);
     virtual ~NonlinearSystemPdf();
   };
  }

查看ConditionalPdf的文档，可以看到其中有很多未实现的方法。本例因为是使用粒子滤波的预测模型，所以只要实现这个就可以了：

bool SampleFrom (…);

这个函数会在预测概率分布中取样。为了实现这个函数，需要在头文件中先声明：

virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, ...)

头文件准备就绪，返回cpp文件实现这些函数nonlinearSystemPdf.cpp。

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

现在我们回到SampleFrom(...)：

//为了计算条件pdf的期望值，必须知道现在的状态和输入：
  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);
  //从加入的高斯噪音中取样：
  Sample<ColumnVector> noise;
  _additiveNoise.SampleFrom(noise, method, args);
  //将输出结果保存在输出变量中：
  one_sample.ValueSet(state + noise.ValueGet());
  //模型建立已结束，终于可以建立非线性条件概率密度函数及其模型：
  NonlinearSystemPdf sys_pdf(system_Uncertainty);
  SystemModel<ColumnVector> sys_model(&sys_pdf);

提示

代码：

Sample<ColumnVector> noise;

每次SampleFrom被调用时都会被分配。想要实时使用噪音则应该在构建的时候分配。

非线性测量更新模型

我们当然可以直接把上个例子中的非线性测量更新模型直接拿来用，但是这里想要将测量更新模型推广至更一般的情况。本例中的非线性测量更新模型并不会局限于高斯分布噪声。

我们仍然假设预测模型的噪声符合高斯分布（就是因为高斯分布简单，你也没必要局限于高斯分布），具体是什么意思之前都说明过了，这里直接上代码：

ColumnVector meas_noise_Mu(MEAS_SIZE);
  meas_noise_Mu(1) = MU_MEAS_NOISE;
  SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
  meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
  Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);

现在已知机器人当前位置，创造一个代表传感器测量距离可能性的一般非线性条件pdf。BFL并没有为我们提供这样的非线性条件pdf，所以我们需要自己编写。本例中我们给它起名：NonlinearMeasurementPdf。BFL为我们提供了最一般情况下的条件概率密度函数ConditionalPdf，我们可以从该类中继承函数。

为了生成我们自己的类，先从头文件开始吧nonlinearMeasurementPdf.h：

//需要继承的类的头文件
  #include <pdf/conditionalpdf.h>
  //构造函数和析构函数，构造函数需要一个代表测量更新模型噪音的高斯分布
  namespace BFL
  {
   class NonLinearSystemPdf: 
     public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector>
   {
    public:
     NonlinearSystemPdf( const Gaussian& additiveNoise);
     virtual ~NonlinearSystemPdf();
   };
  }

查看ConditionalPdf的文档，可以看到其中有很多未实现的方法。本例因为是使用粒子滤波的预测模型，所以只要实现这个就可以了：

Probability ProbabilityGet(meas);

这个函数允许粒子滤波器计算传感器测量的可能性。在头文件中添加如下声明：

virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const;

头文件准备就绪，返回cpp文件实现这些函数nonlinearMeasurementPdf.cpp。

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

现在我们回到ProbabilityGet()：

//为了计算条件pdf的数学期望，需要目前的状态和输入：
  ColumnVector state = ConditionalArgumentGet(0);
  ColumnVector vel  = ConditionalArgumentGet(1);
  //期望值直接计算：
  ColumnVector expected_measurement(1);
  expected_measurement(1) = 2 * state(2);
  //返回期望值的可能性（这里是返回在加入测量噪音之后，实际测量值与期望值直接差别的可能性）：
   return _measNoise.ProbabilityGet(expected_measurement-measurement);
  //模型建立已结束，终于可以建立非线性条件概率密度函数及其模型： 
  NonlinearMeasurementPdf meas_pdf(measurement_Uncertainty);
  MeasurementModel<ColumnVector,ColumnVector> meas_model(&meas_pdf);

非连续初始分布

为了使用粒子滤波器，需要用采样形式表示的非连续初始分布。上个例子中我们已经有了连续初始分布可以对其进行采样：在mobile_robot_wall_cts.h中设定常数代表采样个数：

//在mobile_robot_wall_cts.h中设定常数代表采样个数：
  #define NUM_SAMPLES  2000
  //从之前的连续性模型中直接采样，并把采样结果保存在一个vector中：
  vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
  prior.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
  //通过这些采样结果建立非连续初始分布，一个蒙特卡罗pdf：
  MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,3);
  prior_discr.ListOfSamplesSet(prior_samples);

构造滤波器

本例中使用自举滤波器去估计机器人的位置。自举滤波器如下创造：

BootstrapFilter<ColumnVector,ColumnVector> filter(&prior_discr, 0, NUM_SAMPLES/4.0)
  //三个参数含义如下：
  //1.非连续初始分布
  //2.静态采样的采样时间（不需要则为0）
  //3.动态采样的阈值（不需要则为0）

本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。