Particle Filter Simulation

This simulation was used as a means to test our implementation of particle filter localization on a NAO robot for a class project. We set up six beacons in the lab, and determined the robot's distance and angle from each one using vision-based blob detection. These measurements, along with known real-world positions of each beacon, were used by the particle filter to approximate the robot's position and orientation.


Your browser does not support the canvas element.

Move your mouse around the screen to move the robot. Click to lock the robot's position.

         

Number of Particles:
100
Animation Speed (FPS):
10 FPS
Random Walk Frequency:
Every 5 Frames
Random Walk Max Distance:
40

Robot Position: (x, y)



Click on an image to enlarge.

The robot is displayed in the simulation as a white square, but its actual position is not known by the particle filter. That is what the particle filter is trying to estimate. In the lab, we worked with the Aldebaran NAO humanoid robot.

There are six localization beacons, each represented by a yellow square. The position of each beacon is known by the particle filter. The robot determines its distance from each beacon it sees (in the case of the simulation, no sensing is actually necessary - we just use the distance formula between the robot and each beacon).

All of the particles, represented by small blue squares, are initially placed around the map (screen) randomly. Their weights are set to 1.0. Then every frame, each particle's weight is degraded based on its distance to each beacon as compared to the robot's distance to said beacon. For example, if the robot is 500 pixels from beacon A and particle n is 1000 pixels from beacon A, the probability that particle n accurately represents the location of the robot must be decreased. If a particle's weight goes below a pre-defined threshold, we replace that particle with a copy of the particle that currently has the greatest weight. That way, over time, the particles that do not accurately represent the robot's location will be filtered out.

There is always a chance that none of the particles will end up near the correct location. To deal with this problem, the particle filter does a "random walk" where each particle is randomly moved some distance every few frames (the frequency and distance of the random walk can be adjusted in this simulation). Although this may cause some particles to move away from the robot's actual location, it will also cause some to move the right way. Thus, if all particles end up in the wrong place, the random walk ensures that they will not become "stuck" there. Additionally, if the robot is suddenly moved (the kidnapped robot problem) to a different place, the random walk allows the particle filter to re-discover the right location.

When the robot starts moving, all particles are moved by the same amount. This allows the particle filter to respond more quickly to changes in the robot's position. In this simulation, steadily moving the robot will also move the particles.

The estimated robot location is shown by the green circle. This circle is centered at the weighted average location of all particles. That is, each particle's weight (i.e. probability) is taken into account to calculate the estimated robot position. The red circle represents the unweighted (direct) average location of all particles, providing a slightly less accurate estimate.


This simulation only deals with the robot's location, and not its orientation. Once we were able to get the location part to work, adapting the filter to also handle orientation was trivial.

The Particle Filter simulator source code is available under the MIT Licence and can be downloaded here.

The script that defines the matrices and interfaces with the HTML is also available here.




Back to Top