Changeset 292
 Timestamp:
 01/28/2010 09:27:49 AM (11 years ago)
 Location:
 libs/magicsquares/inprogress/pf/src
 Files:

 2 edited
Legend:
 Unmodified
 Added
 Removed

libs/magicsquares/inprogress/pf/src/ParticleFilter.cpp
r291 r292 22 22 using namespace std; 23 23 24 /////////////////////////////////////////////////////////////// 25 // Some util functions 26 27 float FloatNoise() 28 { 29 return rand()%99999/float(99999)*21; 30 } 31 32 float 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 47 float 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 90atan(x/y)*180/M_PI; 57 else return 270atan(x/y)*180/M_PI; 58 } 59 } 60 61 // the inverse of above 62 void GetPos(float a, float d, float &x, float &y) 63 { 64 a/=180/M_PI; 65 x = cos(a)*d; 66 y = sin(a)*d; 67 } 68 69 float Distance(float ax, float ay, float bx, float by) 70 { 71 float x=axbx; 72 float y=ayby; 73 return sqrt(x*x+y*y); 74 } 75 24 76 //////////////////////////////////////////////////////////////// 25 77 // The particle filter … … 28 80 { 29 81 m_Particles.resize(NumParticles); 30 } 31 32 ParticleFilter::~ParticleFilter() 33 { 34 // Clean up 35 for (vector<Particle>::iterator i=m_Particles.begin(); 36 i!=m_Particles.end(); ++i) 37 { 38 delete i>m_State; 39 } 40 } 41 42 void ParticleFilter::Initialise() 43 { 82 44 83 // Start the particles off in random positions 45 84 for (vector<Particle>::iterator i=m_Particles.begin(); 46 85 i!=m_Particles.end(); ++i) 47 86 { 48 i>m_State = NewState(); 49 i>m_State>Randomise(); 50 } 51 } 52 53 void ParticleFilter::SetNoiseLevels(float Prediction) 87 i>m_State.Randomise(); 88 } 89 } 90 91 92 ParticleFilter::Observation ParticleFilter::State::Observe() 93 { 94 Observation NewObs; 95 NewObs.Angle=GetAngle(x,y); 96 NewObs.Dist=sqrt(x*x+y*y); 97 return NewObs; 98 } 99 100 void ParticleFilter::SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist) 54 101 { 55 102 m_PredictionNoiseLevel=Prediction; 103 m_ObsAngleNoiseLevel=ObsAngle; 104 m_ObsDistNoiseLevel=ObsDist; 56 105 } 57 106 … … 61 110 i!=m_Particles.end(); ++i) 62 111 { 63 i>m_State>Predict(m_PredictionNoiseLevel); 64 } 65 } 66 67 ParticleFilter::State *ParticleFilter::Update(const Observation *Obs) 68 { 69 // Resample first, which leaves the resampled particles 70 // with incorrect weights 112 // Apply the velocity to the particle position 113 i>m_State.x+=i>m_State.dx; 114 i>m_State.y+=i>m_State.dy; 115 // Add some noise to the position 116 i>m_State.x+=GaussianNoise()*m_PredictionNoiseLevel; 117 i>m_State.y+=GaussianNoise()*m_PredictionNoiseLevel; 118 } 119 } 120 121 ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 122 { 71 123 Resample(); 72 124 73 125 float TotalWeight = 0; 74 126 75 // Now resample76 127 for (vector<Particle>::iterator i=m_Particles.begin(); 77 128 i!=m_Particles.end(); ++i) … … 80 131 // between the one observation from the outside world, and the 81 132 // observation of the particle's state 82 Observation *PObs=i>m_State>Observe(); 83 i>m_Weight = PObs>Weight(Obs); 84 delete PObs; 85 133 Observation PObs=i>m_State.Observe(); 134 135 // todo: angle error will be wrong around 360 > 0 boundary 136 float AngErr = (Obs.AnglePObs.Angle)+(m_ObsAngleNoiseLevel*GaussianNoise()); 137 float DistErr = (Obs.DistPObs.Dist)+(m_ObsDistNoiseLevel*GaussianNoise()); 138 139 // Here we can use information about our sensor to modify the error from 140 // different readings. We'll make it so the angle observation is less 'trustworthy' 141 // than the distance readings. This has the effect of making the pdf into a cresent 142 // shape. 143 i>m_Weight = 1/(fabs(AngErr)+fabs(DistErr)); 86 144 TotalWeight+=i>m_Weight; 87 145 } 88 146 89 // Normalise the weights90 147 for (vector<Particle>::iterator i=m_Particles.begin(); 91 148 i!=m_Particles.end(); ++i) … … 93 150 i>m_Weight/=TotalWeight; 94 151 } 95 96 // Return the estimate 97 return WeightedAverage(); 152 153 // Find the weighted average to get an estimate 154 State ret; 155 ret.x=0; ret.y=0; 156 157 for (vector<Particle>::iterator i=m_Particles.begin(); 158 i!=m_Particles.end(); ++i) 159 { 160 ret.x += i>m_State.x * i>m_Weight; 161 ret.y += i>m_State.y * i>m_Weight; 162 } 163 164 return ret; 98 165 } 99 166 … … 130 197 { 131 198 // Cast to a new completely random position/velocity 132 i>m_State >Randomise();199 i>m_State.Randomise(); 133 200 } 134 201 else 135 202 { 136 203 // Copy settings from the 'best' current particle 137 (*i>m_State)=(*Highest>m_State);204 i>m_State=Highest>m_State; 138 205 // And jitter them a little  this stops the particles 139 206 // converging too much on one state 140 i>m_State >Jitter();141 207 i>m_State.Jitter(); 208 } 142 209 } 143 210 } 
libs/magicsquares/inprogress/pf/src/ParticleFilter.h
r291 r292 19 19 20 20 #include <vector> 21 #include <iostream>22 #include <assert.h>23 21 24 22 using namespace std; 25 23 26 ///////////////////////////////////////////////////////// 27 // A general purpose abstract base class particle filter 24 float FloatNoise(); 25 float GaussianNoise(); 26 float GetAngle(float x, float y); 27 void GetPos(float a, float d, float &x, float &y); 28 float Distance(float ax, float ay, float bx, float by); 29 28 30 class ParticleFilter 29 31 { 30 32 public: 31 33 ParticleFilter(unsigned int NumParticles); 32 virtual ~ParticleFilter(); 33 34 // Needs an initialise as we have to call virtual funcs 35 void Initialise(); 34 ~ParticleFilter() {} 36 35 37 36 // An observation of the state … … 39 38 { 40 39 public: 41 virtual float Weight(const Observation *Target)=0;40 float Angle,Dist; 42 41 }; 43 42 … … 47 46 public: 48 47 // Gets the observation we would expect from this state 49 virtual Observation *Observe()=0; 48 Observation Observe(); 49 50 50 // Put the state into a random position and velocity 51 virtual void Randomise()=0; 51 void Randomise() 52 { 53 x = FloatNoise()*100; 54 y = FloatNoise()*100; 55 dx = GaussianNoise(); 56 dy = GaussianNoise(); 57 } 58 52 59 // Add a small random amount to the position and velocity 53 virtual void Jitter()=0; 54 // Run the internal prediction for this state, given the state noise level 55 virtual void Predict(float Noise)=0; 56 // Assign from given state 57 virtual State &operator=(const State &other)=0; 60 void Jitter() 61 { 62 x += GaussianNoise()*5; 63 y += GaussianNoise()*5; 64 dx += GaussianNoise()*0.05; 65 dy += GaussianNoise()*0.05; 66 } 67 68 float x,y; 69 float dx,dy; 58 70 }; 59 71 … … 63 75 // A particle is a state with a cooresponding probabilistic 64 76 // weighting 65 State *m_State;77 State m_State; 66 78 float m_Weight; 79 67 80 }; 68 69 // Needs to be done by the derived classes, 70 // finds a weighted average of the states given the 71 // particle weights. 72 virtual State *WeightedAverage()=0; 73 virtual State *NewState()=0; 74 81 75 82 // Use the model to predict the next state of each particle, 76 83 // not forgetting to add some prediction noise. This should 'spread' 77 84 // the particles out. 78 85 void Predict(); 79 86 80 87 // Set the particle weights according to the current real observation, 81 88 // and resample particles with low weights, which 'tightens' the particles in. 82 State *Update(const Observation *Obs);89 State Update(const Observation &Obs); 83 90 84 void SetNoiseLevels(float Prediction );91 void SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist); 85 92 void SetResampleWeight(float Weight) { m_ResampleWeight=Weight; } 86 93 … … 91 98 Particle* GetMostLikely(); 92 99 93 pr otected:100 private: 94 101 95 102 … … 98 105 99 106 float m_PredictionNoiseLevel; 107 float m_ObsAngleNoiseLevel; 108 float m_ObsDistNoiseLevel; 100 109 float m_ResampleWeight; 101 110
Note: See TracChangeset
for help on using the changeset viewer.