天天看點
傳回

C++中貝葉斯濾波器包bfl的使用(3)-預測模型和測量更新模型皆非線性的粒子濾波器準備.cpp檔案非線性預測模型非線性測量更新模型非連續初始分布構造濾波器結論

摘要

在完成上面兩個教程後，這個教程會一步步的教你建立一個預測模型和測量更新模型皆非線性的濾波器。仿真條件和第二個例子中相同。

連續寫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 (…);

這個函數會在預測機率分布中取樣。為了實作這個函數，需要在頭檔案中先聲明：

頭檔案準備就緒，傳回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);

提示

代碼：

每次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);

這個函數允許粒子濾波器計算傳感器測量的可能性。在頭檔案中添加如下聲明：

頭檔案準備就緒，傳回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）

結論

視訊下載下傳位址

不使用測量更新的顆粒移動視訊下載下傳

使用測量更新的顆粒移動視訊下載下傳

卡爾曼濾波器 c++ ros 粒子濾波器
上一篇: Django關于FileResponse，HTTPResponse， streamingHttpRespones下載下傳檔案問題說明
下一篇: 【轉載】C++ typedef用法小結

繼續閱讀