Self-Driving Car Engineer Diary — 9

Term 2 project 3 : particle filter localisation and mapping in C++

Sun, 4/Jun/2017

Leading up to project 3 we covered a lot of material. Here are the highlights that I found most interesting.

Localisation

1. Initial belief of cars location aka a rough guess using GPS and provided map containing landmarks.
2. Sense obstacles using noisy sensor input from radar and lidar (robot gains information) then apply 1 of many available Bayes Filters.
3. Car / obstacles move (robot loses information), so repeat step 2 above.

Motion, Errors and Map Frame Of Reference

Using the Bicycle Motion Model we focus on the yaw rate (rotation of the car about the z-axis) and didn’t worry about pitch (except if you drive in very hilly areas) and ignore roll (unless you are a stunt driver).

Errors creep into the motion model due to wet / slippery conditions and bumpy roads but not from twisting roads.

Particle Filter Approach

Particle filter is a sequence of algorithms used to estimate the state of a system. Relatively simple to program and are used to solve continuous value localisation problems in spaces up to 4 dimensions.

With the initial particle blast (red dots below) representing the many possible locations (posterior belief of x, y and heading) of the robot, it’s time to take a measurement then apply a weighted filter, rewarding the particles that represent the best guess of the robot’s ground-truth location.

Robot sonar sensing objects (blue rays) and initial ‘shot gun’ of particle guesses (red dots) possible location on floor map.

Particle Filter Implementation

1. Initialisation : initialise system with 100 particle robot ‘clones’, sampling from a Gaussian distribution around the initial GPS position estimate.
2. Prediction : assuming the Bicycle Motion Model, now predict where the car will be at the next time step, updating each particles location based on velocity and yaw rate measurements (adding Gaussian noise to account for the uncertainty in the control input).
3. Link Measurements With Landmarks : using Nearest Neighbour approach, we will solve the data association problem of matching “Feature Measurements” (given in car co-ordinates) to objects in the real world (“Map Landmark Positions”, aka in map co-ordinates).
4. Update Weights : These measurements form the weight of each particle by applying the multi-variate gaussian probability density function. This function tells us how likely a set of landmark measurements is given, our predicted state of our car. See this awesome video on applying trigonometry for transforming from 1 co-ordinate system to another.
5. Resample : Finally, we resample the particles with the probability proportional to these weights.