Ignore:
Timestamp:
01/04/2010 02:38:45 PM (10 years ago)
Author:
dave
Message:

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

File:
1 edited

Legend:

Unmodified
Added
Removed
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp

    r258 r260  
    2222using namespace std; 
    2323 
     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 
    2463 
    2564ParticleFilter::ParticleFilter(unsigned int NumParticles) 
     
    3675} 
    3776 
    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 } 
    5677 
    5778ParticleFilter::Observation ParticleFilter::State::Observe() 
    5879{ 
    5980        Observation NewObs; 
    60         NewObs.Angle=atan(y/x); 
     81        NewObs.Angle=GetAngle(x,y); 
    6182        NewObs.Dist=sqrt(x*x+y*y); 
    6283        return NewObs; 
     
    85106} 
    86107         
    87 void ParticleFilter::Update(const Observation &Obs) 
     108ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 
    88109{ 
     110        float TotalWeight = 0; 
     111 
    89112        for (vector<Particle>::iterator i=m_Particles.begin();  
    90113                i!=m_Particles.end(); ++i) 
     
    93116                Observation PObs=i->m_State.Observe(); 
    94117                 
    95                 float AngErr = Obs.Angle-PObs.Angle+(m_ObsAngleNoiseLevel*GaussianNoise()); 
    96                 float DistErr = Obs.Dist-PObs.Dist+(m_ObsDistNoiseLevel*GaussianNoise()); 
     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()); 
    97121                 
    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; 
    99142        } 
    100143         
    101144        Resample(); 
     145         
     146        return ret; 
    102147} 
    103148 
Note: See TracChangeset for help on using the changeset viewer.