## Tuesday, November 22, 2011

### Particle Filter Code Snippet

Below I present the very core of my particle filter. I use the standard CONDENSATION algorithm with histogram distance for weighting my samples.

        /// <summary>
/// Particle filter prediction step.
/// Each particle represents a hypothesis. The particle's state is propagated
/// according to a linear model , it is then diffused using random noise, the new
/// state is weighted according to histogram distance.
/// The new states are finanlly sampled and the process repeats
/// </summary>
/// <param name="_imageChannels">The current frames channels to be used to update weight</param>
/// <param name="_histOriginal">The original normalized histogram</param>
public void PredictionUpdate(Image<Gray, byte>[] _imageChannels, DenseHistogram _histOriginal)
{
Particle particle;
Matrix<float> tmpState;
float sumWeight = 0.0f;
int noParticlesCurrent = 0, k = 0;

// sample, move , and diffuse particles
for (int i = 0; i < noParticles; i++)
{
particle = particles[i];
particle.State = particlesSampling[i].State;
// create normal distributed random state
randState.SetRandNormal(new MCvScalar(0), new MCvScalar(2.0f * (1.0f - particle.Weight)));
// propagate state and diffuse it
particle.State = particle.kalmanFilter.Predict() + randState;
sumWeight += particle.UpdateWeight(_imageChannels, _histOriginal);
}

Array.Sort(particles);
Array.Reverse(particles);

while (noParticlesCurrent < noParticles && sumWeight > 0.0f)
{
// get number of samples
if ((float)rand.NextDouble() <= particles[k].Weight)
{
particlesSampling[noParticlesCurrent].State = particles[k].State;
noParticlesCurrent++;
}
k = (k + 1) % noParticles;
}
// sum the top 10% of the particles
tmpState = new Matrix<float>(6, 1);
for (int i = 0; i < particles.Length / 10; i++)
tmpState = tmpState + particles[i].State;
tmpState /= particles.Length / 10;
// convert particles to a detection rectangle
detectionRectangle = Particle.StateToRectangle(tmpState);
}