14

我对这里给出的粒子过滤器的简单算法感兴趣：http ://www.aiqus.com/upfiles/PFAlgo.png看起来很简单，但我不知道如何实际操作。关于如何实现它的任何想法（只是为了更好地理解它是如何工作的）？

编辑： 这是一个很好的简单示例，解释了它是如何工作的：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();
}
4

2 回答 2

21

这个在线课程非常容易理解，对我来说它很好地解释了粒子过滤器。

它被称为“编程机器人汽车”，它谈到了三种定位方法：蒙特卡洛定位、卡尔曼滤波器和粒子滤波器。

该课程完全免费（现已结束，因此您无法积极参与，但您仍然可以观看讲座），由斯坦福教授教授。这些“课程”对我来说非常容易上手，并且伴随着一些小练习——其中一些只是合乎逻辑的，但很多都是编程。此外，您可以玩的家庭作业。

它实际上让您在 python 中为所有过滤器编写自己的代码，它们也为您测试。在课程结束时，您应该已经在 python 中实现了所有 3 个过滤器。

强烈建议去看看。

于 2012-05-15T12:43:42.763 回答
3

以下是一些很好的来源链接：

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/

于 2012-05-04T20:48:07.837 回答