├── gif.gif ├── video.mp4 ├── run.sh ├── install-ubuntu.sh ├── clean.sh ├── src ├── map.h ├── particle_filter.h ├── main.cpp ├── helper_functions.h └── particle_filter.cpp ├── install-mac.sh ├── CMakeLists.txt ├── data └── map_data.txt ├── LICENSE ├── cmakepatch.txt └── README.md /gif.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/A7med01/CarND-Kidnapped-Vehicle/master/gif.gif -------------------------------------------------------------------------------- /video.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/A7med01/CarND-Kidnapped-Vehicle/master/video.mp4 -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to run particle filter! 3 | # 4 | # Written by Tiffany Huang, 12/14/2016 5 | # 6 | 7 | # Run particle filter 8 | cd ./build 9 | ./particle_filter 10 | -------------------------------------------------------------------------------- /install-ubuntu.sh: -------------------------------------------------------------------------------- 1 | sudo apt-get install libuv1-dev libssl-dev 2 | git clone https://github.com/uWebSockets/uWebSockets 3 | cd uWebSockets 4 | git checkout e94b6e1 5 | mkdir build 6 | cd build 7 | cmake .. 8 | make 9 | sudo make install 10 | cd .. 11 | cd .. 12 | sudo ln -s /usr/lib64/libuWS.so /usr/lib/libuWS.so 13 | sudo rm -r uWebSockets -------------------------------------------------------------------------------- /clean.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to clean the tree from all compiled files. 3 | # You can rebuild them afterwards using "build.sh". 4 | # 5 | # Written by Tiffany Huang, 12/14/2016 6 | # 7 | 8 | # Remove the dedicated output directories 9 | cd `dirname $0` 10 | 11 | rm -rf build 12 | 13 | # We're done! 14 | echo Cleaned up the project! 15 | -------------------------------------------------------------------------------- /src/map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * map.h 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: mufferm 6 | */ 7 | 8 | #ifndef MAP_H_ 9 | #define MAP_H_ 10 | 11 | class Map { 12 | public: 13 | 14 | struct single_landmark_s{ 15 | 16 | int id_i ; // Landmark ID 17 | float x_f; // Landmark x-position in the map (global coordinates) 18 | float y_f; // Landmark y-position in the map (global coordinates) 19 | }; 20 | 21 | std::vector landmark_list ; // List of landmarks in the map 22 | 23 | }; 24 | 25 | 26 | 27 | #endif /* MAP_H_ */ 28 | -------------------------------------------------------------------------------- /install-mac.sh: -------------------------------------------------------------------------------- 1 | brew install openssl libuv cmake 2 | git clone https://github.com/uWebSockets/uWebSockets 3 | cd uWebSockets 4 | git checkout e94b6e1 5 | patch CMakeLists.txt < ../cmakepatch.txt 6 | mkdir build 7 | export PKG_CONFIG_PATH=/usr/local/opt/openssl/lib/pkgconfig 8 | cd build 9 | OPENSSL_VERSION=`openssl version -v | cut -d' ' -f2` 10 | cmake -DOPENSSL_ROOT_DIR=$(brew --cellar openssl)/$OPENSSL_VERSION -DOPENSSL_LIBRARIES=$(brew --cellar openssl)/$OPENSSL_VERSION/lib .. 11 | make 12 | sudo make install 13 | cd .. 14 | cd .. 15 | sudo rm -r uWebSockets 16 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(PARTICLE_FILTER) 2 | 3 | cmake_minimum_required (VERSION 3.5) 4 | 5 | add_definitions(-std=c++11) 6 | 7 | set(CXX_FLAGS "-Wall") 8 | set(CMAKE_CXX_FLAGS "${CXX_FLAGS}") 9 | 10 | set(sources src/particle_filter.cpp src/main.cpp) 11 | 12 | 13 | if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 14 | 15 | include_directories(/usr/local/include) 16 | include_directories(/usr/local/opt/openssl/include) 17 | link_directories(/usr/local/lib) 18 | link_directories(/usr/local/opt/openssl/lib) 19 | link_directories(/usr/local/Cellar/libuv/1*/lib) 20 | 21 | endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 22 | 23 | 24 | add_executable(particle_filter ${sources}) 25 | 26 | 27 | target_link_libraries(particle_filter z ssl uv uWS) 28 | 29 | -------------------------------------------------------------------------------- /data/map_data.txt: -------------------------------------------------------------------------------- 1 | 92.064 -34.777 1 2 | 61.109 -47.132 2 3 | 17.42 -4.5993 3 4 | -7.1285 -34.54 4 5 | 232.32 32.032 5 6 | 177.43 28.083 6 7 | 286.89 18.159 7 8 | 274.37 31.197 8 9 | 47.484 -36.786 9 10 | 69.2 -78.217 10 11 | 124.61 24.277 11 12 | 36.203 14.827 12 13 | -39.786 -12.615 13 14 | -38.025 -99.976 14 15 | 267.33 -14.272 15 16 | 28.898 -39.754 16 17 | -29.836 -23.277 17 18 | 255.67 9.8137 18 19 | 13.452 -72.827 19 20 | 102.04 5.1777 20 21 | 62.838 1.9057 21 22 | -14.947 -61.919 22 23 | 15.162 -97.037 23 24 | 36.626 -28.898 24 25 | 172.16 -15.217 25 26 | 136.95 -14.13 26 27 | -41.714 -61.328 27 28 | 39.556 -47.361 28 29 | 195.65 8.6677 29 30 | 278 13.181 30 31 | 151.03 8.9127 31 32 | 8.7638 7.5647 32 33 | 83.006 20.959 33 34 | 205.39 29.686 34 35 | 264.51 24.454 35 36 | 214.54 -7.8711 36 37 | 53.27 -55.233 37 38 | 20.139 -20.15 38 39 | 8.2018 -20.97 39 40 | -13.641 -0.098341 40 41 | 278.92 21.918 41 42 | 170.62 28.733 42 43 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017-2018 Udacity, Inc. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cmakepatch.txt: -------------------------------------------------------------------------------- 1 | --- CMakeLists.txt 2017-03-23 20:58:47.000000000 +0900 2 | +++ CMakeListsnew.txt 2017-03-23 20:57:33.000000000 +0900 3 | @@ -32,10 +32,16 @@ 4 | target_link_libraries (uWS LINK_PUBLIC ${OPENSSL_CRYPTO_LIBRARY}) 5 | target_link_libraries (uWS LINK_PUBLIC ${ZLIB_LIBRARY}) 6 | 7 | -if (UNIX) 8 | +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 9 | target_link_libraries (uWS LINK_PUBLIC pthread) 10 | install (TARGETS uWS DESTINATION /usr/lib64) 11 | install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/include/uWS) 12 | -endif (UNIX) 13 | +endif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 14 | + 15 | +if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 16 | +target_link_libraries (uWS LINK_PUBLIC pthread) 17 | +install (TARGETS uWS DESTINATION /usr/local/lib) 18 | +install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/local/include/uWS) 19 | +endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 20 | 21 | add_subdirectory(examples) 22 | -------------------------------------------------------------------------------- /src/particle_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * particle_filter.h 3 | * 4 | * 2D particle filter class. 5 | * Created on: Dec 12, 2016 6 | * Author: Tiffany Huang 7 | */ 8 | 9 | #ifndef PARTICLE_FILTER_H_ 10 | #define PARTICLE_FILTER_H_ 11 | 12 | #include "helper_functions.h" 13 | 14 | struct Particle { 15 | 16 | int id; 17 | double x; 18 | double y; 19 | double theta; 20 | double weight; 21 | std::vector associations; 22 | std::vector sense_x; 23 | std::vector sense_y; 24 | }; 25 | 26 | 27 | 28 | class ParticleFilter { 29 | 30 | // Number of particles to draw 31 | int num_particles; 32 | 33 | 34 | 35 | // Flag, if filter is initialized 36 | bool is_initialized; 37 | 38 | // Vector of weights of all particles 39 | std::vector weights; 40 | 41 | public: 42 | 43 | // Set of current particles 44 | std::vector particles; 45 | 46 | // Constructor 47 | // @param num_particles Number of particles 48 | ParticleFilter() : num_particles(0), is_initialized(false) {} 49 | 50 | // Destructor 51 | ~ParticleFilter() {} 52 | 53 | /** 54 | * init Initializes particle filter by initializing particles to Gaussian 55 | * distribution around first position and all the weights to 1. 56 | * @param x Initial x position [m] (simulated estimate from GPS) 57 | * @param y Initial y position [m] 58 | * @param theta Initial orientation [rad] 59 | * @param std[] Array of dimension 3 [standard deviation of x [m], standard deviation of y [m] 60 | * standard deviation of yaw [rad]] 61 | */ 62 | void init(double x, double y, double theta, double std[]); 63 | 64 | /** 65 | * prediction Predicts the state for the next time step 66 | * using the process model. 67 | * @param delta_t Time between time step t and t+1 in measurements [s] 68 | * @param std_pos[] Array of dimension 3 [standard deviation of x [m], standard deviation of y [m] 69 | * standard deviation of yaw [rad]] 70 | * @param velocity Velocity of car from t to t+1 [m/s] 71 | * @param yaw_rate Yaw rate of car from t to t+1 [rad/s] 72 | */ 73 | void prediction(double delta_t, double std_pos[], double velocity, double yaw_rate); 74 | 75 | /** 76 | * dataAssociation Finds which observations correspond to which landmarks (likely by using 77 | * a nearest-neighbors data association). 78 | * @param predicted Vector of predicted landmark observations 79 | * @param observations Vector of landmark observations 80 | */ 81 | void dataAssociation(std::vector predicted, std::vector& observations); 82 | 83 | /** 84 | * updateWeights Updates the weights for each particle based on the likelihood of the 85 | * observed measurements. 86 | * @param sensor_range Range [m] of sensor 87 | * @param std_landmark[] Array of dimension 2 [Landmark measurement uncertainty [x [m], y [m]]] 88 | * @param observations Vector of landmark observations 89 | * @param map Map class containing map landmarks 90 | */ 91 | void updateWeights(double sensor_range, double std_landmark[], const std::vector &observations, 92 | const Map &map_landmarks); 93 | 94 | /** 95 | * resample Resamples from the updated set of particles to form 96 | * the new set of particles. 97 | */ 98 | void resample(); 99 | 100 | /* 101 | * Set a particles list of associations, along with the associations calculated world x,y coordinates 102 | * This can be a very useful debugging tool to make sure transformations are correct and assocations correctly connected 103 | */ 104 | Particle SetAssociations(Particle& particle, const std::vector& associations, 105 | const std::vector& sense_x, const std::vector& sense_y); 106 | 107 | 108 | std::string getAssociations(Particle best); 109 | std::string getSenseX(Particle best); 110 | std::string getSenseY(Particle best); 111 | 112 | /** 113 | * initialized Returns whether particle filter is initialized yet or not. 114 | */ 115 | const bool initialized() const { 116 | return is_initialized; 117 | } 118 | }; 119 | 120 | 121 | 122 | #endif /* PARTICLE_FILTER_H_ */ 123 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kidnapped Vehicle project 2 | [![Udacity - Self-Driving Car NanoDegree](https://s3.amazonaws.com/udacity-sdc/github/shield-carnd.svg)](http://www.udacity.com/drive) 3 | 4 | ![GIF](./gif.gif) 5 | 6 | # Overview 7 | This repository contains all the code needed to complete the final project for the Localization course in Udacity's Self-Driving Car Nanodegree. 8 | 9 | #### Submission 10 | All you will submit is your completed version of `particle_filter.cpp`, which is located in the `src` directory. You should probably do a `git pull` before submitting to verify that your project passes the most up-to-date version of the grading code (there are some parameters in `src/main.cpp` which govern the requirements on accuracy and run time.) 11 | 12 | ## Project Introduction 13 | Your 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. 14 | 15 | In this project you will implement a 2 dimensional particle filter in C++. Your particle filter will be given a map and some initial localization information (analogous to what a GPS would provide). At each time step your filter will also get observation and control data. 16 | 17 | ## Running the Code 18 | This project involves the Term 2 Simulator which can be downloaded [here](https://github.com/udacity/self-driving-car-sim/releases) 19 | 20 | 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. 21 | 22 | Once the install for uWebSocketIO is complete, the main program can be built and ran by doing the following from the project top directory. 23 | 24 | 1. mkdir build 25 | 2. cd build 26 | 3. cmake .. 27 | 4. make 28 | 5. ./particle_filter 29 | 30 | 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: 31 | 32 | 1. ./clean.sh 33 | 2. ./build.sh 34 | 3. ./run.sh 35 | 36 | Tips for setting up your environment can be found [here](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/23d376c7-0195-4276-bdf0-e02f1f3c665d) 37 | 38 | Note that the programs that need to be written to accomplish the project are src/particle_filter.cpp, and particle_filter.h 39 | 40 | The program main.cpp has already been filled out, but feel free to modify it. 41 | 42 | Here is the main protocol that main.cpp uses for uWebSocketIO in communicating with the simulator. 43 | 44 | INPUT: values provided by the simulator to the c++ program 45 | 46 | // sense noisy position data from the simulator 47 | 48 | ["sense_x"] 49 | 50 | ["sense_y"] 51 | 52 | ["sense_theta"] 53 | 54 | // get the previous velocity and yaw rate to predict the particle's transitioned state 55 | 56 | ["previous_velocity"] 57 | 58 | ["previous_yawrate"] 59 | 60 | // receive noisy observation data from the simulator, in a respective list of x/y values 61 | 62 | ["sense_observations_x"] 63 | 64 | ["sense_observations_y"] 65 | 66 | 67 | OUTPUT: values provided by the c++ program to the simulator 68 | 69 | // best particle values used for calculating the error evaluation 70 | 71 | ["best_particle_x"] 72 | 73 | ["best_particle_y"] 74 | 75 | ["best_particle_theta"] 76 | 77 | //Optional message data used for debugging particle's sensing and associations 78 | 79 | // for respective (x,y) sensed positions ID label 80 | 81 | ["best_particle_associations"] 82 | 83 | // for respective (x,y) sensed positions 84 | 85 | ["best_particle_sense_x"] <= list of sensed x positions 86 | 87 | ["best_particle_sense_y"] <= list of sensed y positions 88 | 89 | 90 | Your job is to build out the methods in `particle_filter.cpp` until the simulator output says: 91 | 92 | ``` 93 | Success! Your particle filter passed! 94 | ``` 95 | 96 | # Implementing the Particle Filter 97 | The directory structure of this repository is as follows: 98 | 99 | ``` 100 | root 101 | | build.sh 102 | | clean.sh 103 | | CMakeLists.txt 104 | | README.md 105 | | run.sh 106 | | 107 | |___data 108 | | | 109 | | | map_data.txt 110 | | 111 | | 112 | |___src 113 | | helper_functions.h 114 | | main.cpp 115 | | map.h 116 | | particle_filter.cpp 117 | | particle_filter.h 118 | ``` 119 | 120 | The only file you should modify is `particle_filter.cpp` in the `src` directory. The file contains the scaffolding of a `ParticleFilter` class and some associated methods. Read through the code, the comments, and the header file `particle_filter.h` to get a sense for what this code is expected to do. 121 | 122 | If you are interested, take a look at `src/main.cpp` as well. This file contains the code that will actually be running your particle filter and calling the associated methods. 123 | 124 | ## Inputs to the Particle Filter 125 | You can find the inputs to the particle filter in the `data` directory. 126 | 127 | #### The Map* 128 | `map_data.txt` includes the position of landmarks (in meters) on an arbitrary Cartesian coordinate system. Each row has three columns 129 | 1. x position 130 | 2. y position 131 | 3. landmark id 132 | 133 | ### All other data the simulator provides, such as observations and controls. 134 | 135 | > * Map data provided by 3D Mapping Solutions GmbH. 136 | 137 | ## Success Criteria 138 | If your particle filter passes the current grading code in the simulator (you can make sure you have the current version at any time by doing a `git pull`), then you should pass! 139 | 140 | The things the grading code is looking for are: 141 | 142 | 143 | 1. **Accuracy**: your particle filter should localize vehicle position and yaw to within the values specified in the parameters `max_translation_error` and `max_yaw_error` in `src/main.cpp`. 144 | 145 | 2. **Performance**: your particle filter should complete execution within the time of 100 seconds. 146 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "json.hpp" 4 | #include 5 | #include "particle_filter.h" 6 | 7 | using namespace std; 8 | 9 | // for convenience 10 | using json = nlohmann::json; 11 | 12 | // Checks if the SocketIO event has JSON data. 13 | // If there is data the JSON object in string format will be returned, 14 | // else the empty string "" will be returned. 15 | std::string hasData(std::string s) { 16 | auto found_null = s.find("null"); 17 | auto b1 = s.find_first_of("["); 18 | auto b2 = s.find_first_of("]"); 19 | if (found_null != std::string::npos) { 20 | return ""; 21 | } 22 | else if (b1 != std::string::npos && b2 != std::string::npos) { 23 | return s.substr(b1, b2 - b1 + 1); 24 | } 25 | return ""; 26 | } 27 | 28 | int main() 29 | { 30 | uWS::Hub h; 31 | 32 | //Set up parameters here 33 | double delta_t = 0.1; // Time elapsed between measurements [sec] 34 | double sensor_range = 50; // Sensor range [m] 35 | 36 | double sigma_pos [3] = {0.3, 0.3, 0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]] 37 | double sigma_landmark [2] = {0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]] 38 | 39 | // Read map data 40 | Map map; 41 | if (!read_map_data("../data/map_data.txt", map)) { 42 | cout << "Error: Could not open map file" << endl; 43 | return -1; 44 | } 45 | 46 | // Create particle filter 47 | ParticleFilter pf; 48 | 49 | h.onMessage([&pf,&map,&delta_t,&sensor_range,&sigma_pos,&sigma_landmark](uWS::WebSocket ws, char *data, size_t length, uWS::OpCode opCode) { 50 | // "42" at the start of the message means there's a websocket message event. 51 | // The 4 signifies a websocket message 52 | // The 2 signifies a websocket event 53 | 54 | if (length && length > 2 && data[0] == '4' && data[1] == '2') 55 | { 56 | 57 | auto s = hasData(std::string(data)); 58 | if (s != "") { 59 | 60 | 61 | auto j = json::parse(s); 62 | std::string event = j[0].get(); 63 | 64 | if (event == "telemetry") { 65 | // j[1] is the data JSON object 66 | 67 | 68 | if (!pf.initialized()) { 69 | 70 | // Sense noisy position data from the simulator 71 | double sense_x = std::stod(j[1]["sense_x"].get()); 72 | double sense_y = std::stod(j[1]["sense_y"].get()); 73 | double sense_theta = std::stod(j[1]["sense_theta"].get()); 74 | 75 | pf.init(sense_x, sense_y, sense_theta, sigma_pos); 76 | } 77 | else { 78 | // Predict the vehicle's next state from previous (noiseless control) data. 79 | double previous_velocity = std::stod(j[1]["previous_velocity"].get()); 80 | double previous_yawrate = std::stod(j[1]["previous_yawrate"].get()); 81 | 82 | pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate); 83 | } 84 | 85 | // receive noisy observation data from the simulator 86 | // sense_observations in JSON format [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}] 87 | vector noisy_observations; 88 | string sense_observations_x = j[1]["sense_observations_x"]; 89 | string sense_observations_y = j[1]["sense_observations_y"]; 90 | 91 | std::vector x_sense; 92 | std::istringstream iss_x(sense_observations_x); 93 | 94 | std::copy(std::istream_iterator(iss_x), 95 | std::istream_iterator(), 96 | std::back_inserter(x_sense)); 97 | 98 | std::vector y_sense; 99 | std::istringstream iss_y(sense_observations_y); 100 | 101 | std::copy(std::istream_iterator(iss_y), 102 | std::istream_iterator(), 103 | std::back_inserter(y_sense)); 104 | 105 | for(int i = 0; i < x_sense.size(); i++) 106 | { 107 | LandmarkObs obs; 108 | obs.x = x_sense[i]; 109 | obs.y = y_sense[i]; 110 | noisy_observations.push_back(obs); 111 | } 112 | 113 | // Update the weights and resample 114 | pf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map); 115 | pf.resample(); 116 | 117 | // Calculate and output the average weighted error of the particle filter over all time steps so far. 118 | vector particles = pf.particles; 119 | int num_particles = particles.size(); 120 | double highest_weight = -1.0; 121 | Particle best_particle; 122 | double weight_sum = 0.0; 123 | for (int i = 0; i < num_particles; ++i) { 124 | if (particles[i].weight > highest_weight) { 125 | highest_weight = particles[i].weight; 126 | best_particle = particles[i]; 127 | } 128 | weight_sum += particles[i].weight; 129 | } 130 | cout << "highest w " << highest_weight << endl; 131 | cout << "average w " << weight_sum/num_particles << endl; 132 | 133 | json msgJson; 134 | msgJson["best_particle_x"] = best_particle.x; 135 | msgJson["best_particle_y"] = best_particle.y; 136 | msgJson["best_particle_theta"] = best_particle.theta; 137 | 138 | //Optional message data used for debugging particle's sensing and associations 139 | msgJson["best_particle_associations"] = pf.getAssociations(best_particle); 140 | msgJson["best_particle_sense_x"] = pf.getSenseX(best_particle); 141 | msgJson["best_particle_sense_y"] = pf.getSenseY(best_particle); 142 | 143 | auto msg = "42[\"best_particle\"," + msgJson.dump() + "]"; 144 | // std::cout << msg << std::endl; 145 | ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); 146 | 147 | } 148 | } else { 149 | std::string msg = "42[\"manual\",{}]"; 150 | ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); 151 | } 152 | } 153 | 154 | }); 155 | 156 | // We don't need this since we're not using HTTP but if it's removed the program 157 | // doesn't compile :-( 158 | h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data, size_t, size_t) { 159 | const std::string s = "

Hello world!

"; 160 | if (req.getUrl().valueLength == 1) 161 | { 162 | res->end(s.data(), s.length()); 163 | } 164 | else 165 | { 166 | // i guess this should be done more gracefully? 167 | res->end(nullptr, 0); 168 | } 169 | }); 170 | 171 | h.onConnection([&h](uWS::WebSocket ws, uWS::HttpRequest req) { 172 | std::cout << "Connected!!!" << std::endl; 173 | }); 174 | 175 | h.onDisconnection([&h](uWS::WebSocket ws, int code, char *message, size_t length) { 176 | ws.close(); 177 | std::cout << "Disconnected" << std::endl; 178 | }); 179 | 180 | int port = 4567; 181 | if (h.listen(port)) 182 | { 183 | std::cout << "Listening to port " << port << std::endl; 184 | } 185 | else 186 | { 187 | std::cerr << "Failed to listen to port" << std::endl; 188 | return -1; 189 | } 190 | h.run(); 191 | } 192 | -------------------------------------------------------------------------------- /src/helper_functions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * helper_functions.h 3 | * Some helper functions for the 2D particle filter. 4 | * Created on: Dec 13, 2016 5 | * Author: Tiffany Huang 6 | */ 7 | 8 | #ifndef HELPER_FUNCTIONS_H_ 9 | #define HELPER_FUNCTIONS_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "map.h" 16 | 17 | // for portability of M_PI (Vis Studio, MinGW, etc.) 18 | #ifndef M_PI 19 | const double M_PI = 3.14159265358979323846; 20 | #endif 21 | 22 | /* 23 | * Struct representing one position/control measurement. 24 | */ 25 | struct control_s { 26 | 27 | double velocity; // Velocity [m/s] 28 | double yawrate; // Yaw rate [rad/s] 29 | }; 30 | 31 | /* 32 | * Struct representing one ground truth position. 33 | */ 34 | struct ground_truth { 35 | 36 | double x; // Global vehicle x position [m] 37 | double y; // Global vehicle y position 38 | double theta; // Global vehicle yaw [rad] 39 | }; 40 | 41 | /* 42 | * Struct representing one landmark observation measurement. 43 | */ 44 | struct LandmarkObs { 45 | 46 | int id; // Id of matching landmark in the map. 47 | double x; // Local (vehicle coordinates) x position of landmark observation [m] 48 | double y; // Local (vehicle coordinates) y position of landmark observation [m] 49 | }; 50 | 51 | /* 52 | * Computes the Euclidean distance between two 2D points. 53 | * @param (x1,y1) x and y coordinates of first point 54 | * @param (x2,y2) x and y coordinates of second point 55 | * @output Euclidean distance between two 2D points 56 | */ 57 | inline double dist(double x1, double y1, double x2, double y2) { 58 | return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); 59 | } 60 | 61 | inline double * getError(double gt_x, double gt_y, double gt_theta, double pf_x, double pf_y, double pf_theta) { 62 | static double error[3]; 63 | error[0] = fabs(pf_x - gt_x); 64 | error[1] = fabs(pf_y - gt_y); 65 | error[2] = fabs(pf_theta - gt_theta); 66 | error[2] = fmod(error[2], 2.0 * M_PI); 67 | if (error[2] > M_PI) { 68 | error[2] = 2.0 * M_PI - error[2]; 69 | } 70 | return error; 71 | } 72 | 73 | /* Reads map data from a file. 74 | * @param filename Name of file containing map data. 75 | * @output True if opening and reading file was successful 76 | */ 77 | inline bool read_map_data(std::string filename, Map& map) { 78 | 79 | // Get file of map: 80 | std::ifstream in_file_map(filename.c_str(),std::ifstream::in); 81 | // Return if we can't open the file. 82 | if (!in_file_map) { 83 | return false; 84 | } 85 | 86 | // Declare single line of map file: 87 | std::string line_map; 88 | 89 | // Run over each single line: 90 | while(getline(in_file_map, line_map)){ 91 | 92 | std::istringstream iss_map(line_map); 93 | 94 | // Declare landmark values and ID: 95 | float landmark_x_f, landmark_y_f; 96 | int id_i; 97 | 98 | // Read data from current line to values:: 99 | iss_map >> landmark_x_f; 100 | iss_map >> landmark_y_f; 101 | iss_map >> id_i; 102 | 103 | // Declare single_landmark: 104 | Map::single_landmark_s single_landmark_temp; 105 | 106 | // Set values 107 | single_landmark_temp.id_i = id_i; 108 | single_landmark_temp.x_f = landmark_x_f; 109 | single_landmark_temp.y_f = landmark_y_f; 110 | 111 | // Add to landmark list of map: 112 | map.landmark_list.push_back(single_landmark_temp); 113 | } 114 | return true; 115 | } 116 | 117 | /* Reads control data from a file. 118 | * @param filename Name of file containing control measurements. 119 | * @output True if opening and reading file was successful 120 | */ 121 | inline bool read_control_data(std::string filename, std::vector& position_meas) { 122 | 123 | // Get file of position measurements: 124 | std::ifstream in_file_pos(filename.c_str(),std::ifstream::in); 125 | // Return if we can't open the file. 126 | if (!in_file_pos) { 127 | return false; 128 | } 129 | 130 | // Declare single line of position measurement file: 131 | std::string line_pos; 132 | 133 | // Run over each single line: 134 | while(getline(in_file_pos, line_pos)){ 135 | 136 | std::istringstream iss_pos(line_pos); 137 | 138 | // Declare position values: 139 | double velocity, yawrate; 140 | 141 | // Declare single control measurement: 142 | control_s meas; 143 | 144 | //read data from line to values: 145 | 146 | iss_pos >> velocity; 147 | iss_pos >> yawrate; 148 | 149 | 150 | // Set values 151 | meas.velocity = velocity; 152 | meas.yawrate = yawrate; 153 | 154 | // Add to list of control measurements: 155 | position_meas.push_back(meas); 156 | } 157 | return true; 158 | } 159 | 160 | /* Reads ground truth data from a file. 161 | * @param filename Name of file containing ground truth. 162 | * @output True if opening and reading file was successful 163 | */ 164 | inline bool read_gt_data(std::string filename, std::vector& gt) { 165 | 166 | // Get file of position measurements: 167 | std::ifstream in_file_pos(filename.c_str(),std::ifstream::in); 168 | // Return if we can't open the file. 169 | if (!in_file_pos) { 170 | return false; 171 | } 172 | 173 | // Declare single line of position measurement file: 174 | std::string line_pos; 175 | 176 | // Run over each single line: 177 | while(getline(in_file_pos, line_pos)){ 178 | 179 | std::istringstream iss_pos(line_pos); 180 | 181 | // Declare position values: 182 | double x, y, azimuth; 183 | 184 | // Declare single ground truth: 185 | ground_truth single_gt; 186 | 187 | //read data from line to values: 188 | iss_pos >> x; 189 | iss_pos >> y; 190 | iss_pos >> azimuth; 191 | 192 | // Set values 193 | single_gt.x = x; 194 | single_gt.y = y; 195 | single_gt.theta = azimuth; 196 | 197 | // Add to list of control measurements and ground truth: 198 | gt.push_back(single_gt); 199 | } 200 | return true; 201 | } 202 | 203 | /* Reads landmark observation data from a file. 204 | * @param filename Name of file containing landmark observation measurements. 205 | * @output True if opening and reading file was successful 206 | */ 207 | inline bool read_landmark_data(std::string filename, std::vector& observations) { 208 | 209 | // Get file of landmark measurements: 210 | std::ifstream in_file_obs(filename.c_str(),std::ifstream::in); 211 | // Return if we can't open the file. 212 | if (!in_file_obs) { 213 | return false; 214 | } 215 | 216 | // Declare single line of landmark measurement file: 217 | std::string line_obs; 218 | 219 | // Run over each single line: 220 | while(getline(in_file_obs, line_obs)){ 221 | 222 | std::istringstream iss_obs(line_obs); 223 | 224 | // Declare position values: 225 | double local_x, local_y; 226 | 227 | //read data from line to values: 228 | iss_obs >> local_x; 229 | iss_obs >> local_y; 230 | 231 | // Declare single landmark measurement: 232 | LandmarkObs meas; 233 | 234 | // Set values 235 | meas.x = local_x; 236 | meas.y = local_y; 237 | 238 | // Add to list of control measurements: 239 | observations.push_back(meas); 240 | } 241 | return true; 242 | } 243 | 244 | #endif /* HELPER_FUNCTIONS_H_ */ 245 | -------------------------------------------------------------------------------- /src/particle_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * particle_filter.cpp 3 | * 4 | * Created on: Dec 12, 2016 5 | * Author: Tiffany Huang 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "particle_filter.h" 19 | 20 | using namespace std; 21 | 22 | void ParticleFilter::init(double x, double y, double theta, double std[]) { 23 | // TODO: Set the number of particles. Initialize all particles to first position (based on estimates of 24 | // x, y, theta and their uncertainties from GPS) and all weights to 1. 25 | // Add random Gaussian noise to each particle. 26 | // NOTE: Consult particle_filter.h for more information about this method (and others in this file). 27 | 28 | default_random_engine gen; 29 | 30 | if (is_initialized) { 31 | return; 32 | } 33 | 34 | // Initializing the number of particles 35 | num_particles = 100; 36 | 37 | // Extracting standard deviations 38 | double std_x = std[0]; 39 | double std_y = std[1]; 40 | double std_theta = std[2]; 41 | 42 | // Creating normal distributions 43 | normal_distribution dist_x(x, std_x); 44 | normal_distribution dist_y(y, std_y); 45 | normal_distribution dist_theta(theta, std_theta); 46 | 47 | // Generate particles with normal distribution with mean on GPS values. 48 | for (int i = 0; i < num_particles; i++) { 49 | 50 | Particle particle; 51 | particle.id = i; 52 | particle.x = dist_x(gen); 53 | particle.y = dist_y(gen); 54 | particle.theta = dist_theta(gen); 55 | particle.weight = 1.0; 56 | 57 | particles.push_back(particle); 58 | } 59 | 60 | // The filter is now initialized. 61 | is_initialized = true; 62 | } 63 | 64 | 65 | 66 | 67 | void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) { 68 | // TODO: Add measurements to each particle and add random Gaussian noise. 69 | // NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful. 70 | // http://en.cppreference.com/w/cpp/numeric/random/normal_distribution 71 | // http://www.cplusplus.com/reference/random/default_random_engine/ 72 | 73 | default_random_engine gen; 74 | 75 | // Extracting standard deviations 76 | double std_x = std_pos[0]; 77 | double std_y = std_pos[1]; 78 | double std_theta = std_pos[2]; 79 | 80 | // Creating normal distributions 81 | normal_distribution dist_x(0, std_x); 82 | normal_distribution dist_y(0, std_y); 83 | normal_distribution dist_theta(0, std_theta); 84 | 85 | // Calculate new state. 86 | for (int i = 0; i < num_particles; i++) { 87 | 88 | double theta = particles[i].theta; 89 | 90 | if ( fabs(yaw_rate) < 0.00001 ) { // When yaw is not changing. 91 | particles[i].x += velocity * delta_t * cos( theta ); 92 | particles[i].y += velocity * delta_t * sin( theta ); 93 | // yaw continue to be the same. 94 | } else { 95 | particles[i].x += velocity / yaw_rate * ( sin( theta + yaw_rate * delta_t ) - sin( theta ) ); 96 | particles[i].y += velocity / yaw_rate * ( cos( theta ) - cos( theta + yaw_rate * delta_t ) ); 97 | particles[i].theta += yaw_rate * delta_t; 98 | } 99 | 100 | // Adding noise. 101 | particles[i].x += dist_x(gen); 102 | particles[i].y += dist_y(gen); 103 | particles[i].theta += dist_theta(gen); 104 | } 105 | } 106 | 107 | void ParticleFilter::dataAssociation(std::vector predicted, std::vector& observations) { 108 | // TODO: Find the predicted measurement that is closest to each observed measurement and assign the 109 | // observed measurement to this particular landmark. 110 | // NOTE: this method will NOT be called by the grading code. But you will probably find it useful to 111 | // implement this method and use it as a helper during the updateWeights phase. 112 | unsigned int nObservations = observations.size(); 113 | unsigned int nPredictions = predicted.size(); 114 | 115 | for (unsigned int i = 0; i < nObservations; i++) { // For each observation 116 | 117 | // Initialize min distance as a really big number. 118 | double minDistance = numeric_limits::max(); 119 | 120 | // Initialize the found map in something not possible. 121 | int mapId = -1; 122 | 123 | for (unsigned j = 0; j < nPredictions; j++ ) { // For each predition. 124 | 125 | double xDistance = observations[i].x - predicted[j].x; 126 | double yDistance = observations[i].y - predicted[j].y; 127 | 128 | double distance = xDistance * xDistance + yDistance * yDistance; 129 | 130 | // If the "distance" is less than min, stored the id and update min. 131 | if ( distance < minDistance ) { 132 | minDistance = distance; 133 | mapId = predicted[j].id; 134 | } 135 | } 136 | 137 | // Update the observation identifier. 138 | observations[i].id = mapId; 139 | } 140 | } 141 | 142 | void ParticleFilter::updateWeights(double sensor_range, double std_landmark[], 143 | const std::vector &observations, const Map &map_landmarks) { 144 | // TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read 145 | // more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution 146 | // NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located 147 | // according to the MAP'S coordinate system. You will need to transform between the two systems. 148 | // Keep in mind that this transformation requires both rotation AND translation (but no scaling). 149 | // The following is a good resource for the theory: 150 | // https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm 151 | // and the following is a good resource for the actual equation to implement (look at equation 152 | // 3.33 153 | // http://planning.cs.uiuc.edu/node99.html 154 | 155 | double stdLandmarkRange = std_landmark[0]; 156 | double stdLandmarkBearing = std_landmark[1]; 157 | 158 | for (int i = 0; i < num_particles; i++) { 159 | 160 | double x = particles[i].x; 161 | double y = particles[i].y; 162 | double theta = particles[i].theta; 163 | // Find landmarks in particle's range. 164 | double sensor_range_2 = sensor_range * sensor_range; 165 | vector inRangeLandmarks; 166 | for(unsigned int j = 0; j < map_landmarks.landmark_list.size(); j++) { 167 | float landmarkX = map_landmarks.landmark_list[j].x_f; 168 | float landmarkY = map_landmarks.landmark_list[j].y_f; 169 | int id = map_landmarks.landmark_list[j].id_i; 170 | double dX = x - landmarkX; 171 | double dY = y - landmarkY; 172 | if ( dX*dX + dY*dY <= sensor_range_2 ) { 173 | inRangeLandmarks.push_back(LandmarkObs{ id, landmarkX, landmarkY }); 174 | } 175 | } 176 | 177 | // Transform observation coordinates. 178 | vector mappedObservations; 179 | for(unsigned int j = 0; j < observations.size(); j++) { 180 | double xx = cos(theta)*observations[j].x - sin(theta)*observations[j].y + x; 181 | double yy = sin(theta)*observations[j].x + cos(theta)*observations[j].y + y; 182 | mappedObservations.push_back(LandmarkObs{ observations[j].id, xx, yy }); 183 | } 184 | 185 | // Observation association to landmark. 186 | dataAssociation(inRangeLandmarks, mappedObservations); 187 | 188 | // Reseting weight. 189 | particles[i].weight = 1.0; 190 | // Calculate weights. 191 | for(unsigned int j = 0; j < mappedObservations.size(); j++) { 192 | double observationX = mappedObservations[j].x; 193 | double observationY = mappedObservations[j].y; 194 | 195 | int landmarkId = mappedObservations[j].id; 196 | 197 | double landmarkX, landmarkY; 198 | unsigned int k = 0; 199 | unsigned int nLandmarks = inRangeLandmarks.size(); 200 | bool found = false; 201 | while( !found && k < nLandmarks ) { 202 | if ( inRangeLandmarks[k].id == landmarkId) { 203 | found = true; 204 | landmarkX = inRangeLandmarks[k].x; 205 | landmarkY = inRangeLandmarks[k].y; 206 | } 207 | k++; 208 | } 209 | 210 | // Calculating weight. 211 | double dX = observationX - landmarkX; 212 | double dY = observationY - landmarkY; 213 | 214 | double weight = ( 1/(2*M_PI*stdLandmarkRange*stdLandmarkBearing)) * exp( -( dX*dX/(2*stdLandmarkRange*stdLandmarkRange) + (dY*dY/(2*stdLandmarkBearing*stdLandmarkBearing)) ) ); 215 | if (weight == 0) { 216 | particles[i].weight *= 0.00001; 217 | } else { 218 | particles[i].weight *= weight; 219 | } 220 | } 221 | } 222 | 223 | } 224 | 225 | void ParticleFilter::resample() { 226 | // TODO: Resample particles with replacement with probability proportional to their weight. 227 | // NOTE: You may find std::discrete_distribution helpful here. 228 | // http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution 229 | // Get weights and max weight. 230 | 231 | default_random_engine gen; 232 | 233 | vector weights; 234 | double maxWeight = numeric_limits::min(); 235 | for(int i = 0; i < num_particles; i++) { 236 | weights.push_back(particles[i].weight); 237 | if ( particles[i].weight > maxWeight ) { 238 | maxWeight = particles[i].weight; 239 | } 240 | } 241 | 242 | // Creating distributions. 243 | uniform_real_distribution distDouble(0.0, maxWeight); 244 | uniform_int_distribution distInt(0, num_particles - 1); 245 | 246 | // Generating index. 247 | int index = distInt(gen); 248 | 249 | double beta = 0.0; 250 | 251 | // the wheel 252 | vector resampledParticles; 253 | for(int i = 0; i < num_particles; i++) { 254 | beta += distDouble(gen) * 2.0; 255 | while( beta > weights[index]) { 256 | beta -= weights[index]; 257 | index = (index + 1) % num_particles; 258 | } 259 | resampledParticles.push_back(particles[index]); 260 | } 261 | 262 | particles = resampledParticles; 263 | } 264 | 265 | 266 | Particle ParticleFilter::SetAssociations(Particle& particle, const std::vector& associations, 267 | const std::vector& sense_x, const std::vector& sense_y) 268 | { 269 | //particle: the particle to assign each listed association, and association's (x,y) world coordinates mapping to 270 | // associations: The landmark id that goes along with each listed association 271 | // sense_x: the associations x mapping already converted to world coordinates 272 | // sense_y: the associations y mapping already converted to world coordinates 273 | 274 | particle.associations= associations; 275 | particle.sense_x = sense_x; 276 | particle.sense_y = sense_y; 277 | } 278 | 279 | string ParticleFilter::getAssociations(Particle best) 280 | { 281 | vector v = best.associations; 282 | stringstream ss; 283 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 284 | string s = ss.str(); 285 | s = s.substr(0, s.length()-1); // get rid of the trailing space 286 | return s; 287 | } 288 | string ParticleFilter::getSenseX(Particle best) 289 | { 290 | vector v = best.sense_x; 291 | stringstream ss; 292 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 293 | string s = ss.str(); 294 | s = s.substr(0, s.length()-1); // get rid of the trailing space 295 | return s; 296 | } 297 | string ParticleFilter::getSenseY(Particle best) 298 | { 299 | vector v = best.sense_y; 300 | stringstream ss; 301 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 302 | string s = ss.str(); 303 | s = s.substr(0, s.length()-1); // get rid of the trailing space 304 | return s; 305 | } 306 | --------------------------------------------------------------------------------