Actually, our plan was to have a Lego Mindstorms NXT robot localize itself (figure out its initially unknown position in a known environment), then navigate to a sound source while avoiding obstacles. We couldn't get the physical robot to cooperate, so we did the localization piece in simulation.
We got the particle filter working, and made some nice videos of the particles in action. Those and more info after the jump.
Watching The Filter Work
In the video below, the robot navigates by moving from the particle-based estimate of its own location to the known goal location, "sensing" obstacles with a simulated sonar and avoiding them on the way.
You can see the particles converge more and more at each step until they track the robot quite closely. The particles actually converge a lot quicker than is shown - I manually extended the length of the first few frames to make the particle motion easier to see.
Also, you may note that the robot and particles sometimes appear to pass through the corner of an obstacle. That's because one of the team members insisted on using a "+" icon for the obstacles, which makes them look slightly larger than they actually are. Thanks [name redacted]!
In the simulated environment below, the robot is the dark blue square and the goal is the black square. The brown boxes are the obstacles, and the particles are the light blue dots.
How It Works
Particle filters are part of the larger set of Monte Carlo methods for estimating complex probability distributions. The basic idea is to sample from a simpler distribution that in some way approximates the difficult distribution you want.
Here, the difficult distribution is the probability that the robot is in a specific position on the map, given that its sonar distance sensor returns a specific set of readings. (Our robot had a rotating sonar, so a set of readings was one 90-degree sweep.)
The particle filter creates a large number of particles, uniformly distributed around the map. Like the robot, each particle has a position and heading.
|The simulated environment at the beginning of a run, showing uniformly distributed particles.|
Every time the robot moves, we update the location of the particles with the same motion, plus a little Gaussian noise. Every time the robot records a sonar sweep, we update for each particle the probability that that particle is in the same location as the robot. We then resample the entire particle array, discarding particles with low probability and making more copies of particles with high probabilities.
If our motion and sensor models are adequate, the particle locations should converge to a reasonably small cluster, the mean of which should be close to the robot's actual location. It's not uncommon for the particle cluster to grow and shrink in size as the robot moves and the algorithm runs.