diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 1fb8dac1..bd9da70c 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -60,15 +60,15 @@ def normalize_weight(particles): sumw = sum([p.w for p in particles]) - if sumw <= 0.0000001: + try: + for i in range(N_PARTICLE): + particles[i].w /= sumw + except ZeroDivisionError: for i in range(N_PARTICLE): particles[i].w = 1.0 / N_PARTICLE return particles - for i in range(N_PARTICLE): - particles[i].w /= sumw - return particles