Monte Carlo Localisation

In MCL, a cloud of weighted particles represents the uncertain position of a robot. We can think of this as a:

• Bayesian probablistic filter.
• Genetic Algorithm (survival of the fittest).

1. Particles

A particle is a point estimate of the state of a robot with a weight , . The full particle set is . The distribution is noramlized if all weights add to 1: .

2. Localisation Algorithm

• Continuous Localisation is a tracking problem - given a good estimate of where the robot was at the last time-step, and some new measurements, estimate where it is now. Here, the initial position is known, so we start with and .
• Global Localisation (kidnapped robot problem): the environment is known, but the robot's position ins completely uncertain. Here the initial particles are sampled randomly from all the positions, so and .

If we need to, we can make a point estimate of the robot's position from the particles, e.g. the weighted mean: . So, for MCL:

1. Motion is predicted based on odometry.
2. Measurements are updated based on outward looking sensors.
3. Normalization.
4. Resampling.

2.1 Measurements Update

A measuerement update consists of applying Bayes Rule to each particle: (as is a constant later removed by normalization) where is the likelihood of the measurement given the particle state .

A likelihood function fully describes the sensors perforamnce, determined by repeated experiments with the sensor where is the measurement and is the ground truth.

2.1.1 Sonar Update

If the robot is at then its forward distance to an infinite wall passing through and is given by:

Then the world coordinates at which the forward vector from the robot would meet the wall a re:

2.1.2 Sonar Update Likelihood

The likelihood of a sonar update depends on the difference where is the sonar measurement and is the expected measurement from the particle. A common model is a Gaussian:

Where is the standard deviation of the sonar noise.

A robust likelihood function takes into account that real sensors sometimes report garbage values values which are not close to the ground truth:

The effect of this is that the filter is less aggressive in killing off particles which are far from aggreing with measurements.

2.1.3 Sonar Angle Correction

Possibly relevant for real sensors may be the angle between sonar direction and the normal to the wall. If is too large, sonar will not give a sensible reading:

2.2 Normalization

The weights of all particles are scaled again so that they add to :

2.3 Resampling

Resampling generates a new set of particles which all have equal weights , but whose spatial distribution now reflects the probability density.

Done by generating the cumulative probability distribution, generating a uniformly distributed random number between and and picking the particle whose cumulative probability this intersects.