def resample_particles(self):
'''Performs an importance particle resampling step based on the current particle weights.
'''
number_resampled_particles = self.filter_params.number_of_particles - self.filter_params.number_of_random_particles
step = 1. / number_resampled_particles
start_weight = uniform(0, step)
current_particle = 0
current_weight = self.particles[0].weight
weight_sum = 0.
#we add random particles to the particle set
new_particles = list()
for i in xrange(self.filter_params.number_of_random_particles):
particle = Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1/self.filter_params.number_of_particles)
weight_sum = weight_sum + particle.weight
new_particles.append(particle)
#we resample particles using low variance resampling
for i in xrange(number_resampled_particles):
weight = start_weight + i * step
while weight > current_weight:
current_particle = current_particle + 1
current_weight = current_weight + self.particles[current_particle].weight
particle = deepcopy(self.particles[current_particle])
weight_sum = weight_sum + particle.weight
new_particles.append(particle)
for i in xrange(self.filter_params.number_of_particles):
new_particles[i].weight = new_particles[i].weight / weight_sum
return new_particles
def generate_new_particle_set(self):
'''Discards the current particle set and creates a new set of random particles.
'''
self.particles = list()
for i in xrange(self.filter_params.number_of_particles):
self.particles.append(Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1./self.filter_params.number_of_particles))