✨

selflocalization/particle.py

@@ -67,7 +67,7 @@ def estimate_pose(particles_list):
     sin_certainty = np.average([abs(np.sin(theta) - i) for i in sin_values])
 
     certainty = np.average([x_certainty, y_certainty, cos_certainty, sin_certainty])
-    return Particle(x, y, theta)
+    return Particle(x, y, theta), certainty
      
      
 def move_particle(particle, delta_x, delta_y, delta_theta):

selflocalization/selflocalize.py

@@ -199,7 +199,7 @@ try:
     num_particles = 2000
     particles = initialize_particles(num_particles)
 
-    est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+    est_pose, _ = particle.estimate_pose(particles) # The estimate of the robots current pose
 
     # Driving parameters
     velocity = 0.0 # cm/sec
@@ -252,11 +252,11 @@ try:
                 p.setWeight(1.0/num_particles)
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
-        est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
         calc_weight(est_pose, landmark_values)
         est_weight = est_pose.weight * 10**23
         print(est_weight)
-        if est_weight > 1:
+        if est_weight > 1 and certainty > 1:
             break
         else:
             arlo.go_diff(POWER, POWER, 0, 1)