├── CMakeLists.txt ├── LICENSE ├── Particle_filter_output.png ├── README.md ├── build.sh ├── clean.sh ├── cmakepatch.txt ├── data └── map_data.txt ├── install-mac.sh ├── install-ubuntu.sh ├── run.sh └── src ├── helper_functions.h ├── json.hpp ├── main.cpp ├── map.h ├── particle_filter.cpp └── particle_filter.h /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Particle_filter_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sharathsrini/Particle-Filter/fb0d81d59c5068fcad68d8adeaf946659f56266b/Particle_filter_output.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Overview 2 | * Particle filter addresses many problem in applied robotics. Assume we might have moving objects that we want to track. Maybe the objects are fighter jets and missiles, or maybe we are tracking people playing cricket in a field. It doesn't really matter. Let's think about the characteristics of this problem. 3 | * Multimodal: We want to track zero, one, or more than one object simultaneously 4 | * Occlusions: One object can hide another, resulting in one measurement for multiple objects. 5 | * Non-linear behaviour: Aircraft are buffeted by winds, balls move in parabolas, and people collide into each other. 6 | * Non-linear measurements: Radar gives us the distance to an object. Converting that to an (x,y,z) coordinate requires a square root, which is nonlinear. 7 | * Non-Gaussian noise: as objects move across a background the computer vision can mistake part of the background for the object. 8 | * Continuous: the object's position and velocity (i.e. the state space) can smoothly vary over time. 9 | * Multivariate: we want to track several attributes, such as position, velocity, turn rates, etc. 10 | * Unknown process model: we may not know the process model of the system 11 | 12 | 13 | ## Project Introduction 14 | 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. 15 | 16 | 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. 17 | 18 | ### Formulae 19 | ## Motion Model 20 | 21 | ![alt text](https://cdn-images-1.medium.com/max/720/1*GT4bfVN82qOPpdCtUwkxig.png) 22 | 23 | 24 | ## Observation 25 | 26 | ![alt_text](https://cdn-images-1.medium.com/max/720/1*uR0dYxOKWHhEPUT6YMGXGg.png) 27 | ![alt_text](https://cdn-images-1.medium.com/max/720/1*nB_6uUWjDKC-pGWToRTQDQ.png) 28 | 29 | ## Simulation 30 | 31 | ![alt_text](https://cdn-images-1.medium.com/max/720/1*y92MO5zzieuqxH5-uHD0YQ.png) 32 | 33 | ## Update Weight 34 | !alt_text](https://cdn-images-1.medium.com/max/720/1*C8vytZC_jE0unb2TSKRUGw.png) 35 | 36 | 37 | 38 | 39 | 40 | # Implementing the Particle Filter 41 | The directory structure of this repository is as follows: 42 | 43 | ``` 44 | root 45 | | build.sh 46 | | clean.sh 47 | | CMakeLists.txt 48 | | README.md 49 | | run.sh 50 | | 51 | |___data 52 | 53 | | | 54 | | | map_data.txt 55 | | 56 | | 57 | |___src 58 | | helper_functions.h 59 | | main.cpp 60 | | map.h 61 | | particle_filter.cpp 62 | | particle_filter.h 63 | ``` 64 | 65 | 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. 66 | 67 | 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. 68 | 69 | ## Inputs to the Particle Filter 70 | You can find the inputs to the particle filter in the `data` directory. 71 | 72 | #### The Map* 73 | `map_data.txt` includes the position of landmarks (in meters) on an arbitrary Cartesian coordinate system. Each row has three columns 74 | 1. x position 75 | 2. y position 76 | 3. landmark id 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to build all components from scratch, using the maximum available CPU power 3 | # 4 | # Given parameters are passed over to CMake. 5 | # Examples: 6 | # * ./build_all.sh -DCMAKE_BUILD_TYPE=Debug 7 | # * ./build_all.sh VERBOSE=1 8 | # 9 | # Written by Tiffany Huang, 12/14/2016 10 | # 11 | 12 | # Go into the directory where this bash script is contained. 13 | cd `dirname $0` 14 | 15 | # Compile code. 16 | mkdir -p build 17 | cd build 18 | cmake .. 19 | make -j `nproc` $* 20 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/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 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | #include "particle_filter.h" 18 | 19 | using namespace std; 20 | 21 | void ParticleFilter::init(double x, double y, double theta, double std[]) { 22 | // TODO: Set the number of particles. Initialize all particles to first position (based on estimates of 23 | // x, y, theta and their uncertainties from GPS) and all weights to 1. 24 | // Add random Gaussian noise to each particle. 25 | // NOTE: Consult particle_filter.h for more information about this method (and others in this file). 26 | 27 | number_of_particles = 100; 28 | 29 | default_random_engine gen; 30 | 31 | normal_distribution dist_x(x, std[0]); 32 | normal_distribution dist_y(y, std[1]); 33 | normal_distribution dist_theta(theta, std[2]); 34 | 35 | // Generate particles with normal distribution with mean on GPS values. 36 | for (int i = 0; i < number_of_particles; i++) { 37 | Particle first_particle; 38 | 39 | first_particle.id = i; 40 | first_particle.x = dist_x(gen); 41 | first_particle.y = dist_y(gen); 42 | first_particle.theta = dist_theta(gen); 43 | first_particle.weight = 1.0; 44 | 45 | particles.push_back(first_particle); 46 | } 47 | 48 | is_initialized = true; 49 | 50 | } 51 | 52 | void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) { 53 | // TODO: Add measurements to each particle and add random Gaussian noise. 54 | // NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful. 55 | // http://en.cppreference.com/w/cpp/numeric/random/normal_distribution 56 | // http://www.cplusplus.com/reference/random/default_random_engine/ 57 | 58 | default_random_engine gen; 59 | 60 | normal_distribution dist_x(0, std_pos[0]); 61 | normal_distribution dist_y(0, std_pos[1]); 62 | normal_distribution dist_theta(0, std_pos[2]); 63 | 64 | for (int i=0; i < number_of_particles; i++){ 65 | if ( fabs(yaw_rate) < 0.0001 ) { // When yaw is not changing. 66 | particles[i].x += velocity * delta_t * cos( particles[i].theta ); 67 | particles[i].y += velocity * delta_t * sin( particles[i].theta ); 68 | // yaw continue to be the same. 69 | } else { 70 | particles[i].x += velocity / yaw_rate * ( sin( particles[i].theta + yaw_rate * delta_t ) - sin( particles[i].theta ) ); 71 | particles[i].y += velocity / yaw_rate * ( cos( particles[i].theta ) - cos( particles[i].theta + yaw_rate * delta_t ) ); 72 | particles[i].theta += yaw_rate * delta_t; 73 | } 74 | 75 | particles[i].x += dist_x(gen); 76 | particles[i].y += dist_y(gen); 77 | particles[i].theta += dist_theta(gen); 78 | } 79 | 80 | } 81 | 82 | void ParticleFilter::dataAssociation(std::vector predicted, std::vector& observations) { 83 | // TODO: Find the predicted measurement that is closest to each observed measurement and assign the 84 | // observed measurement to this particular landmark. 85 | // NOTE: this method will NOT be called by the grading code. But you will probably find it useful to 86 | // implement this method and use it as a helper during the updateWeights phase. 87 | 88 | for(int i=0; i < observations.size(); i++){ 89 | double min_Dist = numeric_limits::max(); 90 | double obs_x = observations[i].x; 91 | double obs_y = observations[i].y; 92 | int matching_Landmark_ID = -1; 93 | 94 | for (int j=0; j < predicted.size(); j++){ 95 | double pred_x = predicted[j].x; 96 | double pred_y = predicted[j].y; 97 | double currentDist = dist(obs_x, obs_y, pred_x, pred_y); 98 | 99 | if (currentDist < min_Dist){ 100 | min_Dist = currentDist; 101 | matching_Landmark_ID = predicted[j].id; 102 | } 103 | } 104 | observations[i].id = matching_Landmark_ID; 105 | } 106 | 107 | 108 | 109 | } 110 | 111 | void ParticleFilter::updateWeights(double sensor_range, double std_landmark[], 112 | const std::vector &observations, const Map &map_landmarks) { 113 | // TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read 114 | // more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution 115 | // NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located 116 | // according to the MAP'S coordinate system. You will need to transform between the two systems. 117 | // Keep in mind that this transformation requires both rotation AND translation (but no scaling). 118 | // The following is a good resource for the theory: 119 | // https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm 120 | // and the following is a good resource for the actual equation to implement (look at equation 121 | // 3.33 122 | // http://planning.cs.uiuc.edu/node99.html 123 | 124 | for (int i = 0; i < number_of_particles; i++) { 125 | double x = particles[i].x; 126 | double y = particles[i].y; 127 | double theta = particles[i].theta; 128 | 129 | // Step 1 -- find landmarks within the sensor range 130 | vector predicted; 131 | for (int j = 0; j < map_landmarks.landmark_list.size(); j++) { 132 | LandmarkObs pred; 133 | pred.id = map_landmarks.landmark_list[j].id_i; 134 | pred.x = map_landmarks.landmark_list[j].x_f; 135 | pred.y = map_landmarks.landmark_list[j].y_f; 136 | 137 | if (fabs(pred.x - x) <= sensor_range && fabs(pred.y - y) <= sensor_range) { 138 | predicted.push_back(pred); 139 | } 140 | } 141 | 142 | 143 | // Transform each observations to map coordinates 144 | vector transformedObservations; 145 | for (int j=0; j < observations.size(); j++){ 146 | LandmarkObs transformedObs; 147 | transformedObs.id = j; 148 | transformedObs.x = x + (cos(theta) * observations[j].x) - (sin(theta) * observations[j].y); 149 | transformedObs.y = y + (sin(theta) * observations[j].x) + (cos(theta) * observations[j].y); 150 | transformedObservations.push_back(transformedObs); 151 | } 152 | 153 | // Associate the landmark in range to landmark observations 154 | dataAssociation(predicted, transformedObservations); 155 | 156 | //Update the particle weight using Multivariate Gaussian distribution 157 | particles[i].weight = 1.0; 158 | 159 | 160 | double sigma_X_Squared = std_landmark[0]*std_landmark[0]; 161 | double sigma_Y_Squared = std_landmark[2]*std_landmark[2]; 162 | double normalizing_Factor = (1.0/(2.0 * M_PI * std_landmark[0] * std_landmark[0])); 163 | 164 | for (int j = 0; j < transformedObservations.size(); j++){ 165 | double trans_Obs_X = transformedObservations[j].x; 166 | double trans_Obs_Y = transformedObservations[j].y; 167 | double trans_Obs_ID = transformedObservations[j].id; 168 | 169 | for (int j_P = 0; j_P < predicted.size(); j_P++){ 170 | double pred_X = predicted[j_P].x; 171 | double pred_Y = predicted[j_P].y; 172 | double predID = predicted[j_P].id; 173 | 174 | if (trans_Obs_ID == predID){ 175 | double dx_Squared = pow(trans_Obs_X - pred_X, 2); 176 | double dy_Squared = pow(trans_Obs_Y - pred_Y, 2); 177 | double weight = normalizing_Factor * exp(-(dx_Squared/(2*sigma_X_Squared) + dy_Squared/(2*sigma_Y_Squared))); 178 | particles[i].weight *= weight; 179 | 180 | break; 181 | } 182 | } 183 | } 184 | } 185 | } 186 | 187 | void ParticleFilter::resample() { 188 | // TODO: Resample particles with replacement with probability proportional to their weight. 189 | // NOTE: You may find std::discrete_distribution helpful here. 190 | // http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution 191 | 192 | default_random_engine gen; 193 | 194 | 195 | vector weights; 196 | double maxWeight = numeric_limits::min(); 197 | for (int i=0; i < number_of_particles; i++){ 198 | weights.push_back(particles[i].weight); 199 | if ( particles[i].weight > maxWeight ) { 200 | maxWeight = particles[i].weight; 201 | } 202 | } 203 | 204 | // uniform random distribution [0.0, max_weight) 205 | uniform_real_distribution maxW(0.0, maxWeight); 206 | 207 | uniform_int_distribution particleIdx(0, number_of_particles - 1); 208 | int index = particleIdx(gen); 209 | 210 | // Re-sampling Step 211 | vector resampledParticles; 212 | double beta = 0; 213 | for (int i=0; i < number_of_particles; i++){ 214 | beta += maxW(gen) * 2.0; 215 | while( beta > weights[index]){ 216 | beta -= weights[index]; 217 | index = (index + 1) % number_of_particles; 218 | } 219 | resampledParticles.push_back(particles[index]); 220 | } 221 | particles = resampledParticles; 222 | 223 | } 224 | 225 | Particle ParticleFilter::SetAssociations(Particle& particle, const std::vector& associations, 226 | const std::vector& sense_x, const std::vector& sense_y) 227 | { 228 | //particle: the particle to assign each listed association, and association's (x,y) world coordinates mapping to 229 | // associations: The landmark id that goes along with each listed association 230 | // sense_x: the associations x mapping already converted to world coordinates 231 | // sense_y: the associations y mapping already converted to world coordinates 232 | 233 | particle.associations= associations; 234 | particle.sense_x = sense_x; 235 | particle.sense_y = sense_y; 236 | 237 | return particle; 238 | } 239 | 240 | string ParticleFilter::getAssociations(Particle best) 241 | { 242 | vector v = best.associations; 243 | stringstream ss; 244 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 245 | string s = ss.str(); 246 | s = s.substr(0, s.length()-1); // get rid of the trailing space 247 | return s; 248 | } 249 | string ParticleFilter::getSenseX(Particle best) 250 | { 251 | vector v = best.sense_x; 252 | stringstream ss; 253 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 254 | string s = ss.str(); 255 | s = s.substr(0, s.length()-1); // get rid of the trailing space 256 | return s; 257 | } 258 | string ParticleFilter::getSenseY(Particle best) 259 | { 260 | vector v = best.sense_y; 261 | stringstream ss; 262 | copy( v.begin(), v.end(), ostream_iterator(ss, " ")); 263 | string s = ss.str(); 264 | s = s.substr(0, s.length()-1); // get rid of the trailing space 265 | return s; 266 | } 267 | -------------------------------------------------------------------------------- /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 number_of_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 number_of_particles Number of particles 48 | ParticleFilter() : number_of_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 | --------------------------------------------------------------------------------