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;
                // add weight to distribution
                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);
        }