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

Revision 260, 3.9 KB checked in by dave, 10 years ago (diff)

generate weighted averages, calculate the estimate, and visualise all this stuff

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///////////////////////////////////////////////////////////////
25// Some util functions
26
27float FloatNoise()
28{
29        return rand()%INT_MAX/float(INT_MAX)*2-1;
30}
31
32float GaussianNoise()
33{
34        float l=0;
35        float x=0;
36        float y=0;
37        while (l>=1 || l==0)
38        {
39                x=FloatNoise();
40                y=FloatNoise();
41                l=x*x+y*y;
42        }
43        return sqrt(-2*log(l)/l)*x;
44}
45
46// gets the angle of a vector from 0 to 360 degrees
47float GetAngle(float x, float y)
48{
49        if(y==0) // prevent division by 0
50        {
51                if(x>=0) return 0;
52                else return 180;
53        }
54        else
55        {
56                if (y>0) return 90-atan(x/y)*180/M_PI;
57                else return 270-atan(x/y)*180/M_PI;
58        }
59}
60
61////////////////////////////////////////////////////////////////
62// The particle filter
63
64ParticleFilter::ParticleFilter(unsigned int NumParticles)
65{
66        m_Particles.resize(NumParticles);
67       
68        // Start the particles off in random positions
69        for (vector<Particle>::iterator i=m_Particles.begin();
70                i!=m_Particles.end(); ++i)
71        {
72                i->m_State.x = FloatNoise()*100;
73                i->m_State.y = FloatNoise()*100;
74        }
75}
76
77
78ParticleFilter::Observation ParticleFilter::State::Observe()
79{
80        Observation NewObs;
81        NewObs.Angle=GetAngle(x,y);
82        NewObs.Dist=sqrt(x*x+y*y);
83        return NewObs;
84}
85
86void ParticleFilter::SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist)
87{
88        m_PredictionNoiseLevel=Prediction;
89        m_ObsAngleNoiseLevel=ObsAngle;
90        m_ObsDistNoiseLevel=ObsDist;
91}
92
93void ParticleFilter::Predict()
94{
95        for (vector<Particle>::iterator i=m_Particles.begin();
96                i!=m_Particles.end(); ++i)
97        {
98                // As we don't have a model we can use for prediction
99                // we just add some noise to the state here.
100                // It would be quite simple to add velocity into our state,
101                // in which case we'd add the velocity to the position here
102                // as well.
103                i->m_State.x+=GaussianNoise()*m_PredictionNoiseLevel;
104                i->m_State.y+=GaussianNoise()*m_PredictionNoiseLevel;
105        }
106}
107       
108ParticleFilter::State ParticleFilter::Update(const Observation &Obs)
109{
110        float TotalWeight = 0;
111
112        for (vector<Particle>::iterator i=m_Particles.begin();
113                i!=m_Particles.end(); ++i)
114        {       
115                // assign the weight to each particle, based on the
116                Observation PObs=i->m_State.Observe();
117               
118                // todo: angle error will be wrong around 360 -> 0 boundary
119                float AngErr = (Obs.Angle-PObs.Angle)+(m_ObsAngleNoiseLevel*GaussianNoise());
120                float DistErr = (Obs.Dist-PObs.Dist)+(m_ObsDistNoiseLevel*GaussianNoise());
121               
122                i->m_Weight =1/(AngErr*AngErr*0.1 + DistErr*DistErr);
123                TotalWeight+=i->m_Weight;
124        }
125       
126        // Normalise the weights
127        for (vector<Particle>::iterator i=m_Particles.begin();
128                i!=m_Particles.end(); ++i)
129        {       
130                i->m_Weight/=TotalWeight;
131        }
132       
133        // Find the weighted average (this is our result)
134        State ret;
135        ret.x=0; ret.y=0;
136       
137        for (vector<Particle>::iterator i=m_Particles.begin();
138                i!=m_Particles.end(); ++i)
139        {       
140                ret.x += i->m_State.x * i->m_Weight;
141                ret.y += i->m_State.y * i->m_Weight;
142        }
143       
144        Resample();
145       
146        return ret;
147}
148
149void ParticleFilter::Resample()
150{
151        for (vector<Particle>::iterator i=m_Particles.begin();
152                i!=m_Particles.end(); ++i)
153        {
154                if (i->m_Weight<m_ResampleWeight)
155                {
156                        // cast to a random position
157                        i->m_State.x = FloatNoise()*100;
158                        i->m_State.y = FloatNoise()*100;
159                }       
160        }
161}
Note: See TracBrowser for help on using the repository browser.