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 | |
---|
22 | using namespace std; |
---|
23 | |
---|
24 | |
---|
25 | ParticleFilter::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 | |
---|
38 | float ParticleFilter::FloatNoise() |
---|
39 | { |
---|
40 | return rand()%INT_MAX/float(INT_MAX)*2-1; |
---|
41 | } |
---|
42 | |
---|
43 | float 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 | |
---|
57 | ParticleFilter::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 | |
---|
65 | void ParticleFilter::SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist) |
---|
66 | { |
---|
67 | m_PredictionNoiseLevel=Prediction; |
---|
68 | m_ObsAngleNoiseLevel=ObsAngle; |
---|
69 | m_ObsDistNoiseLevel=ObsDist; |
---|
70 | } |
---|
71 | |
---|
72 | void 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 | |
---|
87 | void 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 | |
---|
104 | void 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 | } |
---|