✨

selflocalization/particle.py

@@ -41,17 +41,16 @@ class Particle(object):
 def estimate_pose(particles_list):
     """Estimate the pose from particles by computing the average position and orientation over all particles. 
     This is not done using the particle weights, but just the sample distribution."""
-    x_sum = 0.0
-    y_sum = 0.0
-    cos_sum = 0.0
-    sin_sum = 0.0
-     
-    for particle in particles_list:
-        x_sum += particle.getX()
-        y_sum += particle.getY()
-        cos_sum += np.cos(particle.getTheta())
-        sin_sum += np.sin(particle.getTheta())
-        
+    x_values = [i.getX() for i in particles_list]
+    y_values = [i.getY() for i in particles_list]
+    cos_values = [np.cos(i.getTheta()) for i in particles_list]
+    sin_values = [np.sin(i.getTheta()) for i in particles_list]
+
+    x_sum = sum(x_values)
+    y_sum = sum(y_values)
+    cos_sum = sum(cos_values)
+    sin_sum = sum(sin_values)
+
     flen = len(particles_list)
     if flen != 0:
         x = x_sum / flen
@@ -61,8 +60,14 @@ def estimate_pose(particles_list):
         x = x_sum
         y = y_sum
         theta = 0.0
-        
-    return Particle(x, y, theta)
+
+    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
      
      
 def move_particle(particle, delta_x, delta_y, delta_theta):