Реализация последовательного метода monte carlo (фильтры частиц)

Question

Мне интересен простой алгоритм фильтрации частиц, приведенный здесь: http://www.aiqus.com/upfiles/PFAlgo.png Кажется очень простым, но я понятия не имею, как это сделать практически. Любая идея о том, как ее реализовать (чтобы лучше понять, как это работает)?

Edit: Это отличный пример, который объясняет, как это работает: http://www.aiqus.com/questions/39942/very-simple-particle-filters-algorithm-sequential-monte-carlo-method-implementation?page=1#39950

Я попытался реализовать в C++: http://pastebin.com/M1q1HcN4, но я уверен, обратите внимание, если я это правильно , Можете ли вы проверить, хорошо ли я это понял, или есть некоторые недоразумения в соответствии с моим кодом?

#include <iostream> 
#include <vector> 
#include <boost/random/mersenne_twister.hpp> 
#include <boost/random/uniform_01.hpp> 
#include <boost/random/uniform_int_distribution.hpp> 

using namespace std; 
using namespace boost; 

double uniform_generator(void); 

#define N 4 // number of particles 

#define evolutionProba_A_A 1.0/3.0 // P(X_t = A | X_t-1 = A) 
#define evolutionProba_A_B 1.0/3.0 // P(X_t = A | X_t-1 = B) 
#define evolutionProba_B_B 2.0/3.0 // P(X_t = B | X_t-1 = B) 
#define evolutionProba_B_A 2.0/3.0 // P(X_t = B | X_t-1 = A) 

#define observationProba_A_A 4.0/5.0 // P(Y_t = A | X_t = A) 
#define observationProba_A_B 1.0/5.0 // P(Y_t = A | X_t = B) 
#define observationProba_B_B 4.0/5.0 // P(Y_t = B | X_t = B) 
#define observationProba_B_A 1.0/5.0 // P(Y_t = A | X_t = A) 

/// =========================================================================== 

typedef struct distrib { float PA; float PB; } Distribution; 

typedef struct particle 
{ 
    Distribution distribution; // e.g. <0.5, 0.5> 
    char state; // e.g. 'A' or 'B' 
    float weight; // e.g. 0.8 
} 
Particle; 

/// =========================================================================== 

int main() 
{ 
    vector<char> Y; // data observations 
    Y.push_back('A'); Y.push_back('B'); Y.push_back('A'); Y.push_back('A'); Y.push_back('A'); Y.push_back('B'); 
    Y.push_back('A'); Y.push_back('A'); Y.push_back('B'); Y.push_back('A'); Y.push_back('B'); Y.push_back('A'); 
    Y.push_back('A'); Y.push_back('B'); Y.push_back('B'); Y.push_back('A'); Y.push_back('A'); Y.push_back('B'); 

    vector< vector<Particle> > Xall; // vector of all particles from time 0 to t 

    /// Step (1) Initialisation 
    vector<Particle> X; // a vector of N particles 
    for(int i = 0; i < N; ++i) 
    { 
     Particle x; 

     // sample particle Xi from initial distribution 
     x.distribution.PA = 0.5; x.distribution.PB = 0.5; 
     float r = uniform_generator(); 
     if(r <= x.distribution.PA) x.state = 'A'; // r <= 0.5 
     if(x.distribution.PA < r && r <= x.distribution.PA + x.distribution.PB) x.state = 'B'; // 0.5 < r <= 1 

     X.push_back(x); 
    } 

    Xall.push_back(X); 
    X.clear(); 

    /// Observing data 
    for(int t = 1; t <= 18; ++t) 
    { 
     char y = Y[t-1]; // current observation 

     /// Step (2) Importance sampling 
     float sumWeights = 0; 
     vector<Particle> X; // a vector of N particles 
     for(int i = 0; i < N; ++i) 
     { 
      Particle x; 

      // P(X^i_t = A) = P(X^i_t = A | X^i_t-1 = A) * P(X^i_t-1 = A) + P(X^i_t = A | X^i_t-1 = B) * P(X^i_t-1 = B) 
      x.distribution.PA = evolutionProba_A_A * Xall[t-1][i].distribution.PA + evolutionProba_A_B * Xall[t-1][i].distribution.PB; 

      // P(X^i_t = B) = P(X^i_t = B | X^i_t-1 = A) * P(X^i_t-1 = A) + P(X^i_t = B | X^i_t-1 = B) * P(X^i_t-1 = B) 
      x.distribution.PB = evolutionProba_B_A * Xall[t-1][i].distribution.PA + evolutionProba_B_B * Xall[t-1][i].distribution.PB; 

      // sample the a particle from this distribution 
      float r = uniform_generator(); 
      if(r <= x.distribution.PA) x.state = 'A'; 
      if(x.distribution.PA < r && r <= x.distribution.PA + x.distribution.PB) x.state = 'B'; 

      // compute weight of this particle according to the observation y 
      if(y == 'A') 
      { 
       if(x.state == 'A') x.weight = observationProba_A_A; // P(y = A | X^i_t = A) 
       else if(x.state == 'B') x.weight = observationProba_A_B; // P(y = A | X^i_t = B) 
      } 
      else if(y == 'B') 
      { 
       if(x.state == 'A') x.weight = observationProba_B_A; // P(y = B | X^i_t = A) 
       else if(x.state == 'B') x.weight = observationProba_B_B; // P(y = B | X^i_t = B) 
      } 

      sumWeights += x.weight; 

      X.push_back(x); 
     } 

     // normalise weights 
     for(int i = 0; i < N; ++i) 
      X[i].weight /= sumWeights; 

     /// Step (3) resampling N particles according to weights 
     float PA = 0, PB = 0; 
     for(int i = 0; i < N; ++i) 
     { 
      if(X[i].state == 'A') PA += X[i].weight; 
      else if(X[i].state == 'B') PB += X[i].weight; 
     } 

     vector<Particle> reX; // new vector of particles 
     for(int i = 0; i < N; ++i) 
     { 
      Particle x; 

      x.distribution.PA = PA; 
      x.distribution.PB = PB; 

      float r = uniform_generator(); 
      if(r <= x.distribution.PA) x.state = 'A'; 
      if(x.distribution.PA < r && r <= x.distribution.PA + x.distribution.PB) x.state = 'B'; 

      reX.push_back(x); 
     } 

     Xall.push_back(reX); 
    } 

    return 0; 
} 

/// =========================================================================== 

double uniform_generator(void) 
{ 
    mt19937 gen(55); 
    static uniform_01< mt19937, double > uniform_gen(gen); 
    return uniform_gen(); 
} 

Answer 1

This online course очень прост и понятен, и для меня он очень хорошо объяснил фильтры частиц.

Это называется «Программирование роботизированного автомобиля», и в нем рассказывается о трех методах локализации: локализации Монте-Карло, фильтрах Калмана и фильтрах частиц.

Курс полностью бесплатный (теперь он закончен, поэтому вы не можете активно участвовать, но вы все еще можете следить за лекциями), преподаваемым профессором Стэнфорда. «Классы» были для меня удивительно легкими, и они сопровождаются небольшими упражнениями - некоторые из них просто логичны, но многие из них программируют. Кроме того, домашние задания, с которыми вы можете играть.

Это действительно заставляет вас писать свой собственный код в python для всех фильтров, которые они также тестируют для вас. К концу курса вы должны иметь все 3 фильтра, реализованных в python.

Настоятельно рекомендуем проверить его.

Answer 2

Вот несколько хороших ссылок на источник:

Matlab: http://wiki.esl.fim.uni-passau.de/index.php/Filtering_III:_Kalman_and_Particle_Filter

C: http://www.robots.ox.ac.uk/~misard/condensation.html (Конденсация алгоритм)

C++: http://robotics.usc.edu/~boyoon/particle.html

Java: http://www.oursland.net/projects/particlefilter/