Implementación del método secuencial monte carlo (filtros de partículas)

Question

Estoy interesado en el algoritmo simple para el filtro de partículas dado aquí: http://www.aiqus.com/upfiles/PFAlgo.png Parece muy simple, pero no tengo idea de cómo hacerlo prácticamente. ¿Alguna idea sobre cómo implementarlo (solo para comprender mejor cómo funciona)?

Editar: Este es un gran ejemplo sencillo que explica cómo funciona: http://www.aiqus.com/questions/39942/very-simple-particle-filters-algorithm-sequential-monte-carlo-method-implementation?page=1#39950

He tratado de ponerlo en práctica en C++: http://pastebin.com/M1q1HcN4 pero estoy seguro de que si observo que hago de la manera correcta . ¿Puedes verificar si lo entendí bien o hay algún malentendido según mi código?

#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 es muy fácil y sencillo de entender y para mí explica muy bien los filtros de partículas.

Se llama "Programación de un automóvil robótico", y se trata de tres métodos de localiczation: localización de Monte Carlo, filtros de Kalman y filtros de partículas.

El curso es completamente gratuito (está terminado ahora, por lo que no puede participar activamente, pero aún puede ver las clases), impartido por un profesor de Stanford. Las "clases" me resultaron sorprendentemente fáciles de seguir, y van acompañadas de pequeños ejercicios, algunos de ellos lógicos, pero una buena parte de ellos programados. Además, asignaciones de tareas con las que puedes jugar.

En realidad te hace escribir tu propio código en python para todos los filtros, que también prueban para ti. Al final del curso, debe tener los 3 filtros implementados en Python.

Te recomendamos echarle un vistazo.

Answer 2

Éstos son algunos buenos enlaces a la fuente:

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 (algoritmo de condensación)

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

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