Changeset 261


Ignore:
Timestamp:
01/04/2010 03:24:57 PM (10 years ago)
Author:
dave
Message:

added velocity to the model

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

Legend:

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

    r260 r261  
    5151 
    5252        m_PF.SetNoiseLevels(1,0.01,0.01); 
    53         m_PF.SetResampleWeight(0.01); 
     53        m_PF.SetResampleWeight(0.0001); 
    5454 
    5555        cvNamedWindow( "pf", 1 ); 
     
    134134        // Our actual state 
    135135        ParticleFilter::State RealState; 
     136        // We'll move the target on a sine wave - the particle state is not 
     137        // able to model this behaviour very well, as it only copes with  
     138        // linear velocity 
    136139        RealState.x=50*sin(m_FrameNum*0.01f); 
    137140        RealState.y=-50; 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp

    r260 r261  
    7070                i!=m_Particles.end(); ++i) 
    7171        { 
    72                 i->m_State.x = FloatNoise()*100; 
    73                 i->m_State.y = FloatNoise()*100; 
     72                i->m_State.Randomise(); 
    7473        } 
    7574} 
     
    9695                i!=m_Particles.end(); ++i) 
    9796        { 
    98                 // As we don't have a model we can use for prediction 
    99                 // we just add some noise to the state here. 
    100                 // It would be quite simple to add velocity into our state, 
    101                 // in which case we'd add the velocity to the position here  
    102                 // as well. 
     97                // Apply the velocity to the particle position 
     98                i->m_State.x+=i->m_State.dx; 
     99                i->m_State.y+=i->m_State.dy; 
     100                // Add some noise to the position 
    103101                i->m_State.x+=GaussianNoise()*m_PredictionNoiseLevel; 
    104102                i->m_State.y+=GaussianNoise()*m_PredictionNoiseLevel; 
     
    147145} 
    148146 
    149 void ParticleFilter::Resample() 
     147ParticleFilter::Particle* ParticleFilter::GetMostLikely() 
    150148{ 
     149        Particle *ret=NULL; 
     150        float HighestWeight=0; 
    151151        for (vector<Particle>::iterator i=m_Particles.begin();  
    152152                i!=m_Particles.end(); ++i) 
    153153        { 
     154                if (i->m_Weight>HighestWeight) 
     155                { 
     156                        ret = &(*i); 
     157                        HighestWeight = i->m_Weight; 
     158                } 
     159        } 
     160        return ret; 
     161} 
     162 
     163void ParticleFilter::Resample() 
     164{ 
     165        Particle *Highest = GetMostLikely(); 
     166 
     167        for (vector<Particle>::iterator i=m_Particles.begin();  
     168                i!=m_Particles.end(); ++i) 
     169        { 
     170                // If this particle has a low weight 
    154171                if (i->m_Weight<m_ResampleWeight) 
    155172                { 
    156                         // cast to a random position 
    157                         i->m_State.x = FloatNoise()*100; 
    158                         i->m_State.y = FloatNoise()*100; 
     173                        // Randomly choose between either... 
     174                        if (rand()%2==0) 
     175                        { 
     176                                // Cast to a new completely random position/velocity 
     177                                i->m_State.Randomise(); 
     178                        } 
     179                        else 
     180                        { 
     181                                // Copy settings from the 'best' current particle 
     182                                i->m_State=Highest->m_State; 
     183                                // And jitter them a little - this stops the particles  
     184                                // converging too much on one state 
     185                                i->m_State.Jitter(); 
     186                        } 
    159187                }        
    160188        } 
    161189} 
     190 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.h

    r260 r261  
    2222using namespace std; 
    2323 
     24float FloatNoise(); 
     25float GaussianNoise(); 
     26float GetAngle(float x, float y); 
     27 
    2428class ParticleFilter 
    2529{ 
     
    3943        { 
    4044        public: 
    41                 float x,y; 
    4245                // Gets the observation we would expect from this state 
    4346                // includes some noise, based on our expectation of the sensor 
    4447                // used to make the measurement. 
    4548                Observation Observe(); 
     49                 
     50                // Put the state into a random position and velocity 
     51                void Randomise() 
     52                { 
     53                        x = FloatNoise()*100; 
     54                        y = FloatNoise()*100; 
     55                        dx = GaussianNoise(); 
     56                        dy = GaussianNoise(); 
     57                } 
     58                 
     59                // Add a small random amount to the position and velocity 
     60                void Jitter() 
     61                { 
     62                        x += GaussianNoise()*10; 
     63                        y += GaussianNoise()*10; 
     64                        dx += GaussianNoise()*0.5; 
     65                        dy += GaussianNoise()*0.5; 
     66                } 
     67                 
     68                float x,y; 
     69                float dx,dy; 
    4670        }; 
    4771 
     
    5377                State m_State; 
    5478                float m_Weight; 
     79                 
    5580        }; 
    5681         
     
    7196 
    7297private: 
     98 
     99        // Returns the particle with the highest weight 
     100        Particle* GetMostLikely(); 
     101         
     102        // Reset particles with low weight 
    73103        void Resample(); 
    74104         
Note: See TracChangeset for help on using the changeset viewer.