Low variance resampling algorithm for particle filter

Question

For my particle filter, I decided to try using the low variance resampling algorithm as suggested in Probabilistic Robotics. The algorithm implements systematic resampling while still considering relative particle weights. I implemented the algorithm in Matlab, almost word-for-word from the text:

function [state] = lowVarianceRS(prev_state, weight, state_size)
    state = zeros(1,state_size);    % Initialize empty final state
    r = rand;                       % Select random number between 0-1
    w = weight(1);                  % Initial weight
    i = 1;
    j = 1;

    for m = 1:state_size
        U = r + (m - 1)/state_size; % Index of original sample + size^-1
        while U > w                 % I'm not sure what this loop is doing
            i = i + 1;
            w = w + weight(i);
        end
        state(j) = prev_state(i);   % Add selected sample to resampled array
        j = j + 1;
    end
end

As would be expected given the while loop structure, I am getting an error for accessing weight(i), where i exceeds the array dimensions.

To solve this, I was considering circularly shifting my weight array (putting the first index used as the first value in weight, so that I never exceed matrix dimensions). However, I wasn't sure if this would negatively impact the rest of the algorithm, seeing as I'm having trouble understanding the purpose of the U calculation and while loop.

Could anyone help clarify the purpose of U and the while loop, and whether or not a circular shift is an acceptable fix?

Answer 1

I misread the text. r should be a random number between 0 and M^-1. Changing this should solve all your problems.

---

The re-sampling algorithm's purpose is (roughly) to remove particles that have a low probability of representing the system that you're tracking. This is done by stacking the all particles together. Each particle's size is equal to the probability that it represents the system. So the weight of all the particles is 1 or %100. Now M random spots on this range of particles are chosen and the particles that occupiesy those spots are kept. Particles that are not chosen are discarded.

The low variance re-sampling algorithm splits the particle range space into M sections and keeps the particle occupying the rand(0,M^-1) spot at each section. This is what U is, the random spot on the jth section of the probability space.

The advantage of the low variance re-sampling algorithm is the ease of implementation and it's robustness. If we randomly choose a spot M times, there is a non zero chance we could get the same particle each time. This could cause huge problems for the filter.

---

ignore: It looks like your random weight r should have a standard deviation of M^-1.