Tracking a self-driving car with Particle Filter

GPS alone is not accurate enough for self-driving car. According to US government, GPS-enabled smartphones have an accuracy within 5 meters under an open sky, and is noisy in urban environments when signals are bouncing off buildings. In this article, we discuss a landmark-based localization using Particle Filter to remove noise. Localization is about finding the location of moving autonomous cars precisely. This can be helpful in trajectory planning in autonomous driving.

Image for post
Image for post
Ralf Herrtwich “The S500 Intellegient drive”

A LiDAR mapping van prepares a map ahead of time with landmarks. Landmarks can be pole-like structures which are less likely to be moved or blocked, or lanes marked on the street.

Image for post
Image for post
Source

Here is a slide from Daimler on vehicle localization.

Image for post
Image for post
Ralf Herrtwich “The S500 Intellegient drive”

In our example, we use the sensors and cameras on the autonomous car to measure the distance and orientation information between the car and the landmarks. Here, we have a couple challenges. First, landmarks may be out of sight or temporarily blocked. Second, the signal is noisy. In the simulator below, we use Particle filter to filter out the noise from our measurement to predict the car location. We also assume that the measurement does not identify the landmark itself. We need to cross reference the distance information with the map to identify the landmark. In the video below, we trace the location of the car using such measurements and the identified landmarks.

This is part of a 5-series self-driving. Other articles includes

Initial position

For discussion, our car moves in one direction only. We use the GPS to locate the initial position of the car. Since GPS is not accurate, we sample 10 locations (particles) based on the measurement noise specification provided by the GPS vendor. The number of particles used is tunable and we select an unusually low number just for easy visualization. In real-life, we can start with hundreds to thousands of particles and use experiments to find the proper number that achieve a good balance between accuracy and the computational cost.

Image for post
Image for post

If GPS is not available, we can distribute the particles uniformly. This is the same as saying we do not know where the car is.

Image for post
Image for post

For each particle, we apply a dynamic model (like x’ = x + v 𝛿t) to calculate the next position at time t + 𝛿t.

Image for post
Image for post

To simulate the process noise caused by factors like weather and the road conditions, we add independent random noise to each particle’s location. The magnitude of the noise is determined by experimental data or an analytical model. As expected, those particles should spread further apart because of those uncertainty factors.

Image for post
Image for post

With a pre-determined map, we find all the surrounding landmarks.

Image for post
Image for post

But we remove all landmarks that are considered out-of-sight from our sensors (the left-most stop sign).

Image for post
Image for post

Each particle represents the suggested location of the car. We want to compute the likeliness using measurements from our sensors (like camera and LiDAR). Each reading contains a distance and an orientation from a landmark. But, as mentioned before, the measurement does not identify the landmark itself. Instead, we use the distance and orientation information to locate the closest landmark using a map. For example, in P2, we pick the right-most stop sign using our first sensor reading.

Image for post
Image for post

We assume the measurement noise is Gaussian distributed, so the probability for P2 representing our current car location is:

Image for post
Image for post
σ will be provided by the sensor vendor
Image for post
Image for post

We repeat the calculation (PDF) for every new measurement we received. Then the final likelihood for a particle is computed by multiplying all the corresponding PDFs together.

Image for post
Image for post

We repeat the process for every particle. Then we normalize their probabilities such that the total probability is equal to one. This acts as the weight of our particles. The particle with the largest weight is where we estimate our car is.

Image for post
Image for post

We resample another 10 particles based on the weights. For example, P2 will double the chance of being picked if it has twice the weight of P1. In our cases, we have 2 clusters of locations. This indicates our measurements show 2 possible locations for our car. This kind of ambiguity can be reduced by increasing the number of landmarks in the map or reduce the time (𝛿t) between predictions.

Image for post
Image for post

For each particle, we apply our car’s dynamic model again for the next particle position and then add random process noise.

Image for post
Image for post

Then we repeat the process again with a new set of measurements. This time, our sensor measurements are less ambiguous. Particles from the first cluster are eliminated with the new 10 particles cluster closely together now.

Image for post
Image for post

Conclusion

Localization using Particle filter and landmarks helps us to locate the self-driving precisely. We combine what we believe our car is with noisy measurements. Even both may not be accurate but they are not coordinated to make the same type of mistakes. Hence, by combining these information together, we paint a much accurate picture on our car location.

Credit

We use a simulator provided by the Udacity self-driving car class to demonstrate our Particle filter code.

Written by

Deep Learning

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store