diff --git a/Localization/Particle filter/particle_filter.py b/Localization/Particle filter/particle_filter.py index ded5273..3ae27e6 100644 --- a/Localization/Particle filter/particle_filter.py +++ b/Localization/Particle filter/particle_filter.py @@ -135,6 +135,34 @@ def resample_particles(self, weights): self.particles = resampled_particles + def low_variance_resample(self, weights): + """Low variance resampling method.""" + weights = np.array(weights) + weights_sum = np.sum(weights) + + if weights_sum == 0: + print("All weights are zero! Check measurement model.") + weights = np.ones(self.num_particles) / self.num_particles + else: + weights /= weights_sum + + num_particles = self.num_particles + resampled_particles = [] + + r = np.random.uniform(0, 1/num_particles) + cumulative_sum = 0.0 + index = 0 + c = weights[0] + + for i in range(num_particles): + U = r + i / num_particles + while U > c: + index += 1 + c += weights[index] + resampled_particles.append(self.particles[index]) + + self.particles = resampled_particles + def update(self, measurement): """ @@ -232,4 +260,4 @@ def main(): pf.e_plot_particles() if __name__=="__main__": - main() \ No newline at end of file + main()