source: libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp @ 258

Revision 258, 2.9 KB checked in by dave, 11 years ago (diff)

added particle filter example

Line 
1// Copyright (C) 2009 foam
2//
3// This program is free software; you can redistribute it and/or modify
4// it under the terms of the GNU General Public License as published by
5// the Free Software Foundation; either version 2 of the License, or
6// (at your option) any later version.
7//
8// This program is distributed in the hope that it will be useful,
9// but WITHOUT ANY WARRANTY; without even the implied warranty of
10// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11// GNU General Public License for more details.
12//
13// You should have received a copy of the GNU General Public License
14// along with this program; if not, write to the Free Software
15// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16
17#include <limits.h>
18#include <math.h>
19#include "ParticleFilter.h"
20#include <iostream>
21
22using namespace std;
23
24
25ParticleFilter::ParticleFilter(unsigned int NumParticles)
26{
27        m_Particles.resize(NumParticles);
28       
29        // Start the particles off in random positions
30        for (vector<Particle>::iterator i=m_Particles.begin();
31                i!=m_Particles.end(); ++i)
32        {
33                i->m_State.x = FloatNoise()*100;
34                i->m_State.y = FloatNoise()*100;
35        }
36}
37
38float ParticleFilter::FloatNoise()
39{
40        return rand()%INT_MAX/float(INT_MAX)*2-1;
41}
42
43float ParticleFilter::GaussianNoise()
44{
45        float l=0;
46        float x=0;
47        float y=0;
48        while (l>=1 || l==0)
49        {
50                x=FloatNoise();
51                y=FloatNoise();
52                l=x*x+y*y;
53        }
54        return sqrt(-2*log(l)/l)*x;
55}
56
57ParticleFilter::Observation ParticleFilter::State::Observe()
58{
59        Observation NewObs;
60        NewObs.Angle=atan(y/x);
61        NewObs.Dist=sqrt(x*x+y*y);
62        return NewObs;
63}
64
65void ParticleFilter::SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist)
66{
67        m_PredictionNoiseLevel=Prediction;
68        m_ObsAngleNoiseLevel=ObsAngle;
69        m_ObsDistNoiseLevel=ObsDist;
70}
71
72void ParticleFilter::Predict()
73{
74        for (vector<Particle>::iterator i=m_Particles.begin();
75                i!=m_Particles.end(); ++i)
76        {
77                // As we don't have a model we can use for prediction
78                // we just add some noise to the state here.
79                // It would be quite simple to add velocity into our state,
80                // in which case we'd add the velocity to the position here
81                // as well.
82                i->m_State.x+=GaussianNoise()*m_PredictionNoiseLevel;
83                i->m_State.y+=GaussianNoise()*m_PredictionNoiseLevel;
84        }
85}
86       
87void ParticleFilter::Update(const Observation &Obs)
88{
89        for (vector<Particle>::iterator i=m_Particles.begin();
90                i!=m_Particles.end(); ++i)
91        {       
92                // assign the weight to each particle, based on the
93                Observation PObs=i->m_State.Observe();
94               
95                float AngErr = Obs.Angle-PObs.Angle+(m_ObsAngleNoiseLevel*GaussianNoise());
96                float DistErr = Obs.Dist-PObs.Dist+(m_ObsDistNoiseLevel*GaussianNoise());
97               
98                i->m_Weight =1/(AngErr*AngErr*100 + DistErr*DistErr);
99        }
100       
101        Resample();
102}
103
104void ParticleFilter::Resample()
105{
106        for (vector<Particle>::iterator i=m_Particles.begin();
107                i!=m_Particles.end(); ++i)
108        {
109                if (i->m_Weight<m_ResampleWeight)
110                {
111                        // cast to a random position
112                        i->m_State.x = FloatNoise()*100;
113                        i->m_State.y = FloatNoise()*100;
114                }       
115        }
116}
Note: See TracBrowser for help on using the repository browser.