Changeset 261

Timestamp:

01/04/2010 03:24:57 PM (11 years ago)

Author:

dave

Message:

added velocity to the model

Location:

libs/magicsquares/in-progress/pf

Files:

• pf (modified) (previous)
• src/App.cpp (modified) (2 diffs)
• src/ParticleFilter.cpp (modified) (3 diffs)
• src/ParticleFilter.h (modified) (4 diffs)

libs/magicsquares/in-progress/pf/src/App.cpp

@@ -51 +51 @@
 
         m_PF.SetNoiseLevels(1,0.01,0.01);
-        m_PF.SetResampleWeight(0.01);
+        m_PF.SetResampleWeight(0.0001);
 
         cvNamedWindow( "pf", 1 );
@@ -134 +134 @@
         // Our actual state
         ParticleFilter::State RealState;
+        // We'll move the target on a sine wave - the particle state is not
+        // able to model this behaviour very well, as it only copes with 
+        // linear velocity
         RealState.x=50*sin(m_FrameNum*0.01f);
         RealState.y=-50;

libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp

@@ -70 +70 @@
                 i!=m_Particles.end(); ++i)
         {
-                i->m_State.x = FloatNoise()*100;
-                i->m_State.y = FloatNoise()*100;
+                i->m_State.Randomise();
         }
 }
@@ -96 +95 @@
                 i!=m_Particles.end(); ++i)
         {
-                // As we don't have a model we can use for prediction
-                // we just add some noise to the state here.
-                // It would be quite simple to add velocity into our state,
-                // in which case we'd add the velocity to the position here 
-                // as well.
+                // Apply the velocity to the particle position
+                i->m_State.x+=i->m_State.dx;
+                i->m_State.y+=i->m_State.dy;
+                // Add some noise to the position
                 i->m_State.x+=GaussianNoise()*m_PredictionNoiseLevel;
                 i->m_State.y+=GaussianNoise()*m_PredictionNoiseLevel;
@@ -147 +145 @@
 }
 
-void ParticleFilter::Resample()
+ParticleFilter::Particle* ParticleFilter::GetMostLikely()
 {
+        Particle *ret=NULL;
+        float HighestWeight=0;
         for (vector<Particle>::iterator i=m_Particles.begin(); 
                 i!=m_Particles.end(); ++i)
         {
+                if (i->m_Weight>HighestWeight)
+                {
+                        ret = &(*i);
+                        HighestWeight = i->m_Weight;
+                }
+        }
+        return ret;
+}
+
+void ParticleFilter::Resample()
+{
+        Particle *Highest = GetMostLikely();
+
+        for (vector<Particle>::iterator i=m_Particles.begin(); 
+                i!=m_Particles.end(); ++i)
+        {
+                // If this particle has a low weight
                 if (i->m_Weight<m_ResampleWeight)
                 {
-                        // cast to a random position
-                        i->m_State.x = FloatNoise()*100;
-                        i->m_State.y = FloatNoise()*100;
+                        // Randomly choose between either...
+                        if (rand()%2==0)
+                        {
+                                // Cast to a new completely random position/velocity
+                                i->m_State.Randomise();
+                        }
+                        else
+                        {
+                                // Copy settings from the 'best' current particle
+                                i->m_State=Highest->m_State;
+                                // And jitter them a little - this stops the particles 
+                                // converging too much on one state
+                                i->m_State.Jitter();
+                        }
                 }       
         }
 }
+

libs/magicsquares/in-progress/pf/src/ParticleFilter.h

@@ -22 +22 @@
 using namespace std;
 
+float FloatNoise();
+float GaussianNoise();
+float GetAngle(float x, float y);
+
 class ParticleFilter
 {
@@ -39 +43 @@
         {
         public:
-                float x,y;
                 // Gets the observation we would expect from this state
                 // includes some noise, based on our expectation of the sensor
                 // used to make the measurement.
                 Observation Observe();
+                
+                // Put the state into a random position and velocity
+                void Randomise()
+                {
+                        x = FloatNoise()*100;
+                        y = FloatNoise()*100;
+                        dx = GaussianNoise();
+                        dy = GaussianNoise();
+                }
+                
+                // Add a small random amount to the position and velocity
+                void Jitter()
+                {
+                        x += GaussianNoise()*10;
+                        y += GaussianNoise()*10;
+                        dx += GaussianNoise()*0.5;
+                        dy += GaussianNoise()*0.5;
+                }
+                
+                float x,y;
+                float dx,dy;
         };
 
@@ -53 +77 @@
                 State m_State;
                 float m_Weight;
+                
         };
         
@@ -71 +96 @@
 
 private:
+
+        // Returns the particle with the highest weight
+        Particle* GetMostLikely();
+        
+        // Reset particles with low weight
         void Resample();