Changeset 280


Ignore:
Timestamp:
01/12/2010 05:03:46 PM (11 years ago)
Author:
dave
Message:

added input noise and performance output

Location:
libs/magicsquares/in-progress/pf
Files:
4 edited

Legend:

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

    r261 r280  
    5151 
    5252        m_PF.SetNoiseLevels(1,0.01,0.01); 
    53         m_PF.SetResampleWeight(0.0001); 
     53        m_PF.SetResampleWeight(0.005); 
    5454 
    5555        cvNamedWindow( "pf", 1 ); 
     
    7070        {{255,0,0}}, 
    7171        {{255,0,255}}, 
    72                 {{255,255,255}} 
     72                {{255,255,255}}, 
     73                {{0,0,0}} 
    7374    }; 
    7475 
     
    103104} 
    104105 
    105 void Plot(IplImage *Image, ParticleFilter::State State, int colour,int size) 
     106void PlotPoint(IplImage *Image, ParticleFilter::State State, int colour,int size) 
    106107{ 
    107108        int x = State.x*2 + 200; 
    108109        int y = State.y*2 + 200; 
    109         cvRectangle(Image, cvPoint(x-size,y-size), cvPoint(x+size,y+size), colors[colour]);      
     110        cvRectangle(Image, cvPoint(x-size,y-size), cvPoint(x+size,y+size), colors[colour]); 
    110111} 
    111112 
    112 void PlotReal(IplImage *Image, ParticleFilter::State State, int colour) 
     113void PlotRect(IplImage *Image, ParticleFilter::State State, int colour) 
    113114{ 
    114115        int x = State.x*2 + 200; 
     
    117118} 
    118119 
    119 void PlotEst(IplImage *Image, ParticleFilter::State State, int colour) 
     120void PlotXHairs(IplImage *Image, ParticleFilter::State State, int colour) 
     121{ 
     122        int x = State.x*2 + 200; 
     123        int y = State.y*2 + 200; 
     124        cvLine(Image, cvPoint(x,0), cvPoint(x,400), colors[colour]); 
     125        cvLine(Image, cvPoint(0,y), cvPoint(400,y), colors[colour]); 
     126} 
     127 
     128void PlotRadar(IplImage *Image, ParticleFilter::State State, int colour) 
    120129{ 
    121130        int x = State.x*2 + 200; 
     
    124133} 
    125134 
     135float avnoise=0; 
     136float averror=0; 
     137 
    126138void App::Update(IplImage *camera) 
    127139{        
    128         int key=cvWaitKey(10); 
     140        int key=cvWaitKey(); 
    129141         
    130142        cvRectangle(camera, cvPoint(0,0), cvPoint(camera->width,camera->height), colors[8], -1);         
     
    142154        // Create an observation of the state 
    143155        ParticleFilter::Observation Obs = RealState.Observe(); 
    144         PlotReal(camera,RealState,1); 
     156        // Add noise to the observation 
     157        Obs.Angle+=GaussianNoise()*4; 
     158        Obs.Dist+=GaussianNoise()*2; 
     159        // Recalculate the state for this observation as we want to plot it 
     160        ParticleFilter::State ToPF; 
     161        GetPos(Obs.Angle, Obs.Dist, ToPF.x, ToPF.y); 
    145162         
    146         // Feed the observation in and return the estimated state 
    147         ParticleFilter::State Estimate = m_PF.Update(Obs); 
    148         PlotEst(camera,Estimate,3); 
    149163         
    150164        const vector<ParticleFilter::Particle> &p = m_PF.GetParticles(); 
     
    152166                i!=p.end(); ++i) 
    153167        { 
    154                 Plot(camera,i->m_State,0,i->m_Weight*20); 
     168                PlotPoint(camera,i->m_State,2,i->m_Weight*100); 
    155169        } 
     170         
     171        // Feed the observation in and return the estimated state 
     172        ParticleFilter::State Estimate = m_PF.Update(Obs); 
     173         
     174        PlotRadar(camera,Estimate,4); 
     175         
     176        // Plot what is being sent to the filter 
     177        PlotXHairs(camera,ToPF,3); 
     178         
     179        // Plot the real state we are trying to find 
     180        PlotXHairs(camera,RealState,1); 
     181 
     182        float blend=0.9; 
     183        avnoise=(avnoise*blend)+(Distance(ToPF.x, ToPF.y, RealState.x, RealState.y)*(1-blend)); 
     184        averror=(averror*blend)+(Distance(Estimate.x, Estimate.y, RealState.x, RealState.y)*(1-blend)); 
     185        cerr<<"performance: "<<avnoise-averror<<endl; 
     186         
    156187} 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp

    r272 r280  
    5959} 
    6060 
     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 
    6176//////////////////////////////////////////////////////////////// 
    6277// The particle filter 
     
    106121ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 
    107122{ 
     123        Resample(); 
     124         
    108125        float TotalWeight = 0; 
    109126 
     
    124141                // than the distance readings. This has the effect of making the pdf into a cresent  
    125142                // shape. 
    126                 i->m_Weight = 1/(AngErr*AngErr*0.1 + DistErr*DistErr); 
     143                i->m_Weight = 1/(fabs(AngErr)+fabs(DistErr)); 
    127144                TotalWeight+=i->m_Weight; 
    128145        } 
    129146         
    130         // Normalise the weights 
    131147        for (vector<Particle>::iterator i=m_Particles.begin();  
    132148                i!=m_Particles.end(); ++i) 
     
    145161                ret.y += i->m_State.y * i->m_Weight; 
    146162        } 
    147          
    148         Resample(); 
    149163         
    150164        return ret; 
     
    170184{ 
    171185        Particle *Highest = GetMostLikely(); 
    172  
    173         for (vector<Particle>::iterator i=m_Particles.begin();  
    174                 i!=m_Particles.end(); ++i) 
    175         { 
    176                 // If this particle has a low weight 
    177                 if (i->m_Weight<m_ResampleWeight) 
     186         
     187        if (Highest!=NULL) 
     188        { 
     189                for (vector<Particle>::iterator i=m_Particles.begin();  
     190                        i!=m_Particles.end(); ++i) 
    178191                { 
    179                         // Randomly choose between either... 
    180                         if (rand()%2==0) 
     192                        // If this particle has a low weight 
     193                        if (i->m_Weight<m_ResampleWeight) 
    181194                        { 
    182                                 // Cast to a new completely random position/velocity 
    183                                 i->m_State.Randomise(); 
    184                         } 
    185                         else 
    186                         { 
    187                                 // Copy settings from the 'best' current particle 
    188                                 i->m_State=Highest->m_State; 
    189                                 // And jitter them a little - this stops the particles  
    190                                 // converging too much on one state 
    191                                 i->m_State.Jitter(); 
    192                         } 
    193                 }        
    194         } 
    195 } 
    196  
     195                                // Randomly choose between either... 
     196                                if (rand()%2==0) 
     197                                { 
     198                                        // Cast to a new completely random position/velocity 
     199                                        i->m_State.Randomise(); 
     200                                } 
     201                                else 
     202                                { 
     203                                        // Copy settings from the 'best' current particle 
     204                                        i->m_State=Highest->m_State; 
     205                                        // And jitter them a little - this stops the particles  
     206                                        // converging too much on one state 
     207                                        i->m_State.Jitter(); 
     208                                } 
     209                        }        
     210                } 
     211        } 
     212} 
     213 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.h

    r261 r280  
    2525float GaussianNoise(); 
    2626float 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); 
    2729 
    2830class ParticleFilter 
     
    4446        public: 
    4547                // Gets the observation we would expect from this state 
    46                 // includes some noise, based on our expectation of the sensor 
    47                 // used to make the measurement. 
    4848                Observation Observe(); 
    4949                 
     
    6060                void Jitter() 
    6161                { 
    62                         x += GaussianNoise()*10; 
    63                         y += GaussianNoise()*10; 
    64                         dx += GaussianNoise()*0.5; 
    65                         dy += GaussianNoise()*0.5; 
     62                        x += GaussianNoise()*5; 
     63                        y += GaussianNoise()*5; 
     64                        dx += GaussianNoise()*0.05; 
     65                        dy += GaussianNoise()*0.05; 
    6666                } 
    6767                 
     
    9595        const vector<Particle> &GetParticles() { return m_Particles; } 
    9696 
     97        // Returns the particle with the highest weight 
     98        Particle* GetMostLikely(); 
     99         
    97100private: 
    98101 
    99         // Returns the particle with the highest weight 
    100         Particle* GetMostLikely(); 
    101102         
    102103        // Reset particles with low weight 
Note: See TracChangeset for help on using the changeset viewer.