Add a mean point display to show how good the robot's position has be…

…en determined.

README

@@ -10,7 +10,11 @@ for a high resolution laser scanner.
   In the arena display the robot is represented by a small green
 turtle and its beliefs are red/blue dots. The more a belief matches
 the current sensor reading, the more the belief's colour changes to
-red. Beacons are little cyan dots.
+red. Beacons are little cyan dots. For illustration purposes, we
+also compute the mean of all reasonably confident particles, then
+check whether that point is actually a center of a cluster. This
+point is represented by a gray circle, which becomes green when the
+algorithm thinks it actually determined the robot's position.
 
   The robot then starts to randomly move around the maze. As it
 moves, its beliefs are updated using the particle filter algorithm.

draw.py

@@ -66,6 +66,15 @@ def is_free(self, x, y):
         xx = int(x)
         return self.maze[yy][xx] == 0
 
+    def show_mean(self, x, y, confident=False):
+        if confident:
+            turtle.color("#00AA00")
+        else:
+            turtle.color("#cccccc")
+        turtle.setposition(x, y)
+        turtle.shape("circle")
+        turtle.stamp()
+
     def show_particles(self, particles):
         turtle.clearstamps()
         turtle.shape('dot')

particle_filter.py

@@ -173,8 +173,36 @@ def move(self, maze):
         else:
             p.w = 0
 
+    # ---------- Try to find current best estimate for display ----------
+    # (not part of the core algorithm)
+    # We just compute the mean for all particles that have a reasonably
+    # good weight.
+    m_x, m_y, m_count = 0, 0, 0
+    for p in particles:
+        if p.w > 0.6:
+            m_count += 1
+            m_x += p.x
+            m_y += p.y
+
+    if m_count > PARTICLE_COUNT / 10:
+        m_x /= m_count
+        m_y /= m_count
+
+        # Now compute how good that mean is -- check how many particles
+        # actually are in the immediate vicinity
+        m_count = 0
+        for p in particles:
+            if world.distance(p.x, p.y, m_x, m_y) < 1:
+                m_count += 1
+    else:
+        m_x = None
+
     # ---------- Show current state ----------
     world.show_particles(particles)
+
+    if m_x is not None:
+        world.show_mean(m_x, m_y, m_count > PARTICLE_COUNT * 0.8)
+
     world.show_robot(robbie)
 
     # ---------- Shuffle particles ----------