The robot 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.
In this project we implement a two dimensional particle filter in C++ to localize a kidnapped vehicle. The particle filter is given a map and some initial localization information (analogous to what a GPS does). At each time step the filter gets noisy observation and control data.
- cmake >= 3.5
- make >= 4.1 (Linux, Mac), 3.81 (Windows)
- gcc/g++ >= 5.4
- Udacity Term 2 Simulator
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 install 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.
Once the install for uWebSocketIO is complete, the main program can be built and ran by doing the following from the project top directory.
- mkdir build
- cd build
- cmake ..
- make
- ./particle_filter
Alternatively 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 instructions are from here.
- Download the (.deb) package of Unity (3D) version 5.5.1f1 that the Udacity Simulator uses.
- Install dependencies :
sudo apt install gconf-service lib32gcc1 lib32stdc++6 libc6-i386 libgconf-2-4 npm - Run the install :
sudo dpkg -i ~/Downloads/unity-editor_amd64-5.5.1xf1Linux.deb - If you get error about unmet dependencies you may need to run, and retry
sudo apt --fix-broken install - With Unity working now, download and run the latest release of the Udacity Term 2 Simulator.
I used VSCode IDE for this project. Follow the thorough instructions provided by yosoufe to setup the environment. The instructions cover debugging as well which may be useful.
The particle filter is implemented in particle_filter.cpp and particle_filter.h.
The program main.cpp has already been filled out, but feel free to modify it.
Below is how 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
I don't touch main.cpp
The particle filter is implemented in src/particle_filter.cpp -
- Initializtion in ParticleFilter::init from line 28 to 63.
- Prediction in ParticleFilter::prediction from line 65 to 99.
- Data Association in ParticleFilter::dataAssociation from line 101 to 144.
- Update Weights in ParticleFilter::updateWeights from line 146 to 232.
- Resample in ParticleFilter::resample from line 234 to 274.
The rest of the code is untouched.
I ran the particle filter with 100 particles and it passed. Below screenshot shows the last frame from the simulator output.
| Parameter | x | y | yaw |
|---|---|---|---|
| Error | 0.109 | 0.092 | 0.004 |
The simulation output video is here
In addition to running the filter with 100 particles I also ran it with 1 particle (simultor output) and 3 particles (simultor output).
Of course the result was poor with 1 particle:
| Parameter | x | y | yaw |
|---|---|---|---|
| Error | 8.525 | 8.097 | 0.083 |
Surprisingly to me the result is already much better with 3 particles:
| Parameter | x | y | yaw |
|---|---|---|---|
| Error | 0.262 | 0.262 | 0.010 |
Screenshot of the last frame for both the simulations is shown below.
| 1 particle | 3 particles |
|---|---|
 |
 |
The below figures compare the position estimate, R (distance from origin) and Yaw for the three cases.
| Position Estimate | R (distance from origin) | Yaw |
|---|---|---|
 |
 |
 |