Particle filter 연습

1차원 particle filter 간단 예제

// 1-D Particle filter algorithm exercise
// 2009-11-06
// ref. Probabilistic Robotics: 98p

#include <iostream>
#include <cstdlib> //defining RAND_MAX
#include <ctime> //time as a random seed
#include <cmath>
using namespace std;

#define PI 3.14159265
#define N 10 //number of particles

////////////////////////////////////////////////////////////////////////////
// random number generators written by kyu
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}

////////////////////////////////////////////////////////////////////////////


int main (int argc, char * const argv[]) {
   
    double groundtruth[] = {0.5, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
    double measurement[] = {0.4, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
    double transition_noise = 0.3; // covariance of Gaussian noise to control
    double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
   
    double x[N]; // N particles
    double x_p[N]; // predicted particles
    double state; // estimated state with particles
    double x_u[N]; // temporary variables for updating particles
    double v[N]; // velocity
    double v_u[N]; // temporary variables for updating velocity   
    double m[N]; // measurement
    double l[N]; // likelihood
    double lsum; // sum of likelihoods
    double w[N]; // weight of each particle
    double a[N]; // portion between particles
   
    double grn[N]; // Gaussian random number
    double urn[N]; // uniform random number
   
    srand(time(NULL));        
   
    // initialize particles
    for (int n = 0; n < N; n++)
    {
        x[n] = 0.0;
        v[n] = 0.0;
        w[n] = (double)1/(double)N;           
    }
   
    int step = 7;
    for (int t = 0; t < step; t++)
    {
        cout << "step " << t << endl;       
        // measure
        m[t] = measurement[t];
       
        cout << "groundtruth = " << groundtruth[t] << endl;
        cout << "measurement = " << measurement[t] << endl;       
       
        lsum = 0;
        for (int n = 0; n < N; n++)
        {
            // predict
            grn[n] = gaussian_random();
            x_p[n] = x[n] + v[n] + transition_noise * grn[n];
//            cout << grn[n] << endl;
           
            // estimate likelihood between measurement and each prediction
            l[n] = normal_distribution(m[t], measurement_noise, x_p[n]); // ref. 14p eqn(2.3)
            lsum += l[n];
        }
//            cout << "lsum = " << lsum << endl;       
       
        // estimate states       
        state = 0;
        for (int n = 0; n < N; n++)
        {
            w[n] = l[n] / lsum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state += w[n] * x_p[n]; // estimate the state with particles
        }   
        cout << "estimation = " << state << endl;
       
        // update       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int select;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    select = k;
                    break;
                }
            }
            cout << "select" << n << "= " << select << "  ";       
            // retain the selection 
            x_u[n] = x_p[select];
            v_u[n] = x_p[select] - x[select];
        }
        cout << endl << endl;
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            x[n] = x_u[n];
            v[n] = v_u[n];
//            cout << "v" << n << "= " << v[n] << "  ";   
        }
    }
   
    return 0;
}

실행 결과:

실행 결과 (N = 10):

step 0
groundtruth = 0.5
measurement = 0.4
estimation = 0.118585
select0= 8  select1= 6  select2= 8  select3= 4  select4= 2  select5= 5  select6= 5  select7= 5  select8= 6  select9= 1 

step 1
groundtruth = 2
measurement = 2.1
estimation = 0.620147
select0= 1  select1= 1  select2= 1  select3= 1  select4= 1  select5= 1  select6= 1  select7= 1  select8= 1  select9= 1 

step 2
groundtruth = 3.5
measurement = 3.2
estimation = 1.37839
select0= 6  select1= 6  select2= 6  select3= 1  select4= 6  select5= 6  select6= 6  select7= 6  select8= 6  select9= 6 

step 3
groundtruth = 5
measurement = 5.3
estimation = 2.70976
select0= 8  select1= 8  select2= 8  select3= 8  select4= 8  select5= 8  select6= 8  select7= 8  select8= 8  select9= 8 

step 4
groundtruth = 7
measurement = 7.4
estimation = 4.41965
select0= 9  select1= 9  select2= 9  select3= 9  select4= 1  select5= 9  select6= 1  select7= 9  select8= 9  select9= 9 

step 5
groundtruth = 8
measurement = 8.1
estimation = 6.80852
select0= 3  select1= 3  select2= 3  select3= 3  select4= 3  select5= 3  select6= 3  select7= 3  select8= 3  select9= 3 

step 6
groundtruth = 10
measurement = 9.6
estimation = 9.34482
select0= 5  select1= 5  select2= 5  select3= 9  select4= 9  select5= 5  select6= 5  select7= 8  select8= 0  select9= 4 

실행 결과 (N = 100):

step 0
groundtruth = 0.5
measurement = 0.4
estimation = 0.149642
select0= 55  select1= 89  select2= 41  select3= 14  select4= 54  select5= 4  select6= 65  select7= 64  select8= 69  select9= 42  select10= 70  select11= 98  select12= 75  select13= 69  select14= 54  select15= 63  select16= 93  select17= 2  select18= 90  select19= 96  select20= 90  select21= 58  select22= 36  select23= 65  select24= 69  select25= 25  select26= 31  select27= 36  select28= 96  select29= 54  select30= 60  select31= 75  select32= 26  select33= 27  select34= 77  select35= 54  select36= 47  select37= 63  select38= 59  select39= 76  select40= 75  select41= 59  select42= 3  select43= 58  select44= 31  select45= 57  select46= 77  select47= 69  select48= 45  select49= 8  select50= 76  select51= 36  select52= 54  select53= 29  select54= 60  select55= 67  select56= 26  select57= 69  select58= 40  select59= 90  select60= 5  select61= 36  select62= 98  select63= 1  select64= 25  select65= 20  select66= 39  select67= 73  select68= 90  select69= 84  select70= 41  select71= 87  select72= 76  select73= 20  select74= 76  select75= 42  select76= 98  select77= 57  select78= 99  select79= 75  select80= 23  select81= 93  select82= 62  select83= 14  select84= 90  select85= 81  select86= 41  select87= 23  select88= 69  select89= 75  select90= 73  select91= 14  select92= 55  select93= 55  select94= 58  select95= 9  select96= 65  select97= 62  select98= 26  select99= 76 

step 1
groundtruth = 2
measurement = 2.1
estimation = 1.36152
select0= 50  select1= 72  select2= 77  select3= 96  select4= 52  select5= 31  select6= 3  select7= 83  select8= 31  select9= 45  select10= 23  select11= 23  select12= 31  select13= 96  select14= 96  select15= 3  select16= 77  select17= 77  select18= 3  select19= 72  select20= 3  select21= 45  select22= 31  select23= 83  select24= 3  select25= 31  select26= 31  select27= 3  select28= 77  select29= 31  select30= 3  select31= 72  select32= 22  select33= 3  select34= 72  select35= 3  select36= 3  select37= 31  select38= 23  select39= 96  select40= 40  select41= 45  select42= 31  select43= 77  select44= 31  select45= 41  select46= 3  select47= 77  select48= 96  select49= 77  select50= 77  select51= 72  select52= 22  select53= 72  select54= 72  select55= 83  select56= 83  select57= 3  select58= 83  select59= 77  select60= 31  select61= 35  select62= 72  select63= 3  select64= 45  select65= 96  select66= 31  select67= 96  select68= 77  select69= 77  select70= 41  select71= 31  select72= 83  select73= 31  select74= 31  select75= 45  select76= 31  select77= 3  select78= 72  select79= 23  select80= 31  select81= 22  select82= 3  select83= 52  select84= 45  select85= 31  select86= 77  select87= 3  select88= 40  select89= 31  select90= 45  select91= 40  select92= 31  select93= 41  select94= 23  select95= 3  select96= 31  select97= 31  select98= 41  select99= 77 

step 2
groundtruth = 3.5
measurement = 3.2
estimation = 2.87022
select0= 12  select1= 76  select2= 76  select3= 82  select4= 44  select5= 12  select6= 85  select7= 82  select8= 80  select9= 85  select10= 5  select11= 12  select12= 42  select13= 70  select14= 74  select15= 80  select16= 82  select17= 2  select18= 57  select19= 74  select20= 76  select21= 12  select22= 2  select23= 74  select24= 2  select25= 82  select26= 65  select27= 5  select28= 76  select29= 2  select30= 74  select31= 74  select32= 26  select33= 85  select34= 76  select35= 92  select36= 2  select37= 96  select38= 26  select39= 2  select40= 80  select41= 86  select42= 26  select43= 44  select44= 82  select45= 65  select46= 12  select47= 12  select48= 2  select49= 76  select50= 26  select51= 1  select52= 80  select53= 74  select54= 26  select55= 76  select56= 2  select57= 80  select58= 1  select59= 1  select60= 76  select61= 85  select62= 31  select63= 82  select64= 80  select65= 44  select66= 2  select67= 86  select68= 2  select69= 80  select70= 92  select71= 76  select72= 12  select73= 26  select74= 33  select75= 92  select76= 2  select77= 26  select78= 5  select79= 80  select80= 37  select81= 92  select82= 2  select83= 57  select84= 5  select85= 82  select86= 26  select87= 8  select88= 31  select89= 80  select90= 44  select91= 80  select92= 82  select93= 92  select94= 76  select95= 74  select96= 32  select97= 52  select98= 26  select99= 1 

step 3
groundtruth = 5
measurement = 5.3
estimation = 4.86956
select0= 21  select1= 17  select2= 79  select3= 54  select4= 64  select5= 64  select6= 50  select7= 15  select8= 21  select9= 40  select10= 50  select11= 77  select12= 64  select13= 79  select14= 23  select15= 68  select16= 23  select17= 79  select18= 50  select19= 5  select20= 48  select21= 50  select22= 57  select23= 1  select24= 50  select25= 54  select26= 40  select27= 73  select28= 24  select29= 15  select30= 24  select31= 15  select32= 85  select33= 77  select34= 48  select35= 16  select36= 56  select37= 86  select38= 56  select39= 56  select40= 42  select41= 54  select42= 54  select43= 50  select44= 50  select45= 50  select46= 56  select47= 15  select48= 86  select49= 79  select50= 9  select51= 3  select52= 56  select53= 15  select54= 85  select55= 48  select56= 50  select57= 86  select58= 21  select59= 56  select60= 42  select61= 50  select62= 68  select63= 50  select64= 64  select65= 42  select66= 54  select67= 50  select68= 42  select69= 86  select70= 66  select71= 50  select72= 50  select73= 64  select74= 1  select75= 79  select76= 54  select77= 18  select78= 54  select79= 15  select80= 56  select81= 50  select82= 56  select83= 73  select84= 5  select85= 54  select86= 64  select87= 54  select88= 2  select89= 3  select90= 64  select91= 64  select92= 48  select93= 79  select94= 57  select95= 50  select96= 50  select97= 50  select98= 73  select99= 39 

step 4
groundtruth = 7
measurement = 7.4
estimation = 7.21604
select0= 56  select1= 34  select2= 95  select3= 24  select4= 86  select5= 45  select6= 17  select7= 5  select8= 43  select9= 63  select10= 97  select11= 63  select12= 5  select13= 34  select14= 95  select15= 42  select16= 56  select17= 21  select18= 80  select19= 21  select20= 86  select21= 96  select22= 17  select23= 24  select24= 18  select25= 47  select26= 72  select27= 56  select28= 82  select29= 80  select30= 76  select31= 42  select32= 31  select33= 31  select34= 71  select35= 61  select36= 63  select37= 21  select38= 52  select39= 21  select40= 67  select41= 5  select42= 63  select43= 18  select44= 18  select45= 56  select46= 24  select47= 21  select48= 6  select49= 18  select50= 52  select51= 13  select52= 55  select53= 63  select54= 43  select55= 10  select56= 79  select57= 24  select58= 72  select59= 82  select60= 18  select61= 86  select62= 18  select63= 79  select64= 39  select65= 82  select66= 42  select67= 43  select68= 42  select69= 72  select70= 52  select71= 21  select72= 72  select73= 60  select74= 6  select75= 13  select76= 80  select77= 18  select78= 67  select79= 43  select80= 76  select81= 97  select82= 43  select83= 56  select84= 42  select85= 39  select86= 43  select87= 42  select88= 86  select89= 97  select90= 82  select91= 82  select92= 45  select93= 18  select94= 12  select95= 18  select96= 63  select97= 6  select98= 7  select99= 18 

step 5
groundtruth = 8
measurement = 8.1
estimation = 8.32131
select0= 36  select1= 52  select2= 1  select3= 20  select4= 7  select5= 13  select6= 73  select7= 42  select8= 63  select9= 13  select10= 20  select11= 96  select12= 20  select13= 96  select14= 1  select15= 1  select16= 94  select17= 63  select18= 20  select19= 36  select20= 63  select21= 1  select22= 54  select23= 13  select24= 63  select25= 7  select26= 63  select27= 54  select28= 96  select29= 42  select30= 42  select31= 96  select32= 54  select33= 1  select34= 73  select35= 63  select36= 54  select37= 63  select38= 13  select39= 73  select40= 54  select41= 7  select42= 42  select43= 96  select44= 13  select45= 2  select46= 13  select47= 14  select48= 63  select49= 13  select50= 22  select51= 42  select52= 96  select53= 20  select54= 1  select55= 42  select56= 54  select57= 1  select58= 1  select59= 54  select60= 54  select61= 1  select62= 7  select63= 73  select64= 42  select65= 42  select66= 63  select67= 13  select68= 63  select69= 73  select70= 20  select71= 96  select72= 96  select73= 7  select74= 73  select75= 52  select76= 96  select77= 20  select78= 52  select79= 7  select80= 96  select81= 42  select82= 63  select83= 23  select84= 13  select85= 13  select86= 80  select87= 87  select88= 13  select89= 1  select90= 42  select91= 20  select92= 42  select93= 54  select94= 96  select95= 42  select96= 63  select97= 42  select98= 42  select99= 7 

step 6
groundtruth = 10
measurement = 9.6
estimation = 9.61741
select0= 13  select1= 44  select2= 2  select3= 40  select4= 15  select5= 39  select6= 54  select7= 84  select8= 57  select9= 77  select10= 5  select11= 38  select12= 59  select13= 56  select14= 99  select15= 71  select16= 61  select17= 70  select18= 69  select19= 55  select20= 57  select21= 11  select22= 33  select23= 79  select24= 9  select25= 22  select26= 72  select27= 25  select28= 85  select29= 90  select30= 26  select31= 71  select32= 62  select33= 52  select34= 41  select35= 30  select36= 69  select37= 28  select38= 81  select39= 39  select40= 28  select41= 27  select42= 46  select43= 29  select44= 93  select45= 36  select46= 98  select47= 93  select48= 81  select49= 71  select50= 0  select51= 94  select52= 17  select53= 15  select54= 44  select55= 39  select56= 72  select57= 63  select58= 89  select59= 93  select60= 99  select61= 69  select62= 92  select63= 56  select64= 24  select65= 24  select66= 93  select67= 35  select68= 71  select69= 58  select70= 41  select71= 39  select72= 48  select73= 81  select74= 62  select75= 0  select76= 56  select77= 71  select78= 18  select79= 42  select80= 36  select81= 59  select82= 79  select83= 11  select84= 56  select85= 59  select86= 35  select87= 90  select88= 44  select89= 2  select90= 24  select91= 27  select92= 68  select93= 9  select94= 94  select95= 10  select96= 42  select97= 0  select98= 57  select99= 91 

2차원 particle filter 간단 예제

// 2-D Particle filter algorithm exercise
// 2009-11-10
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

// uniform random number generator
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

// Gaussian random number generator
double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void set_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
//    cout << "check measurement type3 = " << mtype[3] << endl; // just to check

    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
//    return exp( -0.5 * cvmGet(dist, 0, 0) );
//    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
   
}



/*
struct particle
{
    double weight; // weight of a particle
    CvMat* loc_p = cvCreateMat(2, 1, CV_64FC1); // previously estimated position of a particle
    CvMat* loc = cvCreateMat(2, 1, CV_64FC1); // currently estimated position of a particle
    CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); // estimated velocity of a particle
    cvSub(loc, loc_p, velocity); // loc - loc_p -> velocity
};
*/


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    int width = 400; // width of image window
    int height = 400; // height of image window   
   
    IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("ParticleFilter-2d", 0);
   
   
     // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 0.1
    cvmSet(transition_noise, 0, 1, 0.0);
    cvmSet(transition_noise, 1, 0, 0.0);
    cvmSet(transition_noise, 1, 1, 6);      
   
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 0.3
    cvmSet(measurement_noise, 0, 1, 0.0);
    cvmSet(measurement_noise, 1, 0, 0.0);
    cvmSet(measurement_noise, 1, 1, 2);  
   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);
    // declare particles
/*    particle pb[N]; // N estimated particles
    particle pp[N]; // N predicted particles       
    particle pu[N]; // temporary variables for updating particles       
*/
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize particles and the state
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
            cvmSet(state, row, 0, 0.0);           
        }
    }
   
    // set the system   
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }
   
    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // set groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        set_groundtruth(groundtruth, gx, gy);

        // set measurement types
        double c1 = 1.0, c2 = 4.0;   
        // measured point 1
        cvmSet(measurement[0], 0, 0, gx + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[0], 1, 0, gy + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 2
        cvmSet(measurement[1], 0, 0, gx + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[1], 1, 0, gy + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 3 // clutter noise
        cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
        cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

        // count the number of measurements       
        int count = count_detections(); // number of detections
        cout << "# of measured points = " << count << endl;
   
        // get measurement           
        for (int index = 0; index < count; index++)
        {
            cout << "measurement #" << index << " : "
                << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(200, 0, 255), 1);
           
        }
       
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
               
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
           
            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));

           
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
///            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
       
///            cout << "likelihood of #" << n << " = " << like[n] << endl;
           
            like_sum += like[n];
        }
       
///        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
   
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
///            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
   
        // update particles       
        cout << endl << "updating particles" << endl;
        double urn[N]; // uniform random number
        double a[N]; // portion between particles

        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    pselected = k;
                    break;
                }
            }
///            cout << "particle " << n << " => " << pselected << "  ";       
            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value
           
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
   
        }
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 4, cvScalarAll(255), 1);
       
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-2d", iplImg);
        cvWaitKey(1000);   
       
       
       
       
    } // for "t"
   
   
    cvWaitKey();   
   
    return 0;
}

실행 결과:

콘솔 창:

console

start filtering...

step 0
groundtruth = 0  0
# of measured points = 1
measurement #0 : 3.33711  -0.94521

* * * * * *
estimation = 4.27132  -0.56567

updating particles


step 1
groundtruth = 10  19.75
# of measured points = 1
measurement #0 : 12.2605  16.1354

* * * * * *
estimation = 12.8682  14.8424

updating particles


step 2
groundtruth = 20  39
# of measured points = 1
measurement #0 : 23.0287  36.439

* * * * * *
estimation = 23.8277  35.9763

updating particles


step 3
groundtruth = 30  57.75
# of measured points = 1
measurement #0 : 27.0806  56.4054

* * * * * *
estimation = 27.901  56.583

updating particles


step 4
groundtruth = 40  76
# of measured points = 2
measurement #0 : 37.1672  77.1363
measurement #1 : 28.5532  63.1869

* * * * * *
estimation = 36.1101  75.6839

updating particles


step 5
groundtruth = 50  93.75
# of measured points = 1
measurement #0 : 52.7732  92.4224

* * * * * *
estimation = 51.5393  91.9914

updating particles


step 6
groundtruth = 60  111
# of measured points = 2
measurement #0 : 59.2392  109.913
measurement #1 : 68.243  118.967

* * * * * *
estimation = 65.3468  116.505

updating particles


step 7
groundtruth = 70  127.75
# of measured points = 2
measurement #0 : 72.4296  126.721
measurement #1 : 66.9045  135.525

* * * * * *
estimation = 70.1203  129.206

updating particles


step 8
groundtruth = 80  144
# of measured points = 1
measurement #0 : 79.8778  144.733

* * * * * *
estimation = 80.1469  144.239

updating particles


step 9
groundtruth = 90  159.75
# of measured points = 1
measurement #0 : 92.799  161.092

* * * * * *
estimation = 92.8553  161.317

updating particles


step 10
groundtruth = 100  175
# of measured points = 2
measurement #0 : 101.89  176.408
measurement #1 : 106.811  183.759

* * * * * *
estimation = 103.757  179.192

updating particles


step 11
groundtruth = 110  189.75
# of measured points = 2
measurement #0 : 105.579  186.642
measurement #1 : 123.096  192.458

* * * * * *
estimation = 116.223  190.511

updating particles


step 12
groundtruth = 120  204
# of measured points = 1
measurement #0 : 123.096  205.035

* * * * * *
estimation = 128.034  206.975

updating particles


step 13
groundtruth = 130  217.75
# of measured points = 1
measurement #0 : 129.921  215.284

* * * * * *
estimation = 131.788  215.51

updating particles


step 14
groundtruth = 140  231
# of measured points = 2
measurement #0 : 142.833  230.913
measurement #1 : 132.181  222.889

* * * * * *
estimation = 132.992  222.784

updating particles


step 15
groundtruth = 150  243.75
# of measured points = 2
measurement #0 : 151.054  246.003
measurement #1 : 157.896  234.062

* * * * * *
estimation = 134.122  228.661

updating particles


step 16
groundtruth = 160  256
# of measured points = 2
measurement #0 : 164.453  254.971
measurement #1 : 160.959  260.881

* * * * * *
estimation = 136.417  232.74

updating particles


step 17
groundtruth = 170  267.75
# of measured points = 2
measurement #0 : 172.76  270.828
measurement #1 : 166.605  266.583

* * * * * *
estimation = 170.796  275.304

updating particles


step 18
groundtruth = 180  279
# of measured points = 1
measurement #0 : 181.005  279.943

* * * * * *
estimation = 179.841  283.606

updating particles


step 19
groundtruth = 190  289.75
# of measured points = 1
measurement #0 : 189.536  291.038

* * * * * *
estimation = 188.726  291.697

updating particles


step 20
groundtruth = 200  300
# of measured points = 1
measurement #0 : 200.378  300.232

* * * * * *
estimation = 200.838  300.631

updating particles


step 21
groundtruth = 210  309.75
# of measured points = 2
measurement #0 : 209.443  307.787
measurement #1 : 208.58  314.728

* * * * * *
estimation = 209.798  310.874

updating particles


step 22
groundtruth = 220  319
# of measured points = 1
measurement #0 : 220.317  318.776

* * * * * *
estimation = 220.091  318.428

updating particles


step 23
groundtruth = 230  327.75
# of measured points = 2
measurement #0 : 225.224  328.509
measurement #1 : 241.184  337.333

* * * * * *
estimation = 225.568  328.482

updating particles


step 24
groundtruth = 240  336
# of measured points = 1
measurement #0 : 238.06  331.916

* * * * * *
estimation = 236.787  331.548

updating particles


step 25
groundtruth = 250  343.75
# of measured points = 2
measurement #0 : 250.569  346.632
measurement #1 : 246.839  340.68

* * * * * *
estimation = 246.718  341.02

updating particles


step 26
groundtruth = 260  351
# of measured points = 2
measurement #0 : 256.636  352.123
measurement #1 : 264.949  354.267

* * * * * *
estimation = 257.819  352.452

updating particles


step 27
groundtruth = 270  357.75
# of measured points = 1
measurement #0 : 273.807  357.344

* * * * * *
estimation = 273.103  357.203

updating particles


step 28
groundtruth = 280  364
# of measured points = 1
measurement #0 : 281.317  366.607

* * * * * *
estimation = 282.76  366.197

updating particles


step 29
groundtruth = 290  369.75
# of measured points = 1
measurement #0 : 291.158  372.073

* * * * * *
estimation = 291.815  372.35

updating particles

Lessons:
1. transition noise와 measurement noise, (그것들의 covariances) 그리고 각 입자의 초기 위치와 상태를 알맞게 설정하는 것이 관건임을 배웠다. 그것을 Tuning이라 부른다는 것도.
1-1. 코드에서 가정한 system에서는 특히 입자들의 초기 속도를 어떻게 주느냐에 따라 tracking의 성공 여부가 좌우된다.
2. 실제 상태는 등가속도 운동을 하는 비선형 시스템이나, 여기에서는 프레임 간의 운동을 등속으로 가정하여 선형 시스템으로 근사한 모델을 적용한 것이다.
2-1. 그러므로 여기에 Kalman filter를 적용하여 결과를 비교해 볼 수 있겠다. 이 경우, 3차원 Kalman gain을 계산해야 한다.
2-2. 분홍색 부분을 고쳐 비선형 모델로 만든 후 Particle filtering을 하면 결과가 더 좋지 않을까? Tuning도 더 쉬어지지 않을까?
3. 코드 좀 정리하자. 너무 지저분하다. ㅡㅡ;
4. 아니, 근데 영 헤매다가 갑자기 따라잡는 건 뭐지??? (아래 결과)

white: groundtruth / pink: measurements / green: estimation

console:

console:

start filtering...

step 0
groundtruth = 0  0
# of measured points = 2
measurement #0 : -0.688539  2.86813
measurement #1 : -2.35666  0.738435

* * * * * *
estimation = -0.770065  1.88966

updating particles


step 1
groundtruth = 10  19.75
# of measured points = 1
measurement #0 : 10.3522  20.3912

* * * * * *
estimation = -1.45133  3.29854

updating particles


step 2
groundtruth = 20  39
# of measured points = 2
measurement #0 : 21.7012  40.8506
measurement #1 : 20.1907  31.8869

* * * * * *
estimation = -1.55052  6.75417

updating particles


step 3
groundtruth = 30  57.75
# of measured points = 1
measurement #0 : 30.1194  56.6674

* * * * * *
estimation = -3.30433  9.70937

updating particles


step 4
groundtruth = 40  76
# of measured points = 1
measurement #0 : 36.0523  73.8999

* * * * * *
estimation = -3.72132  9.82318

updating particles


step 5
groundtruth = 50  93.75
# of measured points = 1
measurement #0 : 49.8408  95.6712

* * * * * *
estimation = -3.34364  18.8461

updating particles


step 6
groundtruth = 60  111
# of measured points = 1
measurement #0 : 56.8027  108.984

* * * * * *
estimation = -8.05506  18.2603

updating particles


step 7
groundtruth = 70  127.75
# of measured points = 2
measurement #0 : 70.7897  129.308
measurement #1 : 71.8024  105.443

* * * * * *
estimation = -10.3673  31.1528

updating particles


step 8
groundtruth = 80  144
# of measured points = 1
measurement #0 : 80.1928  141.053

* * * * * *
estimation = -15.5645  55.2588

updating particles


step 9
groundtruth = 90  159.75
# of measured points = 1
measurement #0 : 86.8175  159.338

* * * * * *
estimation = -10.9739  57.8888

updating particles


step 10
groundtruth = 100  175
# of measured points = 1
measurement #0 : 103.086  175.356

* * * * * *
estimation = -7.66367  83.0255

updating particles


step 11
groundtruth = 110  189.75
# of measured points = 1
measurement #0 : 108.707  191.76

* * * * * *
estimation = -14.7215  89.1776

updating particles


step 12
groundtruth = 120  204
# of measured points = 2
measurement #0 : 119.874  203.004
measurement #1 : 129.157  200.849

* * * * * *
estimation = -21.323  66.7973

updating particles


step 13
groundtruth = 130  217.75
# of measured points = 1
measurement #0 : 129.513  216.419

* * * * * *
estimation = -14.8319  59.2833

updating particles


step 14
groundtruth = 140  231
# of measured points = 1
measurement #0 : 141.322  231.575

* * * * * *
estimation = -21.0061  78.8326

updating particles


step 15
groundtruth = 150  243.75
# of measured points = 2
measurement #0 : 150.082  242.501
measurement #1 : 148.783  249.803

* * * * * *
estimation = -21.2331  91.5894

updating particles


step 16
groundtruth = 160  256
# of measured points = 2
measurement #0 : 160.145  258.928
measurement #1 : 161.927  256.816

* * * * * *
estimation = -2.43187  136.125

updating particles


step 17
groundtruth = 170  267.75
# of measured points = 1
measurement #0 : 168.569  268.705

* * * * * *
estimation = -6.06448  150.747

updating particles


step 18
groundtruth = 180  279
# of measured points = 1
measurement #0 : 175.064  275.32

* * * * * *
estimation = 1.07476  124.791

updating particles


step 19
groundtruth = 190  289.75
# of measured points = 1
measurement #0 : 190.415  288.131

* * * * * *
estimation = 0.95148  97.3328

updating particles


step 20
groundtruth = 200  300
# of measured points = 1
measurement #0 : 202.677  300.904

* * * * * *
estimation = 205.002  299.882

updating particles


step 21
groundtruth = 210  309.75
# of measured points = 1
measurement #0 : 212.009  314.555

* * * * * *
estimation = 216.437  316.336

updating particles


step 22
groundtruth = 220  319
# of measured points = 1
measurement #0 : 217.192  320.7

* * * * * *
estimation = 225.312  329.566

updating particles


step 23
groundtruth = 230  327.75
# of measured points = 2
measurement #0 : 227.763  330.381
measurement #1 : 239.111  327.127

* * * * * *
estimation = 232.821  329.497

updating particles


step 24
groundtruth = 240  336
# of measured points = 1
measurement #0 : 242.03  335.01

* * * * * *
estimation = 241.096  334.482

updating particles


step 25
groundtruth = 250  343.75
# of measured points = 1
measurement #0 : 252.527  342.531

* * * * * *
estimation = 251.577  341.811

updating particles


step 26
groundtruth = 260  351
# of measured points = 1
measurement #0 : 261.053  348.174

* * * * * *
estimation = 260.709  348.515

updating particles


step 27
groundtruth = 270  357.75
# of measured points = 2
measurement #0 : 269.689  358.197
measurement #1 : 272.889  359.509

* * * * * *
estimation = 270.917  358.778

updating particles


step 28
groundtruth = 280  364
# of measured points = 1
measurement #0 : 278.591  366.521

* * * * * *
estimation = 278.907  366.859

updating particles


step 29
groundtruth = 290  369.75
# of measured points = 1
measurement #0 : 291.132  369.428

* * * * * *
estimation = 291.015  370.118

updating particles

// 2-D Particle filter algorithm exercise
// lym, VIP Lab, Sogang Univ.
// 2009-11-23
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

int width = 400; // width of image window
int height = 400; // height of image window   
IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);

// uniform random number generator
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

// Gaussian random number generator
double gaussian_random(void) {
   
    static int next_gaussian = 0;
    static double saved_gaussian_value;
   
    double fac, rsq, v1, v2;
   
    if(next_gaussian == 0) {
       
        do {
            v1 = 2.0 * uniform_random() - 1.0;
            v2 = 2.0 * uniform_random() - 1.0;
            rsq = v1 * v1 + v2 * v2;
        }
        while(rsq >= 1.0 || rsq == 0.0);
        fac = sqrt(-2.0 * log(rsq) / rsq);
        saved_gaussian_value = v1 * fac;
        next_gaussian = 1;
        return v2 * fac;
    }
    else {
        next_gaussian = 0;
        return saved_gaussian_value;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void get_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
    cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 2, cvScalarAll(255), 2);   
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
    //    cout << "check measurement type3 = " << mtype[3] << endl; // just to check
   
    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// get measurements
int get_measurement (CvMat* measurement[], CvMat* measurement_noise, double x, double y)
{
    // set measurement types
    double c1 = 1.0, c2 = 4.0;   
    // measured point 1
    cvmSet(measurement[0], 0, 0, x + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[0], 1, 0, y + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 2
    cvmSet(measurement[1], 0, 0, x + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[1], 1, 0, y + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 3 // clutter noise
    cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
    cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

    // count the number of measured points   
    int number = count_detections(); // number of detections
    cout << "# of measured points = " << number << endl;

    // get measurement           
    for (int index = 0; index < number; index++)
    {
        cout << "measurement #" << index << " : "
        << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
       
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(255, 0, 255), 2);           
    }

    return number;
}


// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
    //    return exp( -0.5 * cvmGet(dist, 0, 0) );
    //    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
}


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    // set the system   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);    // state of the system to be estimated
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }   

    // declare particles
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize the state and particles
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(state, row, 0, 0.0);   
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
        }
    }
   
    // set the process noise
    // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 3.0
    cvmSet(transition_noise, 0, 1, 0.0);
    cvmSet(transition_noise, 1, 0, 0.0);
    cvmSet(transition_noise, 1, 1, 6);      
   
    // set the measurement noise
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 2.0
    cvmSet(measurement_noise, 0, 1, 0.0);
    cvmSet(measurement_noise, 1, 0, 0.0);
    cvmSet(measurement_noise, 1, 1, 2);  
   
    // initialize the image window
    cvZero(iplImg);   
    cvNamedWindow("ParticleFilter-3d", 0);

    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // get the groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        get_groundtruth(groundtruth, gx, gy);
        // get measurements
        int count = get_measurement(measurement, measurement_noise, gx, gy);
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
//            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
//            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
//            cout << "likelihood of #" << n << " = " << like[n] << endl;           
            like_sum += like[n];
        }
//        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
       
        // update particles       
        cout << endl << "updating particles" << endl;
        double a[N]; // portion between particles
       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if ( uniform_random() < a[k] )               
                {
                    pselected = k;
                    break;
                }
            }
//            cout << "p " << n << " => " << pselected << "  ";       

            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value               
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
        }
       
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-3d", iplImg);
        cvWaitKey(1000);   
       
    } // for "t"
   
    cvWaitKey();   
   
    return 0;
}