Robot Localization

Establishing a particular position in the world (Localization) is a problem solved by Global position systems (GPS). However, GPS accuracy for autonomous vehicles does not provide the right information at the right time. The accuracy and response time are not sufficiently precise, the distance is given in meters not centimeters of error, as well as environmental and contextual conditions such as rain, clouds, or nearby buildings increase the noise[1]. Navigation for mobile robots requires to solve four challenges: Environmental perception, localization, path planning(cognition), and vehicle or motion control[2].

Udacity - Self-Driving Car NanoDegree

particles

Image by Tomislav Jakupec from Pixabay

Solution

On this project[3], we have implemented the Particle Filter to solve the localization problem.

There are different approaches to solve these issues, based on Lidar, Radar, and/or Video, that support in providing the right accuracy. The drawback is that they require more computing resources on the vehicle. Most know methods use Bayesian filters, such as Kalman or Particle Filters[1].

particles simulation

Algorithm

The particle filter algorithm occurs in four (4) steps:

  1. Initialize all the particles using GPS information, considering the noise
  2. Given the yaw rate, time change, and velocity predict the position of all particles
  3. Provided the observed points and the map update the belief(weight) of all the particles with the landmarks close to our actual position
  4. Finally, resample the weight distribution of the particles

In the following [4], you can find a deep explanation of the algorithm.

Implementation

Our implementation[3] receives a map of the vehicle information from the simulator and provides the best particle belief found. It is also supplied the ground truth to calculate the RMSE to be aware of the error from the calculations.

Giving the math and probability calculation explanation, we did implement the code to find a solution that worked on the simulator. It is a very good exercise to have implemented in C++ to understand some of the peculiarities that one has when developing such solutions.

References

[1] Robotic Autonomy course Carnegie Mellon, Chapter 5 http://www.cs.cmu.edu/~rasc/Download/AMRobots5.pdf

[2] A Particle Filter Localization Method Using 2D Laser Sensor Measurements and Road Features for Autonomous Vehicle http://downloads.hindawi.com/journals/jat/2019/3680181.pdf

[3] Particle Filter for localization Implementation https://github.com/hurtadosanti/ParticleFilterLocalization

[4] Particle Filters Video https://www.youtube.com/watch?v=lzN18y_z6HQ