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

Revision 258, 2.2 KB checked in by dave, 10 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#ifndef FOAM_PARTICLE_FILTER
18#define FOAM_PARTICLE_FILTER
19
20#include <vector>
21
22using namespace std;
23
24class ParticleFilter
25{
26public:
27        ParticleFilter(unsigned int NumParticles);
28        ~ParticleFilter() {}
29
30        // An observation of the state
31        class Observation
32        {
33        public:
34                float Angle,Dist;
35        };
36
37        // The hidden state we want to estimate
38        class State
39        {
40        public:
41                float x,y;
42                // Gets the observation we would expect from this state
43                // includes some noise, based on our expectation of the sensor
44                // used to make the measurement.
45                Observation Observe();
46        };
47
48        class Particle
49        {
50        public:
51                // A particle is a state with a cooresponding probabilistic
52                // weighting
53                State m_State;
54                float m_Weight;
55        };
56       
57        // Use the model to predict the next state of each particle,
58        // not forgetting to add some prediction noise. This should 'spread'
59        // the particles out.
60        void Predict();
61       
62        // Set the particle weights according to the current real observation,
63        // and resample particles with low weights, which 'tightens' the particles in.
64        void Update(const Observation &Obs);
65
66        void SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist);
67        void SetResampleWeight(float Weight) { m_ResampleWeight=Weight; }
68
69        // For debug rendering
70        const vector<Particle> &GetParticles() { return m_Particles; }
71
72private:
73        float FloatNoise();
74        float GaussianNoise();
75        void Resample();
76       
77        float m_PredictionNoiseLevel;
78        float m_ObsAngleNoiseLevel;
79        float m_ObsDistNoiseLevel;
80        float m_ResampleWeight;
81       
82        vector<Particle> m_Particles;
83};
84
85#endif
Note: See TracBrowser for help on using the repository browser.