Kidnapped Vehicle: A Particle Filter Demo for Vehicle Localization

A 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 I implement a 2 dimensional particle filter in C++ to dynamically localize the robot. A map and some initial localization information are given (analogous to what a GPS would provide). At each time step the filter will also get observation and control data.

This project is my solution to Udacity term 2, assignment 3. src/particle_filter.cpp contains the implementation and src/main.cpp governs the requirements on accuracy and run time as per project assignment.

Output

At each time step, the likelihood of each n particle is estimated based on observed distance (Lidar) and calculated distance to landmarks on the map (black circles). Importance sampling is applied to filter those particles which are most likely, representing the position of the vehicle on the map. Error rates are calculated at each time step. The Simulator interface output looks like below，with n = 400 in the example.

If the filter localizes the object and detect its yaw within the error range (values specified in the parameters max_translation_error and max_yaw_error in src/main.cpp) within the time of 100 seconds, the Simulator will output the message:

Success! Your particle filter passed!

Particle Filter Implementation Notes

The algorithm processing follows the following steps

Below is the pseudocode of the Particle Filter implementation

---

Dependencies

• Udacity Term 2 Simulator
• uWebSocketIO. Run install-mac.sh or install-ubuntu.sh to set up and install for either Linux or Mac systems. For windows you can use either Docker, VMware, or even Windows 10 Bash on Ubuntu to install uWebSocketIO.

Running the Code

Once the installation for uWebSocketIO and the simulator is complete, the main program can be built and ran from the project top directory.

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

---

Data Requirement

Data consists of the following

• The map (data provided by 3D Mapping Solutions GmbH): 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, and (3) landmark id
• Observations and controls provided by Udacity simulator

Data Flow Between the Filter and the Simulator

Once the Simulator is installed. The main protocol that main.cpp uses for uWebSocketIO in communicating with the simulator is the following

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 and for respective (x,y) sensed positions

  • ["best_particle_associations"]
  • ["best_particle_sense_x"] <= list of sensed x positions
  • ["best_particle_sense_y"] <= list of sensed y positions

---

References

• Coordinate transformation introduction
• Coordinate transformation formula