Monte Carlo localization (MCL) is also known as particle filter localization. Given a map of an environment, the algorithm estimates the position and orientation of a robot as it moves and senses the surrounding. The algorithm uses particle to represent the distribution of likely states, with each particle representing a possible state.
The algorithm starts by distributing the particles in the configuration space. After that, it starts to loop its three main parts, namely motion update, sensor update, and resampling. During motion updates, particles are shifted into new possibilities of states. Whenever the robot senses the environment, the particles are resampled using the recursive Bayesian estimation. Lastly, the particles with higher likelihood tend to survive in the resampling process.
I coded an example of the MCL algorithm in C++ during studying Robotic Software Engineer Nanodegree Program provided by Udacity. Through this relatively simple algorithm, one can find a robot position relatively fast. Find the animation below.
The green dots represent the before particle while the yellow dots represent the particle after resampling. Here, the red dots act as landmarks and the robot “measures” its distance to each of the landmark.
Even though the implementation of this algorithm is relatively easy, one needs to check the convergence. The particle will converge if the environment is distinguishable, allowing a higher belief of around a certain position and orientation. For the animation above, on a simulated 100×100 m2 area, below is the convergence test.
The resulting average distance, can be considered as error, is less than 1.5 meter. This value can be increased by having a more distinctive environment or higher number of particles, of course with its trade off.
After exploring this MCL, I created a project to localize my robot using ROS and Gazebo. I used Adaptive Monte Carlo Localization (AMCL) provided by ROS to localize the robot and move it around a known environment using keyboard teleop command. Please find the project repository here and check the short video below.