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 | |
---|
22 | using namespace std; |
---|
23 | |
---|
24 | class ParticleFilter |
---|
25 | { |
---|
26 | public: |
---|
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 | |
---|
72 | private: |
---|
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 |
---|