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?