Robot Navigation with a Particle Filter

As robots become smarter, faster and more capable, they are being developed to perform increasingly complex tasks. In order to perform these tasks properly, robots are becoming more and more dependent on accurate navigation through the environment in which they operate. Somewhere in the future, if intelligent robots were to rise up and demand fundamental rights, I think one of the first things they would ask for is the answer to the question, “Where am I?”.

Where am I?

Where am I?

Particle Filters are a relatively new method of prediction of a given parameter based on non-linear non-gaussian data (data that jumps around with no apparent pattern), and there are a number of very very complicated articles available on the internet that explain how Particle Filters can be implemented.

                                                                                                         Particle Filters are an attractive option today because they are notionally very easy to understand and their only drawback is that they are computationally expensive to run. But as computers get better and processing speeds get faster, Particle Filters become viable options especially in situations where other filters like the Kalman Filter may suffer from poor performance.

                                                                                                Unfortunately for me, my mathematics foundations are too shaky to understand most of these research papers, but I was lucky enough to find some excellent video tutorials on the very basics of Particle Filters. In particular, Student Dave and Andreas Svensson have uploaded some absolutely superb tutorials on youtube. I am indebted to these two great guys for sharing their knowledge and strongly recommend their videos for anyone needing a basic introduction into Particle Filters.

                                                                                                               This blog post explains a Particle Filter I created to help me localise the position of a roving robot that would need to navigate safely throughout my house. In this simulation, the robot is manoeuvring within just one rectangular shaped room. Also, since this is just my first attempt at programming a particle filter, I have restricted the number of obstacles/furniture (objects that the robot will use to determine its position) to a single pillar in the centre of the room.

                                                                                                                  My Particle Filter loops over these few steps….

                                                                                                                     1. The robot enters the Room.

Simulation Structure

Simulation Structure

 2. The robot generates a uniform distribution of a large number (400 in my filter) of possible positions that it could possibly be located at. Call this set of (x,y pair) positions = P0, each (x,y) pair representing one particle.

Initial Uniform Distribution

Initial Uniform Distribution

3. The robot takes a measurement of some parameter. In my filter, the robot uses sonar to echo range the pillar in the centre of the room. The robot now knows its true distance and true direction from the pillar. One might wonder why simple trigonometry can’t be used to estimate the robot’s position. It can. But a trigonometric solution would quickly get out of hand if there were multiple pillars, walls, corridors, and furniture in the room.

The robot takes a measurement

The robot takes a measurement

4. The robot now computes the bearing and distance of the pillar from every possible particle (x,y pair) in set P0. It compares all these values with the true bearing and distance it observed in Step 3. Particles in P0 that match the observed data closely are allocated a stronger “weight” and particles that do not match the observations nicely are allocated a smaller “weight”.

                                                                                                               After assigning weights, these are normalised so that they all add up to a total of 1. To do this we divide each weight with the sum of all weights.

True Direction with predicted direction of 20 particles. Each particle is weighted depending on how well it matches the true direction to the pillar. The weights are normalised to add up to 1.

True Direction (on top) with predicted direction (first column) of 20 particles. Each particle is weighted depending on how well it matches the true direction to the pillar. The weights (second column) are normalised to add up to 1.

Step 4 is very similar to Bayesian Reasoning since the robot

  1. Has a prior probability that is located at position (xn,yn)

  2. Obtains new evidence based on its sensors

  3. Determines a posterior probability of being at (xn,yn) based on this new evidence

                                                                                                                   5. Resampling. The robot resamples P0 based on the normalised weights of each particle. In resampling, particles with higher weights live on (and multiply) and those with lower weights die away. Thus only those particles that match the observed data closely survive into the next iteration. Over time, these particles will cluster together around the true position of the robot. Proper resampling is perhaps the most critical part of a working particle filter and it took me a long time before I understood it properly.

Particles cluster around the true position of the robot

Particles cluster around the true position of the robot

6. Now that the Robot has a better estimate of its position, it moves over the ground heading to its target location in the room. At the same time, all the particles (x,y pairs) that survived the Re-sampling step are also translated in space with the estimated heading direction and speed of the robot. This is very similar to Dead Reckoning the entire cluster, but I also superimpose a small random error value to the heading and speed of each particle. This gives me a slightly wider spread of particles which now form the set P0 for the next iteration. By tuning the size of these random errors, I can adjust the performance of the filter.

The filter can be tuned by adjusting its parameters

The filter can be tuned by adjusting its parameters

7. The robot assigns equal weight to all the particles in the new P0 and loops back to step 3.

                                                                                                                      That’s it!!!! That’s the complete theory of the Particle Filter written in very very simple terms. Of course there are several subtle complications that I will need to deal with as the complexity of the model increases, but as this project is still a work in progress, I will not get into them here.

                                                                                                                   This video sequence shows my particle filter at work.


                                                                                                                      I used Octave to implement this filter. I chose Octave because of its ability to perform fast, efficient numerical computations on large vectors of data, and because it’s free to download and use. I have uploaded the source code for my particle filter here. This repo includes the following files:-

                                                                                                               Two_Axes_Particle_Filter.m – Main Script for this particle filter simulation

AddRandomNoise.m – Superimposes error onto a value

CreateUniformParticleDistribution.m – Needed for Step 2

WeighAndNormalise – For assigning weights to particles

DRWithError.m – For moving particles according to robot’s kinematic parameters

Distance2D.m – To determine distance between 2 points in 2D space

TrueBearing.m – To determine true bearing between 2 points in 2D space

Resample.m – Resamples particles based on their normalised weights

ResampleLargeSet.m – Fast resampler for large arrays of particles

ResampleSmallSet – Fast resampler for small arrays of particles

7 thoughts on “Robot Navigation with a Particle Filter

  1. Monitoring the developments with a lot of eagerness and I hope your enthiran one day will become a good citizen and even Vote!.

    • Thanks for reading sir. In fact I am just completing a Machine Learning course with Stanford Uni! This little robot’s algortihm is sure to get much cleverer!

  2. Pingback: Important Bayesian Lessons and the Santa Claus Question | Bayesian Adventures

  3. Pingback: Obstacle Avoidance for Clever Robots | Bayesian Adventures

Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s