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);
        }


Comments

  1. Κalman filter to the rescue. Το καλύττερο πράμα μετά τον διχαλωτό δονητή.

    το captcha ήταν "kingle"... υποσυνείδητα μυνήματα της amazon....

    ReplyDelete
  2. Βασικά δε χρησιμοποιώ καν το kalman filter, αν δεις εν χρησιμοποιώ καν το kalman.Correct(measurement) , απλά βολεύει το structure του για να κρατώ το state.

    ReplyDelete

Post a Comment