Changeset 261
- Timestamp:
- 01/04/2010 03:24:57 PM (11 years ago)
- Location:
- libs/magicsquares/in-progress/pf
- Files:
-
- 4 edited
Legend:
- Unmodified
- Added
- Removed
-
libs/magicsquares/in-progress/pf/src/App.cpp
r260 r261 51 51 52 52 m_PF.SetNoiseLevels(1,0.01,0.01); 53 m_PF.SetResampleWeight(0.0 1);53 m_PF.SetResampleWeight(0.0001); 54 54 55 55 cvNamedWindow( "pf", 1 ); … … 134 134 // Our actual state 135 135 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 136 139 RealState.x=50*sin(m_FrameNum*0.01f); 137 140 RealState.y=-50; -
libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp
r260 r261 70 70 i!=m_Particles.end(); ++i) 71 71 { 72 i->m_State.x = FloatNoise()*100; 73 i->m_State.y = FloatNoise()*100; 72 i->m_State.Randomise(); 74 73 } 75 74 } … … 96 95 i!=m_Particles.end(); ++i) 97 96 { 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 103 101 i->m_State.x+=GaussianNoise()*m_PredictionNoiseLevel; 104 102 i->m_State.y+=GaussianNoise()*m_PredictionNoiseLevel; … … 147 145 } 148 146 149 void ParticleFilter::Resample()147 ParticleFilter::Particle* ParticleFilter::GetMostLikely() 150 148 { 149 Particle *ret=NULL; 150 float HighestWeight=0; 151 151 for (vector<Particle>::iterator i=m_Particles.begin(); 152 152 i!=m_Particles.end(); ++i) 153 153 { 154 if (i->m_Weight>HighestWeight) 155 { 156 ret = &(*i); 157 HighestWeight = i->m_Weight; 158 } 159 } 160 return ret; 161 } 162 163 void 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 154 171 if (i->m_Weight<m_ResampleWeight) 155 172 { 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 } 159 187 } 160 188 } 161 189 } 190 -
libs/magicsquares/in-progress/pf/src/ParticleFilter.h
r260 r261 22 22 using namespace std; 23 23 24 float FloatNoise(); 25 float GaussianNoise(); 26 float GetAngle(float x, float y); 27 24 28 class ParticleFilter 25 29 { … … 39 43 { 40 44 public: 41 float x,y;42 45 // Gets the observation we would expect from this state 43 46 // includes some noise, based on our expectation of the sensor 44 47 // used to make the measurement. 45 48 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; 46 70 }; 47 71 … … 53 77 State m_State; 54 78 float m_Weight; 79 55 80 }; 56 81 … … 71 96 72 97 private: 98 99 // Returns the particle with the highest weight 100 Particle* GetMostLikely(); 101 102 // Reset particles with low weight 73 103 void Resample(); 74 104
Note: See TracChangeset
for help on using the changeset viewer.