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.
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.
Here is a slide from Daimler on vehicle localization.
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
- Self-driving perception: Sensor fusion with Kalman Filter.
- Self-driving perception: Extended Kalman Filter and Unscented Kalman Filter.
- Self-driving localization: Localization with Particle Filter.
- Self-driving control: Control with Model Predictive Control & PID.
- Self-driving Path finding.
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.
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.
For each particle, we apply a dynamic model (like x’ = x + v 𝛿t) to calculate the next position at time t + 𝛿t.
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.
With a pre-determined map, we find all the surrounding landmarks.
But we remove all landmarks that are considered out-of-sight from our sensors (the left-most stop sign).
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.
We assume the measurement noise is Gaussian distributed, so the probability for P2 representing our current car location is:
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.
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.
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.
For each particle, we apply our car’s dynamic model again for the next particle position and then add random process noise.
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.
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.
