Skip to content

Latest commit

 

History

History
129 lines (105 loc) · 9.39 KB

README.md

File metadata and controls

129 lines (105 loc) · 9.39 KB
Raw

Kidnapped Vehicle

C++ Udacity - Self-Driving Car NanoDegree

In this project we utilize a particle filter approach to achieve a high accuracy for the localization module on a self driving car. This project involves the Term 2 Simulator which can be downloaded here.

The localization module is a critical one in a self driving vehicle. Localization can be defined as predicting the location of a vehicle with high accuracy in the range 3-10 cm, one way to localize a vehicle is by using data from the Global Positioning System (GPS), which makes use of triangulation to predict the position of an object detected by multiple satellites. But GPS doesn't always provide high accuracy data, e.g. In case of strong GPS signal, the accuracy in location is in the range of 1-3 m, whereas for a weak GPS signal, the accuracy drops to a range of 10-50 m. Hence the use of only GPS is not reliable and desirable.

To achieve an accuracy of 3-10 cm, sensor information from Laser (LIDAR) and/or Radial distance and angle sensor (RADAR) is used and fused together using a Particle Filter.

Dependencies

Running the Code

Some scripts have been included to streamline this process, these can be leveraged by executing the following in the top directory of the project:

> ./clean.sh
> ./build.sh
> ./run.sh

Tips for setting up your environment can be found here.

The Map

map_data.txt includes the position of landmarks (in meters) on an arbitrary Cartesian coordinate system. Each row has three columns.

  1. x position
  2. y position
  3. landmark id

Goal

In this project, a vehicle is kidnapped inside a closed environment and has no idea of its location. This environment is simulated in Udacity's self driving car simulator. The vehicle travels through the environment and takes roughly 2400 steps with change in orientation and position. The goal is to predict the location of vehicle using a Particle Filter approach implemented in C++. The error between ground truth location of robot and the predicted location should be minimal. Additionally, the program must be performant enough to run within 100 seconds while maintaining minimal error.

Implementation

The directory structure of this repository is as follows:

.
├── build.sh
├── clean.sh
├── CMakeLists.txt
├── README.md
├── run.sh
├── data
    ├── map_data.txt   
├── src
    ├──  helper_functions.h
    ├──  main.cpp
    ├──  map.h
    ├──  particle_filter.cpp
    ├──  particle_filter.h

The localization module was implemented as described below:

  1. A noisy measurement from GPS sensor was received and used to initialize the position of vehicle. This measurement included the x coordinate, y coordinate (both in m) and the theta (orientation) of vehicle in radian. Noise is modelled by Gaussian distribution for all the previously described parameters. The number of particles chosen for this project was 30 and were initialized to locations taken from normal distribution with mean equal to the location received from GPS and standard deviation equal to the GPS measurement uncertainty.

  2. Global map of environment is initialized. This map is represented by a list x and y coordinates of landmarks in the environment.

  3. Once map and particles are initialized, the vehicle implements the Prediction step in which the location of each particle at next time step is predicted. This is done by using information of control inputs and time elapsed between time steps. The control inputs are nothing but magnitude of velocity (v) and yaw rate (θ). Location update is done with the help of the formulas given below:

\begin{tabular}{c|c} \widehat\theta = 0 & \widehat\theta \neq 0 \\ \hline \noalign{\vskip 2mm} \begin{aligned}[t] % placement: default is "center", options are "top" and "bottom" x_f &= x_0 + v(dt) (\cos{\theta}_0) \\ y_f &= y_0 + v(dt) (\sin{\theta}_0) \\ \theta_f &= \theta_0 \end{aligned} & \begin{aligned}[t] x_f &= x_0 + \dfrac{v}{\hat\theta}[\sin(\theta_0 + \hat\theta(dt)) - \sin(\theta_0)] \\ y_f &= y_0 + \dfrac{v}{\hat\theta}[\cos(\theta_0) - \cos(\theta_0 + \hat\theta(dt))] \\ \theta_f &= \theta_0 + \hat\theta(dt) \end{aligned} \end{tabular}

  1. After prediction step, the vehicle implements the Update step by using the information provided by the sensors (LIDAR and RADAR). However the coordinates of these measurements are relative to our vehicle and not the map, so a homogenous transformation is performed to map the vehicle coordinates to the global map ones in the following way.

\begin{bmatrix} x_{map} \\ y_{map} \\ 1 \end{bmatrix} & = \begin{bmatrix} \cos\theta & -\sin\theta & x_{particle} \\ \sin\theta & \cos\theta & y_{particle}\\ 0 & 0 & 1 \end{bmatrix} \times \begin{bmatrix} x_{vehicle} \\ y_{vehicle}\\ 1 \end{bmatrix} \\ \\ x_{map} = x_{particle} + (\cos\theta \times x_{vehicle}) - (\sin\theta \times y_{vehicle}) \\ y_{map} = y_{particle} + (\sin\theta \times x_{vehicle}) + (\cos\theta \times y_{vehicle})

Not all the particles can be in the vehicle's range at a given momment, so we retain only those that are in the range of the sensors from the particle position, and after this filtering process we map every sensor observation to a landmark using 1-NN (nearest neightbour algorithm). Finally the weight of the particle is updated using a Multivariate Gaussian Distribution for all observations associated to landmarks using the formula below (individual probabilities).

P(x,y) = \dfrac{1}{2\pi\sigma_x\sigma_y} \Bigg{e}^{-\left(\dfrac{(x-\mu_x)^2}{2\sigma_x^2} + \dfrac{(y-\mu_y)^2}{2\sigma_y^2}\right)}

  1. The last step is resampling the particles with replacement using their normalized weight as probabilities of appearance causing removal of the unlikely ones and more presence of the likely ones.