Changeset 260 for libs/magicsquares/inprogress/pf/src/ParticleFilter.cpp
 Timestamp:
 01/04/2010 02:38:45 PM (10 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

libs/magicsquares/inprogress/pf/src/ParticleFilter.cpp
r258 r260 22 22 using namespace std; 23 23 24 /////////////////////////////////////////////////////////////// 25 // Some util functions 26 27 float FloatNoise() 28 { 29 return rand()%INT_MAX/float(INT_MAX)*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 //////////////////////////////////////////////////////////////// 62 // The particle filter 24 63 25 64 ParticleFilter::ParticleFilter(unsigned int NumParticles) … … 36 75 } 37 76 38 float ParticleFilter::FloatNoise()39 {40 return rand()%INT_MAX/float(INT_MAX)*21;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 77 57 78 ParticleFilter::Observation ParticleFilter::State::Observe() 58 79 { 59 80 Observation NewObs; 60 NewObs.Angle= atan(y/x);81 NewObs.Angle=GetAngle(x,y); 61 82 NewObs.Dist=sqrt(x*x+y*y); 62 83 return NewObs; … … 85 106 } 86 107 87 voidParticleFilter::Update(const Observation &Obs)108 ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 88 109 { 110 float TotalWeight = 0; 111 89 112 for (vector<Particle>::iterator i=m_Particles.begin(); 90 113 i!=m_Particles.end(); ++i) … … 93 116 Observation PObs=i>m_State.Observe(); 94 117 95 float AngErr = Obs.AnglePObs.Angle+(m_ObsAngleNoiseLevel*GaussianNoise()); 96 float DistErr = Obs.DistPObs.Dist+(m_ObsDistNoiseLevel*GaussianNoise()); 118 // todo: angle error will be wrong around 360 > 0 boundary 119 float AngErr = (Obs.AnglePObs.Angle)+(m_ObsAngleNoiseLevel*GaussianNoise()); 120 float DistErr = (Obs.DistPObs.Dist)+(m_ObsDistNoiseLevel*GaussianNoise()); 97 121 98 i>m_Weight =1/(AngErr*AngErr*100 + DistErr*DistErr); 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; 99 142 } 100 143 101 144 Resample(); 145 146 return ret; 102 147 } 103 148
Note: See TracChangeset
for help on using the changeset viewer.