Changeset 292


Ignore:
Timestamp:
01/28/2010 09:27:49 AM (11 years ago)
Author:
dave
Message:

reverted to old version

Location:
libs/magicsquares/in-progress/pf/src
Files:
2 edited

Legend:

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

    r291 r292  
    2222using namespace std; 
    2323 
     24/////////////////////////////////////////////////////////////// 
     25// Some util functions 
     26 
     27float FloatNoise() 
     28{ 
     29        return rand()%99999/float(99999)*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// the inverse of above 
     62void 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 
     69float Distance(float ax, float ay, float bx, float by) 
     70{ 
     71        float x=ax-bx; 
     72        float y=ay-by; 
     73        return sqrt(x*x+y*y); 
     74} 
     75 
    2476//////////////////////////////////////////////////////////////// 
    2577// The particle filter 
     
    2880{ 
    2981        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         
    4483        // Start the particles off in random positions 
    4584        for (vector<Particle>::iterator i=m_Particles.begin();  
    4685                i!=m_Particles.end(); ++i) 
    4786        { 
    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 
     92ParticleFilter::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 
     100void ParticleFilter::SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist) 
    54101{ 
    55102        m_PredictionNoiseLevel=Prediction; 
     103        m_ObsAngleNoiseLevel=ObsAngle; 
     104        m_ObsDistNoiseLevel=ObsDist; 
    56105} 
    57106 
     
    61110                i!=m_Particles.end(); ++i) 
    62111        { 
    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         
     121ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 
     122{ 
    71123        Resample(); 
    72124         
    73125        float TotalWeight = 0; 
    74126 
    75     // Now resample 
    76127        for (vector<Particle>::iterator i=m_Particles.begin();  
    77128                i!=m_Particles.end(); ++i) 
     
    80131                // between the one observation from the outside world, and the  
    81132                // 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.Angle-PObs.Angle)+(m_ObsAngleNoiseLevel*GaussianNoise()); 
     137                float DistErr = (Obs.Dist-PObs.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)); 
    86144                TotalWeight+=i->m_Weight; 
    87145        } 
    88146         
    89     // Normalise the weights 
    90147        for (vector<Particle>::iterator i=m_Particles.begin();  
    91148                i!=m_Particles.end(); ++i) 
     
    93150                i->m_Weight/=TotalWeight; 
    94151        } 
    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; 
    98165} 
    99166 
     
    130197                                { 
    131198                                        // Cast to a new completely random position/velocity 
    132                                         i->m_State->Randomise(); 
     199                                        i->m_State.Randomise(); 
    133200                                } 
    134201                                else 
    135202                                { 
    136203                                        // Copy settings from the 'best' current particle 
    137                                         (*i->m_State)=(*Highest->m_State); 
     204                                        i->m_State=Highest->m_State; 
    138205                                        // And jitter them a little - this stops the particles  
    139206                                        // converging too much on one state 
    140                                         i->m_State->Jitter(); 
    141                 } 
     207                                        i->m_State.Jitter(); 
     208                                } 
    142209                        }        
    143210                } 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.h

    r291 r292  
    1919 
    2020#include <vector> 
    21 #include <iostream> 
    22 #include <assert.h> 
    2321 
    2422using namespace std; 
    2523 
    26 ///////////////////////////////////////////////////////// 
    27 // A general purpose abstract base class particle filter 
     24float FloatNoise(); 
     25float GaussianNoise(); 
     26float GetAngle(float x, float y); 
     27void GetPos(float a, float d, float &x, float &y); 
     28float Distance(float ax, float ay, float bx, float by); 
     29 
    2830class ParticleFilter 
    2931{ 
    3032public: 
    3133        ParticleFilter(unsigned int NumParticles); 
    32         virtual ~ParticleFilter(); 
    33  
    34     // Needs an initialise as we have to call virtual funcs 
    35     void Initialise(); 
     34        ~ParticleFilter() {} 
    3635 
    3736        // An observation of the state 
     
    3938        { 
    4039        public: 
    41         virtual float Weight(const Observation *Target)=0; 
     40                float Angle,Dist; 
    4241        }; 
    4342 
     
    4746        public: 
    4847                // Gets the observation we would expect from this state 
    49                 virtual Observation *Observe()=0; 
     48                Observation Observe(); 
     49                 
    5050                // 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                 
    5259                // 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; 
    5870        }; 
    5971 
     
    6375                // A particle is a state with a cooresponding probabilistic  
    6476                // weighting 
    65                 State *m_State; 
     77                State m_State; 
    6678                float m_Weight; 
     79                 
    6780        }; 
    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         
    7582        // Use the model to predict the next state of each particle,  
    7683        // not forgetting to add some prediction noise. This should 'spread' 
    7784        // the particles out.  
    7885        void Predict(); 
    79  
     86         
    8087        // Set the particle weights according to the current real observation, 
    8188        // and resample particles with low weights, which 'tightens' the particles in. 
    82         State *Update(const Observation *Obs); 
     89        State Update(const Observation &Obs); 
    8390 
    84         void SetNoiseLevels(float Prediction); 
     91        void SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist); 
    8592        void SetResampleWeight(float Weight) { m_ResampleWeight=Weight; } 
    8693 
     
    9198        Particle* GetMostLikely(); 
    9299         
    93 protected: 
     100private: 
    94101 
    95102         
     
    98105         
    99106        float m_PredictionNoiseLevel; 
     107        float m_ObsAngleNoiseLevel; 
     108        float m_ObsDistNoiseLevel; 
    100109        float m_ResampleWeight; 
    101110         
Note: See TracChangeset for help on using the changeset viewer.