Particle filter, resampling step implementation

Question

I am not very good in statistic neither in probability theory, and I do not completely understand the mathematical background of Monte Carlo simulation or SIR sampling, but I'd like computer vision and I'm experimenting with it hoping to get some nice results with already known techniques (I don't want to reinvent the wheel neither to find some new method).

I have done an approximate implementation of particle filter with Bhattacharyya distance as liklihood score function. What I mean with approximate refers to the resampling part.

Now, my rough algorithm does something like that, in python pseudo-code:

for each particles:
    p.position = move with a random walk motion model

    p_histogram = calculate histogram that is under the particle
    p.weight = bhattacharyya_distance( p_histogram, model_histogram )

sort(particles, less_weight_comparator)
# now particles[0] has less weight --> less distance
# and particles[nParticles-1] has more weight --> more distance

# find best k particles
# i.e., the particles that has weight less then a threshold 0.25
k = -1
for each particles:
    if p.weight < 0.25: 
         k++;
    else:
         break

# resample all the other particles near those k best ones
if k!= -1:
    for i in range(k, nParticles):
        # choose a random number between 0 and k
        new_index = rand(0,k) 
        # resample the particle near the k_th 
        # with a gaussian random with 0 mean and 0.6 variance
        particles[i].position = particles[k].position * random_gauss(0, 0.6)
else:
    print "target is lost"

predicted_position = particles[0].position

this work quite well, but not every time, so I'm trying to improving it following some famous implementation, like the one in the Arulampalam_etal_2002 paper.

But.. I'm confused and I don't understand what is happening in the resampling stage.

With my rough and superficial algorithm I'm always aware of what is happening: particle with the weight less then my threshold are too far from the histogram model, and if k == -1 the target is lost. It is clear, in my mind.

In the paper mentioned (and in all the other particle filter papers) I'm reading about normalizing weight as all weights sums up to 1, and calculate a degeneracy value $N_eff$ (Equation 51) and.. I'm lost. My n_eff has values from 249.444 (at the first frame after the particle filter initialization so that some particles has to be near to the target) to 245.452 (more frame later, when the target is lost) and here, again I cannot find a threshold. If I normalize the weights, after the sorting, I have particles weight that goes from values like 0.00418279 to 0.00240669, and I cannot easily find a threashold value that can help me in understanding which are near the target or not, and it depends on how many particles I have so if I add more particles I'm more lost.

More over, in paper like Nummiaro_2002 the Bhattacharyya is calculated inside a gaussian (Equation 10) and I'm not sure on what this means, and I lost an easy way to understand which particle has an histogram near to the model or not.

I'm totally lost in what including the Bhattacharyya into a gaussian means, and why normalizing and how to find the threshold for understand how many particles has to be resamples and which are out which are in.

I hope someone can explain me a little bit, with simple and easy words.

Answer 1

Your question is about the resampling step, and let's focus on that.

Resampling is used in particle filtering to counteract "sample impoverishment" that is the fact that some particles may have very low weights. This is a waste of resources as you want to describe your probability distribution ate best. You should note that it should not interfere (a priori) with the measurement of prediction steps.

The optimal situation is to have all particles having roughly uniform weights and that's the goal set by most resampling algorithms. I propose you use this function which uses something similar to histogram equalization for a ratio resampleof particles (a nd-array were the last line represents the weights):

def resampling(particles, resample):
    """
    Resample weighted particles.

    The particles are grouped in N_pop batches to represent multiple velocities.
    The probability is given for all particles but corresponds to the proba-
    bility of one batch.

    Input
    -----
    N particles as a particles.shape array

    Output
    ------
    N particles as a particles.shape array

    Parameters
    ----------
    resample : gives the ratio of particles that get resampled at every frame (between 0 and 1)

    """
    particles_out = particles.copy()
    N = particles.shape[1]  # number of particles


    # resample a percentage of the particles (the total number keeps to N)
    if resample > 0:
        # draw resample (in %) random addresses of particles that we will reassign
        N_resample = int(N*resample)
        address_resample = np.random.permutation(np.arange(N))[0:N_resample]

        # draw from this subset some addresses uniformly over their pdf using histogram equalization
        proba_resample = particles_out[-1, address_resample]
        proba_resample /= proba_resample.sum()
        address = np.interp(np.linspace(0, 1, N_resample, endpoint=False)+1/2./N_resample,#np.random.rand(N_resample),
                            np.concatenate(([0.], np.cumsum(proba_resample))),
                            np.arange(N_resample+1))
        address = [int(k) for k in address]

        # reassign these particles and set their weight to a uniform value
        particles_out[:, address_resample] = particles_out[:, address_resample[address]]
        subset_weight = proba_resample[address].sum() # should be \approx N_resample / N, that is, resample
        particles_out[4, address_resample] = subset_weight / N_resample #1/N_resample #particles_out[4, address_resample].sum()

        # ultimately, normalize weights
        particles_out[4, :] /= particles_out[4, :].sum()

    return particles_out