In this project we try to locate a moving vehicle using particle filter technique. We have noisy sensor data as inputs and initial GPS location. Velocity and yaw rate are control data that are available to help make vehicles location in the next instant of time. Using a discrete distribution of weighted probability of the particles we locate the vehicle
Note: When there are multiple Lidar measurements then we take the Lidar measurement that is closest to the map landmark.
This is a method for localization. Which means finding oneself where we are in the world. It is named as particle filter because the technique we use creates a number of particles that are nothing but the guesses we have made about ourself over the question of where we are. The filter term is used due to the fact that we keep progressively reduce/narrow our guess everytime.
- Initialize few number of particles based on a coarse GPS signal. The informations we receive from GPS intially are the co-ordinates and orientation. Each of these measurements has a noise that follows a normal distribution with a mean of zero and a standard deviation that depends on the device. We pick up these noises and addup to our co-ordinate. We also initialize the weights of these particles equally (say 1.0). These weights represents the fact that our confidence in position at this time step is uniformly distributed (probability).
- Everytime we receive the values of measurements that are based from our controls (velocity and yaw rate) we try to predict the position of the vehicle in the next time step using the mechanics equations. There is an element of incorrectness due to friction and slippage
- Update the weights by getting the measurements from sensors. These are noisy data that mi 8000 ght be coming from a Kalman Filter pipeline. These sensors could be a LIDAR or a RADAR measurement. We try to find the landmarks that are detected by the sensors projected onto a map co-ordinate system. We do this because the measurements that are received are from the perspective of he vehicle (vehicle back-front is x and right-left is y). Once transformed we compute the closest lying landmark as defined in the map data. The closeness of the landmark with the observation and map defines the probability distribution of the weights (location probability). It is the conditional probability of weights of the particle by the multi-variable Gaussian distribution of landmarks and observations closeness.
- Resampling is done on particles with replacement at every time step. This is done as a cycle(wheel) selected by the discrete distribution driven by the weights of the particles. This makes sure that particles with higher weights appear more frequently in the resampled distribution.
Using 15 particles I managed to get a descent run-time and accuracy. While trying out with the number of particles I once saw the vehicle passing the necessary criteria even with 10 particles. The number of particles thus becomes a hyper parameter for our vehicle detection model
Equation Source
Theory about transformation
This project requires a 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.
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
Note that the programs that need to be written to accomplish the project are src/particle_filter.cpp, and particle_filter.h
The program main.cpp has already been filled out, but feel free to modify it.
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
The directory structure of this repository is as follows:
root
| 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
You can find the inputs to the particle filter in the data directory.
map_data.txt includes the position of landmarks (in meters) on an arbitrary Cartesian coordinate system. Each row has three columns
- x position
- y position
- landmark id
- Map data provided by 3D Mapping Solutions GmbH.
- Download the contents from here to a folder named
ide_profilesfrom your project root directory - Rename the files as per your project name
- Update the
install-windows.batas per your project name - Edit
projectname.sln,projectname.vcxproj, ,projectname.filtersby renaming with your project name - Update the
headerandcppfile references in the above two files - Run the
install-windows.batfile - Open the
projectname.slnand start coding - You could check if your external dependencies are correctly loaded to verify if you did all these steps right
- Can improve the performance of the detection model further
- Dynamic particle size ?