✨

selflocalization/particle.py

@@ -60,14 +60,7 @@ def estimate_pose(particles_list):
         x = x_sum
         y = y_sum
         theta = 0.0
-
-    x_certainty = np.average([abs(x - i) for i in x_values])
-    y_certainty = np.average([abs(y - i) for i in y_values])
-    cos_certainty = np.average([abs(np.cos(theta) - i) for i in cos_values])
-    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), certainty
+    return Particle(x, y, theta)
      
      
 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,21 +252,20 @@ try:
                 p.setWeight(1.0/num_particles)
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
-        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
+        est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
         calc_weight(est_pose, landmark_values)
         est_weight = est_pose.weight * 10**10
         print("est_weight:", est_weight)
-        print("certainty:", certainty)
         if est_weight > 1:
             draw_world(est_pose, particles, world)
             cv2.imwrite("test.png", world)
             break
-        else:
-            arlo.go_diff(POWER, POWER, 0, 1)
-            sleep((15 * TURN_T)/1000)
-            arlo.stop()
-            for p in particles:
-                p.setTheta(p.theta + np.deg2rad(15))
+        # else:
+        #     arlo.go_diff(POWER, POWER, 0, 1)
+        #     sleep((15 * TURN_T)/1000)
+        #     arlo.stop()
+        #     for p in particles:
+        #         p.setTheta(p.theta + np.deg2rad(15))
 
         if showGUI:
             # Draw map