# Optimizing a particle filter weighted selection step

I have a vector with 10000 particles that I use in a Particle Filter. The problem is that it's taking too long to compute the weighted selection of the particles.

I want to use a vector of weights (probabilities) to select elements from a vector (particles) to generate a new vector (new_particles) with the same size as the original, which has the most random items from the original set based on their weights.

Is there any way I can optimize this?

// util::NUMBER_OF_PARTICLES is a const int equal to 10000

// vector that holds particles weights
std::vector< double > probabilities;
probabilities.reserve(util::NUMBER_OF_PARTICLES);

// vector that will hold new particles
std::vector< GameParticle > new_particles;
new_particles.reserve(util::NUMBER_OF_PARTICLES);

// sum of all particle's weights
double sum_prob_all_particles = 0;

// calculate particle weights and save in vector
for(std::vector< GameParticle >::iterator it = game_particles_.begin(); it != game_particles_.end(); ++it)
{
double probability = util::getProbOfMeasurementGivenParticle(*it, measurement);
probabilities.push_back(probability);
sum_prob_all_particles += probability;
}

ROS_ERROR_STREAM_COND(sum_prob_all_particles == 0, "Error, all particles have a zero probability of being correct");

// select new particle group
// counter used to count number of selected particles
int particlesCounter = util::NUMBER_OF_PARTICLES;
// random variable used to select particle value from 0 to sum_prob_all_particles
double random_variable = ( std::rand() / (double) RAND_MAX ) * sum_prob_all_particles;
double sum_probs = 0;

// counter of current particle to select
int counter = util::NUMBER_OF_PARTICLES - 1;

// while there are particles to select
while (particlesCounter > 0)
{
// loops through all old particles and their weights
int counter = util::NUMBER_OF_PARTICLES - 1;
for(std::vector< double >::reverse_iterator it = probabilities.rbegin(); it != probabilities.rend(); ++it)
{
sum_probs += (double) *it;
// if random variable selected this weight, get this particle
while(sum_probs >= random_variable)
{
particlesCounter--;
new_particles.push_back(game_particles_[counter]);
random_variable += ( std::rand() / (double) RAND_MAX ) * sum_prob_all_particles;

// get out of loops if no more particles
if(particlesCounter == 0)
goto AFTER_PARTICLES_SELECTED;
}
counter--;
}
}

AFTER_PARTICLES_SELECTED:

ROS_INFO_STREAM("Observation step complete");


The part which is actually taking too long is after the // select new particle group comment. It takes an average of 0.984703 seconds for 5000 particles and 3.89787 seconds for 10000 particles, which is just too much.

The part before that comment only takes an average of 0.00142544 seconds for 10000 particles, so it's good enough.

• The first loop is O(n) the second one is O(n^2). Your algorithm for selecting a set of new elements is not very good. Aug 27 '14 at 13:56
• Do you have any examples of better ones? I'm open to suggestions. It just have to be weighted selection. Aug 27 '14 at 14:31
• Do you have a design of what you are trying to achieve (Which should be a comment in the code (as what you are doing is none trivial)). Seems you are picking some random number of elements but that may not be an accurate description. But I am sure you can convert that to O(n) algorithm. If you add comments to your code we could probably describe why the O(n^2) is sub optimal in achieving the goal (or it could be perfect). Aug 27 '14 at 17:50
• Tried to add extra info, but putting in simple words I have to select x random items of a vector (with also size x), based on their weights (probabilities). Aug 27 '14 at 18:06

I don't see any need for that goto, and it should be avoided whenever possible. It would be better to just perform the action after the condition, then break from the loop.

if (particlesCounter == 0)
{
ROS_INFO_STREAM("Observation step complete");
break;
}


Although this will only break from the for loop, the state of particlesCounter at that point will stop the while loop as well.

• I added that goto because I was changing the if(sum_probs >= random_variable) for a while and couldn't break both loops. Forgot to change that here. Aug 27 '14 at 6:09
• Yes. the "goto" is bad style but isn't causing the poor performance. He should still remove it. Aug 27 '14 at 16:24
• @CashCow: I know. Any aspect of the code on this site is open for review.
– Jamal
Aug 27 '14 at 16:56

There are some things that can improve the code by removing the C cast to double (unnecessary) and the goto.

The killer that will improve your algorithm and thus speed is to cache the partial sums of your probabilities.

I notice your loop then continues after you've "hit" the point where you increase your random_variable.

As you know the sum to the current point, you also know all the partial sums from this point onward.

Your vector of partial sums will be sorted and it will be $O(log N)$ to find how far you need to iterate through these then just add it to sum_probs in one go. You now add something to random_variable, however, if you've stored the original value that you had at the start of the loop, you know the new total before you have to increase it again.

If the total sum of all your probabilities (sum_prob_all_particles? or just the last element in your sum) doesn't "reach" random_variable at this point, just add that to sum_probs (that you started with) before you loop again.

• Thanks, I'll try to make this changes and see how much performance gain I get! Aug 27 '14 at 14:32