Kidnapped vehicle

Introduction

Our self-driving car has been kidnapped and transported to a new location! Luckily it has a map of this location, a (noisy) GPS estimate of its initial location, and lots of (noisy) sensor and control data.

This project implements a 2 dimensional particle filter in C++. The particle filter is given a map and some initial localization information (analogous to what a GPS would provide). At each time step the filter also gets observation and control data.

The project's rubric can be found here

The simulation video on youtube

Particle filter flowchart

The following flowchart summarizes the particle filter algorithm to achieve the vehicle's localization.

Image source: Udacity

The filter predicts the location of the self driving car from the velocity, yaw rate and time elapsed using the following equations:

The observations of the self driving car sensors are taken in the vehicle's coordinate system. In order to correlate the sensors observations with the actual landmarks of the map, we have to convert the coordinates of the observations from vehicle coordinates to map coordinates. This is performed with the homogeneous transformation using the following equations:

The particles final weight is calculated as the product of each observations Multivariant-Gaussian probability.

The weights are updated for each particle using the following equation:

The mean of the Multivariate-Gaussian equation is the observation's associated landmark position and is evaluated at the point of the transformed observation's position.

Installation

This project involves the Term 2 Simulator which can be downloaded here

This repository includes two files that can be used to set up and intall uWebSocketIO for either Linux or Mac systems. For windows you can use either Docker, VMware, or even Windows 10 Bash on Ubuntu to install uWebSocketIO.

Important Dependencies

cmake >= 3.5
- All OSes: click here for installation instructions
make >= 4.1 (Linux, Mac), 3.81 (Windows)
- Linux: make is installed by default on most Linux distros
- Mac: install Xcode command line tools to get make
- Windows: Click here for installation instructions
gcc/g++ >= 5.4
- Linux: gcc / g++ is installed by default on most Linux distros
- Mac: same deal as make - install Xcode command line tools
- Windows: recommend using MinGW

Basic Build Instructions

Once the install for uWebSocketIO is complete, the main program can be built and ran by doing the following from the project top directory.

Clone this repo
Make a build directory: mkdir build && cd build
Compile: cmake .. && make

On windows, you may need to run: cmake .. -G "Unix Makefiles" && make

Run it: ./particle_filter

$ mkdir build && cd build
$ cmake .. && make
$ ./particle_filter

Tips for setting up your environment can be found here

Data flow between the program and the Simulator

Here is the main protcol that main.cpp uses for uWebSocketIO in communicating with the simulator.

INPUT: values provided by the simulator to the c++ program

// sense noisy position data from the simulator

["sense_x"]

["sense_y"]

["sense_theta"]

// get the previous velocity and yaw rate to predict the particle's transitioned state

["previous_velocity"]

["previous_yawrate"]

// receive noisy observation data from the simulator, in a respective list of x/y values

["sense_observations_x"]

["sense_observations_y"]

OUTPUT: values provided by the c++ program to the simulator

// best particle values used for calculating the error evaluation

["best_particle_x"]

["best_particle_y"]

["best_particle_theta"]

//Optional message data used for debugging particle's sensing and associations

// for respective (x,y) sensed positions ID label

["best_particle_associations"]

// for respective (x,y) sensed positions

["best_particle_sense_x"] <= list of sensed x positions

["best_particle_sense_y"] <= list of sensed y positions

itornaza/sdc-kidnapped-vehicle