Tracking a self-driving car with high precision

Chapter 6: Doing cool things with data!

Localization, or knowing “where am I” is critical for a self driving car. At every instant it needs to know where in the world it is. When we drive a car we use GPS and Maps App to know where our car is. But GPS have an accuracy of 1–3 meters, sometimes more. This won’t work for a self driving car since this is a width of a lane. The car can drift off from its lane which can be dangerous. A self driving car needs a much higher accuracy — less than 10 cm. Here we will discuss a technique called Particle Filters which can be used to help the car locate itself in a map.

I was able to implement a particle filter that could track a car with a mean x-error and y-error of less than 0.07. I have shared the link to my GitHub with the full code in C++.

Below is a video showing a particle filter in action tracking a car in a simulated environment. The circular points are the landmarks in the map, the green lines are the LASER sensors from the car to these landmarks. The blue circle is the closest particle that is tracking the car. Notice how closely the particle tracks the actual car.

Here is how Particle Filters work. To begin with we have a map of the world. The map has landmarks which could be houses, buildings, lamp posts etc. and we know the location of these landmarks within the map

  1. We initialize our map with N number of particle randomly placed around the map. Think of these particles as possible location of the car. In the beginning we don’t know where the car is in the map, so all particles are given equal weight meaning that they have equal probability of representing the true location of the car
  2. At each time step, the car moves. Our sensors give us information of the velocity and the angle in which the car is moving.We move each of the particle by the same distance and in the same direction as the car. This is called the Prediction step
  3. Now comes the Measurement step. Using LIDAR we measure the distance of the car to the known landmarks on the map.

LIDAR uses LASER for measurement and generates a point cloud of the world around it providing the car with fairly accurate position x and position y values. However LIDAR is not very accurate in poor weather conditions or if the sensor gets dirty.

Below is an image of LIDAR point cloud

LIDAR point cloud

Now comes the critical step. For each particle we calculate the distance between the particle and the known landmarks in the map. We compare how close this distance is to the LIDAR measurements from the car and use that to assign weight to the particle. Particles that are closer to the actual car will be given higher weight.

Couple of practical things to take care of when applying the LIDAR measurements:

i) Data Association — Multiple LIDAR measurement can be associated with each landmark. If that’s the case we use nearest neighbor to chose the closest LIDAR reading to a landmark as the LIDAR reading as shown in the image below

Data Association

ii) Transformation — When LIDAR measurements are taken from the car, their x and y position will be with reference to the car which is called the local coordinates of the car. These need to be converted to the global “map” coordinates by also taking into consideration the position of the car/particle

4. We re-sample particles such that particles with higher weight (and hence higher probability to be close to the true location of the car) are picked and those with lower weight are dropped.

Then we go back to the step 2 and repeat this process.

Very quickly this technique can help us localize a car.

Here is an interesting question? — How would we use this technique for a real self driving car traveling between City A and City B? Particle filters assumes we have a map of the world with known location of many landmarks. How can we determine location of hundreds of landmarks and feed those to the car?

One approach that was used by the Google car was to match the image from the car’s camera to the image from Google maps street view. This was used to generate landmarks and localize the car. Also some suppliers are creating detailed maps of the world with this information

Overall building my first Particle Filter and using it for localization was a great experience.

PS: I live in Toronto and I am looking to switch career into deep learning. If you like my post and can connect me to anyone, I will be grateful :). My email is

Other writings:


Udacity Self Driving Cars Nano Degree — I thank Udacity and Sebastian Thrun for giving me the chance to be part of their new Self Driving Car program. It has been a very interesting journey. Most of the code used by me was suggested in classroom lectures. The images and the videos references here were also shared in the lectures