├── .cproject ├── .gitignore ├── .project ├── .settings └── language.settings.xml ├── LICENSE ├── README.md ├── data ├── example01 │ ├── control_data.txt │ ├── gt_example01.txt │ └── observations │ │ ├── observations_000001.txt │ │ ├── observations_000002.txt │ │ ├── observations_000003.txt │ │ ├── observations_000004.txt │ │ ├── observations_000005.txt │ │ ├── observations_000006.txt │ │ ├── observations_000007.txt │ │ ├── observations_000008.txt │ │ ├── observations_000009.txt │ │ ├── observations_000010.txt │ │ ├── observations_000011.txt │ │ ├── observations_000012.txt │ │ ├── observations_000013.txt │ │ └── observations_000014.txt ├── example03 │ ├── control_data.txt │ ├── gt_example01.txt │ └── observations │ │ ├── observations_000001.txt │ │ ├── observations_000002.txt │ │ ├── observations_000003.txt │ │ ├── observations_000004.txt │ │ ├── observations_000005.txt │ │ ├── observations_000006.txt │ │ ├── observations_000007.txt │ │ ├── observations_000008.txt │ │ ├── observations_000009.txt │ │ ├── observations_000010.txt │ │ ├── observations_000011.txt │ │ ├── observations_000012.txt │ │ ├── observations_000013.txt │ │ └── observations_000014.txt └── map_1d.txt └── src ├── bayesian_filter.cpp ├── bayesian_filter.h ├── help_functions.cpp ├── help_functions.h ├── main.cpp ├── map.h └── measurement_package.h /.cproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 38 | 39 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 86 | 87 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # silly macOS 2 | .DS_Store 3 | 4 | # build files 5 | build/ 6 | 7 | # editor config 8 | .vscode/ 9 | /Debug/ 10 | Release/ -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | MarkovLocalization 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | clean,full,incremental, 11 | 12 | 13 | 14 | 15 | org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder 16 | full,incremental, 17 | 18 | 19 | 20 | 21 | 22 | org.eclipse.cdt.core.cnature 23 | org.eclipse.cdt.core.ccnature 24 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 25 | org.eclipse.cdt.managedbuilder.core.ScannerConfigNature 26 | 27 | 28 | -------------------------------------------------------------------------------- /.settings/language.settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Ramiz Raja 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Markov-Localization (Bayesian Filter) 2 | 3 | Localize a vehicle in a given map and sensors data, using Markov Localization. This code follows a very simple form of 1D Markov Localization. 4 | 5 | 6 | ## Getting Started 7 | 8 | ``` 9 | //declare map: 10 | Map map_1d; 11 | 12 | //declare measurement list: 13 | std::vector measurement_pack_list; 14 | 15 | //declare helpers: 16 | HelpFunctions helper; 17 | 18 | //read map: 19 | helper.ReadMapData("data/map_1d.txt", map_1d); 20 | 21 | //read in data to measurement package list: 22 | helper.ReadMeasurementData( "data/example03/control_data.txt", 23 | "data/example03/observations/", 24 | measurement_pack_list); 25 | 26 | //create instance of 1d_bayesian localization filter: 27 | BayesianFilter localization_1d_bayesian; 28 | 29 | //define number of time steps: 30 | size_t T = measurement_pack_list.size(); 31 | 32 | //cycle: 33 | for (size_t t = 0; t < T; ++t){ 34 | 35 | //Call 1d_bayesian filter: 36 | localization_1d_bayesian.ProcessMeasurement(measurement_pack_list[t], 37 | map_1d, 38 | helper); 39 | } 40 | 41 | ``` 42 | 43 | ## Assumptions 44 | 45 | This code implements 1D Markov localization and assumes that 46 | 47 | - A landmarks based 1D map `m` is given 48 | - The sensor measures the distances to nearest `k` landmarks on the road, in the driving direction. 49 | - The sensor has `standard deviation` of `1m`. 50 | 51 | ## Learn the Math Behind Markov Localization (Bayesian Filter) 52 | 53 | I did take [notes](https://drive.google.com/open?id=0Bxv9kPZMr-zbWFBXUXp4UHRyNzA) when I was learning which (although not very neat) may be helpful. Other than that, there are plenty of resources on the web to learn from. 54 | 55 | 56 | -------------------------------------------------------------------------------- /data/example01/control_data.txt: -------------------------------------------------------------------------------- 1 | 1 2 | 1 3 | 1 4 | 1 5 | 1 6 | 1 7 | 1 8 | 1 9 | 1 10 | 1 11 | 1 12 | 1 13 | 1 14 | 1 -------------------------------------------------------------------------------- /data/example01/gt_example01.txt: -------------------------------------------------------------------------------- 1 | 1.838853e-10 2 | 1.020496e-09 3 | 4.684929e-09 4 | 1.943246e-08 5 | 7.457298e-08 6 | 2.663344e-07 7 | 8.866826e-07 8 | 2.753151e-06 9 | 7.974774e-06 10 | 2.155324e-05 11 | 5.436161e-05 12 | 1.279817e-04 13 | 2.813110e-04 14 | 5.774873e-04 15 | 1.107611e-03 16 | 1.985872e-03 17 | 3.330796e-03 18 | 5.231370e-03 19 | 7.704914e-03 20 | 1.066270e-02 21 | 1.390297e-02 22 | 1.714362e-02 23 | 2.008828e-02 24 | 2.249929e-02 25 | 2.424334e-02 26 | 2.528851e-02 27 | 2.566086e-02 28 | 2.539494e-02 29 | 2.451668e-02 30 | 2.307402e-02 31 | 2.119500e-02 32 | 1.912923e-02 33 | 1.723369e-02 34 | 1.589405e-02 35 | 1.540982e-02 36 | 1.589405e-02 37 | 1.723369e-02 38 | 1.912923e-02 39 | 2.119500e-02 40 | 2.307402e-02 41 | 2.451668e-02 42 | 2.539494e-02 43 | 2.566086e-02 44 | 2.528851e-02 45 | 2.424334e-02 46 | 2.249929e-02 47 | 2.008828e-02 48 | 1.714362e-02 49 | 1.390297e-02 50 | 1.066270e-02 51 | 7.704914e-03 52 | 5.231371e-03 53 | 3.330801e-03 54 | 1.985891e-03 55 | 1.107686e-03 56 | 5.777537e-04 57 | 2.821975e-04 58 | 1.307338e-04 59 | 6.233169e-05 60 | 4.308702e-05 61 | 6.226180e-05 62 | 1.304686e-04 63 | 2.813113e-04 64 | 5.750016e-04 65 | 1.099716e-03 66 | 1.964357e-03 67 | 3.276514e-03 68 | 5.103655e-03 69 | 7.424485e-03 70 | 1.008794e-02 71 | 1.280325e-02 72 | 1.517904e-02 73 | 1.681096e-02 74 | 1.739315e-02 75 | 1.681177e-02 76 | 1.518177e-02 77 | 1.281122e-02 78 | 1.010947e-02 79 | 7.478772e-03 80 | 5.231369e-03 81 | 3.556933e-03 82 | 2.539073e-03 83 | 2.199282e-03 84 | 2.539073e-03 85 | 3.556933e-03 86 | 5.231369e-03 87 | 7.478772e-03 88 | 1.010947e-02 89 | 1.281122e-02 90 | 1.518177e-02 91 | 1.681177e-02 92 | 1.739315e-02 93 | 1.681096e-02 94 | 1.517904e-02 95 | 1.280325e-02 96 | 1.008794e-02 97 | 7.424476e-03 98 | 5.103490e-03 99 | 3.273686e-03 100 | 1.935709e-03 -------------------------------------------------------------------------------- /data/example01/observations/observations_000001.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000001.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000002.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000002.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000003.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000003.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000004.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000004.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000005.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000005.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000006.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000006.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000007.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000007.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000008.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000008.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000009.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000009.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000010.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000010.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000011.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000011.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000012.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000012.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000013.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000013.txt -------------------------------------------------------------------------------- /data/example01/observations/observations_000014.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example01/observations/observations_000014.txt -------------------------------------------------------------------------------- /data/example03/control_data.txt: -------------------------------------------------------------------------------- 1 | 1 2 | 1 3 | 1 4 | 1 5 | 1 6 | 1 7 | 1 8 | 1 9 | 1 10 | 1 11 | 1 12 | 1 13 | 1 14 | 1 -------------------------------------------------------------------------------- /data/example03/gt_example01.txt: -------------------------------------------------------------------------------- 1 | 1.864427e-71 2 | 1.040826e-67 3 | 4.528286e-64 4 | 1.535648e-60 5 | 4.060258e-57 6 | 8.372472e-54 7 | 1.346981e-50 8 | 1.691547e-47 9 | 1.659134e-44 10 | 1.271984e-41 11 | 7.629360e-39 12 | 3.584151e-36 13 | 1.320564e-33 14 | 3.822080e-31 15 | 8.705771e-29 16 | 1.563792e-26 17 | 2.220221e-24 18 | 2.497462e-22 19 | 2.231244e-20 20 | 1.586987e-18 21 | 9.006218e-17 22 | 4.086216e-15 23 | 1.484773e-13 24 | 4.326964e-12 25 | 1.012501e-10 26 | 1.904110e-09 27 | 2.879856e-08 28 | 3.504634e-07 29 | 3.432756e-06 30 | 2.706750e-05 31 | 1.718298e-04 32 | 8.782505e-04 33 | 3.614333e-03 34 | 1.197693e-02 35 | 3.195838e-02 36 | 6.866761e-02 37 | 1.188074e-01 38 | 1.655188e-01 39 | 1.856743e-01 40 | 1.677084e-01 41 | 1.219738e-01 42 | 7.143400e-02 43 | 3.368874e-02 44 | 1.279413e-02 45 | 3.912728e-03 46 | 9.635731e-04 47 | 1.910832e-04 48 | 3.051377e-05 49 | 3.923873e-06 50 | 4.063412e-07 51 | 3.388664e-08 52 | 2.275734e-09 53 | 1.230707e-10 54 | 5.359382e-12 55 | 1.879311e-13 56 | 5.306566e-15 57 | 1.206620e-16 58 | 2.209417e-18 59 | 3.257883e-20 60 | 3.868450e-22 61 | 3.698894e-24 62 | 2.847959e-26 63 | 1.765740e-28 64 | 8.815870e-31 65 | 3.544583e-33 66 | 1.147717e-35 67 | 2.992750e-38 68 | 6.284380e-41 69 | 1.062679e-43 70 | 1.447066e-46 71 | 1.586810e-49 72 | 1.401264e-52 73 | 9.965038e-56 74 | 5.706871e-59 75 | 2.631870e-62 76 | 9.773762e-66 77 | 2.922678e-69 78 | 7.037621e-73 79 | 1.364621e-76 80 | 2.132174e-80 81 | 2.900803e-84 82 | 3.100760e-87 83 | 2.830220e-89 84 | 2.212553e-91 85 | 1.356587e-93 86 | 6.527760e-96 87 | 2.469146e-98 88 | 7.354978e-101 89 | 1.728702e-103 90 | 3.212595e-106 91 | 4.730236e-109 92 | 5.529036e-112 93 | 5.139512e-115 94 | 3.805050e-118 95 | 2.246512e-121 96 | 1.058766e-124 97 | 3.986363e-128 98 | 1.199826e-131 99 | 2.888405e-135 100 | 5.564057e-139 -------------------------------------------------------------------------------- /data/example03/observations/observations_000001.txt: -------------------------------------------------------------------------------- 1 | 4.5 -------------------------------------------------------------------------------- /data/example03/observations/observations_000002.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000002.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000003.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000003.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000004.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000004.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000005.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000005.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000006.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000006.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000007.txt: -------------------------------------------------------------------------------- 1 | 28 -------------------------------------------------------------------------------- /data/example03/observations/observations_000008.txt: -------------------------------------------------------------------------------- 1 | 27 -------------------------------------------------------------------------------- /data/example03/observations/observations_000009.txt: -------------------------------------------------------------------------------- 1 | 26 -------------------------------------------------------------------------------- /data/example03/observations/observations_000010.txt: -------------------------------------------------------------------------------- 1 | 25 -------------------------------------------------------------------------------- /data/example03/observations/observations_000011.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000011.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000012.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000012.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000013.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000013.txt -------------------------------------------------------------------------------- /data/example03/observations/observations_000014.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/informramiz/Markov-Localization/e957b1e529b28c16ad616e9898b77801adc06571/data/example03/observations/observations_000014.txt -------------------------------------------------------------------------------- /data/map_1d.txt: -------------------------------------------------------------------------------- 1 | 1 9 2 | 2 15 3 | 3 25 4 | 4 31 5 | 5 59 6 | 6 77 -------------------------------------------------------------------------------- /src/bayesian_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * basian_filter.cpp 3 | * 4 | * Created on: Apr 26, 2017 5 | * Author: ramiz 6 | */ 7 | 8 | #include "bayesian_filter.h" 9 | 10 | #include 11 | 12 | BayesianFilter::BayesianFilter() { 13 | is_initialized_ = false; 14 | control_std_ = 1.0f; 15 | observation_std_ = 1.0f; 16 | 17 | belief_x_init_.resize(100, 0); 18 | belief_x.resize(100, 0); 19 | } 20 | 21 | BayesianFilter::~BayesianFilter() { 22 | 23 | } 24 | 25 | void BayesianFilter::Init(const Map &map_1d, HelpFunctions &helpers) { 26 | //run over map, all landmark values in map_1d 27 | for (int l = 0; l < map_1d.landmark_list_.size(); ++l) { 28 | //get landmark l from map 29 | Map::SingleLandmark single_landmark = map_1d.landmark_list_[l]; 30 | 31 | //check if landmark position x fits in map[0, 100] 32 | if (!(single_landmark.x >= 0 && single_landmark.x < 100)) { 33 | continue; 34 | } 35 | 36 | //set belief to 1 at position and +/- from position: 37 | int x = single_landmark.x; 38 | belief_x_init_[x] = 1; 39 | if (x > 0) { 40 | belief_x_init_[x - 1] = 1; 41 | } 42 | 43 | if (x < 99) { 44 | belief_x_init_[x + 1] = 1; 45 | } 46 | } //end for loop 47 | 48 | //normalize initial believe 49 | belief_x_init_ = helpers.NormalizeVector(belief_x_init_); 50 | 51 | is_initialized_ = true; 52 | } 53 | 54 | float BayesianFilter::Predict(int i, const MeasurementPackage &measurements, 55 | const Map &map_1d, HelpFunctions &helpers) { 56 | /****************************************************************************** 57 | * motion model update 58 | ******************************************************************************/ 59 | // std::cout << "-->motion model for state x ! \n" << std::endl; 60 | 61 | //get current observations and control information: 62 | MeasurementPackage::Control controls = measurements.control_; 63 | float pose_i = float(i); 64 | 65 | /************************************************************************** 66 | * posterior for motion model 67 | **************************************************************************/ 68 | 69 | // motion posterior: 70 | // used to set up the convlution 71 | float posterior_motion = 0.0f; 72 | 73 | //loop over state space x_t-1 * same size as bel_x (Perform Convolution): 74 | for (int j = 0; j < belief_x_init_.size(); ++j) { 75 | 76 | float pose_j = float(j); 77 | float distance_ij = pose_i - pose_j; 78 | //Calculate transition probabilites using helpers.normpdf() 79 | // x: difference between bel_x index and state space index 80 | // mu: the movement from controls defined above 81 | // std: defined eariler 82 | float transition_model_probability = helpers.Normpdf(distance_ij, 83 | controls.delta_x, control_std_); 84 | 85 | //Calculate motion model 86 | // ADD the transition prob multiplied by the initial believe 87 | // at state space index 88 | 89 | posterior_motion += transition_model_probability * belief_x_init_[j]; 90 | } 91 | return posterior_motion; 92 | } 93 | 94 | float BayesianFilter::Update(int i, const MeasurementPackage &measurements, 95 | const Map &map_1d, HelpFunctions &helpers) { 96 | 97 | MeasurementPackage::Observation observations = measurements.observation_; 98 | 99 | /************************************************************************** 100 | * observation update: 101 | **************************************************************************/ 102 | float pose_i = float(i); 103 | //define pseudo observation vector: 104 | std::vector pseudo_ranges; 105 | 106 | //define maximum distance: 107 | float distance_max = 100; 108 | 109 | //loop over number of landmarks and estimate pseudo ranges: 110 | for (int l = 0; l < map_1d.landmark_list_.size(); ++l) { 111 | 112 | //calculate difference between landmark position 113 | // and current believe state index 114 | float range_l = map_1d.landmark_list_[l].x - pose_i; 115 | 116 | //check, if distances are positive, and store positive range: 117 | if (range_l >= 0.0f) { 118 | pseudo_ranges.push_back(range_l); 119 | } 120 | } 121 | 122 | //sort pseudo range vector: 123 | sort(pseudo_ranges.begin(), pseudo_ranges.end()); 124 | 125 | //define observation posterior: 126 | float posterior_obs = 1.0f; 127 | 128 | //run over current observations vector defined above: 129 | for (int z = 0; z < observations.distances.size(); ++z) { 130 | 131 | //define min distance: 132 | float pseudo_range_min_distance; 133 | 134 | // set min distance either to the closet landmark 135 | // or if no landmarks exist to the maximum set distance 136 | if (pseudo_ranges.size() > 0) { 137 | //set min distance 138 | pseudo_range_min_distance = pseudo_ranges[0]; 139 | //remove this entry 140 | pseudo_ranges.erase(pseudo_ranges.begin()); 141 | } else { 142 | pseudo_range_min_distance = distance_max; 143 | } 144 | 145 | //estimate the posterior for observation model: 146 | // MULTIPLY by normpdf of obseravations distance, 147 | // min distance, and obseravtion_std 148 | posterior_obs *= helpers.Normpdf(observations.distances[z], 149 | pseudo_range_min_distance, observation_std_); 150 | } 151 | 152 | return posterior_obs; 153 | } 154 | 155 | void BayesianFilter::ProcessMeasurement(const MeasurementPackage &measurements, 156 | const Map &map_1d, 157 | HelpFunctions &helpers) { 158 | 159 | /****************************************************************************** 160 | * Set init belief of state vector: 161 | ******************************************************************************/ 162 | if (!is_initialized_) { 163 | Init(map_1d, helpers); 164 | } //end if 165 | 166 | for (int i = 0; i < belief_x.size(); i++) { 167 | /****************************************************************************** 168 | * motion model update 169 | ******************************************************************************/ 170 | float posterior_motion = Predict(i, measurements, map_1d, helpers); 171 | 172 | /****************************************************************************** 173 | * observation model update 174 | ******************************************************************************/ 175 | float posterior_observation = Update(i, measurements, map_1d, helpers); 176 | 177 | //update the belief for Xt_i 178 | belief_x[i] = posterior_observation * posterior_motion; 179 | } 180 | 181 | //normalize bel_x so that sum of all probabilities equals 1 182 | belief_x = helpers.NormalizeVector(belief_x); 183 | 184 | //set initial believe to bel_x for next time 185 | belief_x_init_ = belief_x; 186 | } 187 | -------------------------------------------------------------------------------- /src/bayesian_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * basian_filter.h 3 | * 4 | * Created on: Apr 26, 2017 5 | * Author: ramiz 6 | */ 7 | 8 | #ifndef BAYESIAN_FILTER_H_ 9 | #define BAYESIAN_FILTER_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "measurement_package.h" 16 | #include "map.h" 17 | #include "help_functions.h" 18 | 19 | class BayesianFilter { 20 | public: 21 | //constructor 22 | BayesianFilter(); 23 | 24 | //destructor 25 | virtual ~BayesianFilter(); 26 | 27 | //main public member function, which estimate the beliefs: 28 | void ProcessMeasurement(const MeasurementPackage &measurements, 29 | const Map &map_1d, 30 | HelpFunctions &helpers); 31 | 32 | std::vector belief_x; 33 | 34 | private: 35 | /** 36 | * Initializes the initial belief 37 | * @param map_1d, 1D map of all landmarks 38 | * @helpers, helper functions class reference 39 | */ 40 | void Init(const Map &map_1d, HelpFunctions &helpers); 41 | /** 42 | * Applies motion model to calculate prior Xt (predict Xt) 43 | * @param measurements, list of measurements (controls, observations) 44 | * @param map_1d, 1D map of all landmarks 45 | * @helpers, helper functions class reference 46 | */ 47 | float Predict(int i, const MeasurementPackage &measurements, 48 | const Map &map_1d, 49 | HelpFunctions &helpers); 50 | 51 | /** 52 | * Applies observation model to calculate likelihood of current observation Zt for Xt predicted 53 | * @param measurements, list of measurements (controls, observations) 54 | * @param map_1d, 1D map of all landmarks 55 | * @helpers, helper functions class reference 56 | */ 57 | float Update(int i, const MeasurementPackage &measurements, 58 | const Map &map_1d, 59 | HelpFunctions &helpers); 60 | 61 | bool is_initialized_; 62 | std::vector belief_x_init_; 63 | float control_std_; 64 | float observation_std_; 65 | }; 66 | 67 | 68 | 69 | #endif /* BAYESIAN_FILTER_H_ */ 70 | -------------------------------------------------------------------------------- /src/help_functions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * help_functions.cpp 3 | * 4 | * Created on: Apr 24, 2017 5 | * Author: ramiz 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "help_functions.h" 14 | 15 | HelpFunctions::HelpFunctions() 16 | : ONE_OVER_SQRT_2PI(1/sqrt(2*M_PI)) { 17 | 18 | } 19 | 20 | //definition square: 21 | float HelpFunctions::squared(float x) { 22 | return x*x; 23 | } 24 | 25 | /***************************************************************************** 26 | * normpdf(X,mu,sigma) computes the probability function at values x using the 27 | * normal distribution with mean mu and standard deviation std. x, mue and 28 | * sigma must be scalar! The parameter std must be positive. 29 | * The normal pdf is y=f(x;mu,std)= 1/(std*sqrt(2pi)) e[ -(x−mu)^2 / 2*std^2 ] 30 | *****************************************************************************/ 31 | float HelpFunctions::Normpdf(float x, float mu, float std) { 32 | return (ONE_OVER_SQRT_2PI/std)*exp(-0.5*squared((x-mu)/std)); 33 | } 34 | 35 | //function to normalize a vector: 36 | std::vector HelpFunctions::NormalizeVector(std::vector inputVector){ 37 | 38 | //declare sum: 39 | float sum = 0.0f; 40 | 41 | //declare and resize output vector: 42 | std::vector outputVector ; 43 | outputVector.resize(inputVector.size()); 44 | 45 | //estimate the sum: 46 | for (unsigned int i = 0; i < inputVector.size(); ++i) { 47 | sum += inputVector[i]; 48 | } 49 | 50 | //normalize with sum: 51 | for (unsigned int i = 0; i < inputVector.size(); ++i) { 52 | outputVector[i] = inputVector[i]/sum ; 53 | } 54 | 55 | //return normalized vector: 56 | return outputVector ; 57 | } 58 | 59 | 60 | /* Reads map data from a file. 61 | * @param filename Name of file containing map data. 62 | */ 63 | bool HelpFunctions::ReadMapData(std::string filename, Map& map) { 64 | 65 | // Get file of map: 66 | std::ifstream in_file_map(filename.c_str(),std::ifstream::in); 67 | // Return if we can't open the file. 68 | if (!in_file_map) { 69 | return false; 70 | } 71 | 72 | //declare single line of map file: 73 | std::string line_map; 74 | 75 | //run over each single line: 76 | while(getline(in_file_map, line_map)){ 77 | 78 | std::istringstream iss_map(line_map); 79 | 80 | //declare landmark values and ID: 81 | float landmark_x_f; 82 | int id_i; 83 | 84 | //read data from current line to values:: 85 | iss_map >> id_i ; 86 | iss_map >> landmark_x_f ; 87 | 88 | 89 | //declare single_landmark: 90 | Map::SingleLandmark single_landmark_temp ; 91 | 92 | //set values 93 | single_landmark_temp.id = id_i ; 94 | single_landmark_temp.x = landmark_x_f; 95 | 96 | //push_back in landmark list of map_1d: 97 | map.landmark_list_.push_back(single_landmark_temp); 98 | 99 | } 100 | return true; 101 | } 102 | 103 | 104 | /* Reads measurements from a file. 105 | * @param filename Name of file containing measurement data. 106 | */ 107 | bool HelpFunctions::ReadMeasurementData(std::string filename_control, 108 | std::string filename_obs, 109 | std::vector& measurement_pack_list) { 110 | //get file of measurements: 111 | std::ifstream in_file_control(filename_control.c_str(),std::ifstream::in); 112 | if (!in_file_control) { 113 | return false; 114 | } 115 | //declare single line of measurement file: 116 | std::string line; 117 | 118 | int count = 1 ; 119 | 120 | //run over each single line: 121 | while(getline(in_file_control, line)){ 122 | 123 | //declare measurement package: 124 | MeasurementPackage meas_package; 125 | 126 | std::istringstream iss(line); 127 | 128 | //declare position values: 129 | float delta_x_f; 130 | 131 | //read data from line to values: 132 | iss >> delta_x_f; 133 | 134 | 135 | //set control information: 136 | meas_package.control_.delta_x = delta_x_f ; 137 | 138 | //read observations for each control information: 139 | char str_obs[1024]; 140 | 141 | //define file name of observations for current control/position info: 142 | sprintf(str_obs,"%sobservations_%06i.txt", filename_obs.c_str(), count); 143 | std::string in_file_name_observation = std::string(str_obs); 144 | 145 | 146 | //get file of observations: 147 | std::ifstream in_file_observation(in_file_name_observation.c_str(), 148 | std::ifstream::in); 149 | if (!in_file_observation) { 150 | return false; 151 | } 152 | 153 | std::string line_obs; 154 | 155 | //run over each single line: 156 | while(getline(in_file_observation, line_obs)){ 157 | 158 | std::istringstream iss_obs(line_obs); 159 | 160 | //declare observation values: 161 | float distance_f; 162 | 163 | //read data from line to values: 164 | iss_obs >> distance_f; 165 | 166 | //set observation information: 167 | meas_package.observation_.distances.push_back(distance_f); 168 | } 169 | //push_back single package in measurement list: 170 | measurement_pack_list.push_back(meas_package); 171 | 172 | //increase counter for observation files: 173 | count++; 174 | } 175 | return true; 176 | } 177 | 178 | bool HelpFunctions::CompareData(std::string filename_gt, 179 | std::vector& result_vec) { 180 | /***************************************************************************** 181 | * print/compare results: * 182 | *****************************************************************************/ 183 | //get GT data: 184 | //define file name of map: 185 | 186 | std::vector gt_vec; 187 | 188 | //get file of map: 189 | std::ifstream in_file_gt(filename_gt.c_str(),std::ifstream::in); 190 | 191 | //declare single line of map file: 192 | std::string line_gt; 193 | 194 | //run over each single line: 195 | while(getline(in_file_gt, line_gt)){ 196 | 197 | std::istringstream iss_gt(line_gt); 198 | float gt_value; 199 | 200 | //read data from current line to values:: 201 | iss_gt >> gt_value ; 202 | gt_vec.push_back(gt_value); 203 | 204 | } 205 | float error_sum; 206 | float belief_sum; 207 | error_sum = 0.0f; 208 | belief_sum = 0.0f; 209 | std::cout <<"..................................................."<< std::endl; 210 | std::cout <<"...............----> Results <----................."<< std::endl; 211 | std::cout <<"..................................................."<< std::endl; 212 | 213 | for (unsigned int i = 0; i < result_vec.size(); ++i){ 214 | error_sum+= (gt_vec[i]-result_vec[i])*(gt_vec[i]-result_vec[i]); 215 | belief_sum+= result_vec[i] ; 216 | std::cout << std::fixed << std::setprecision(5) <<"bel_x="<< i <<":" << "\t" 217 | << result_vec[i]<<"\t" 218 | << "ground truth:" << "\t" 219 | << gt_vec[i]<<"\t" << std::endl ; 220 | } 221 | std::cout <<"..................................................."<< std::endl; 222 | std::cout << std::fixed << std::setprecision(5)<< "sum bel:" << "\t" << belief_sum < 12 | #include 13 | #include "map.h" 14 | #include "measurement_package.h" 15 | 16 | class HelpFunctions { 17 | public: 18 | 19 | //definition of one over square root of 2*pi: 20 | const float ONE_OVER_SQRT_2PI; 21 | 22 | //Constructor 23 | HelpFunctions(); 24 | 25 | //definition square: 26 | float squared(float x); 27 | 28 | /***************************************************************************** 29 | * normpdf(X,mu,sigma) computes the probability function at values x using the 30 | * normal distribution with mean mu and standard deviation std. x, mue and 31 | * sigma must be scalar! The parameter std must be positive. 32 | * The normal pdf is y=f(x;mu,std)= 1/(sqrt(2pi * square(std)) * e ^ [ -(x−mu)^2 / 2*std^2 ] 33 | *****************************************************************************/ 34 | float Normpdf(float x, float mu, float std); 35 | 36 | //function to normalize a vector: 37 | std::vector NormalizeVector(std::vector inputVector); 38 | 39 | 40 | /* Reads map data from a file. 41 | * @param filename Name of file containing map data. 42 | */ 43 | bool ReadMapData(std::string filename, Map& map); 44 | 45 | 46 | /* Reads measurements from a file. 47 | * @param filename Name of file containing measurement data. 48 | */ 49 | bool ReadMeasurementData(std::string filename_control, 50 | std::string filename_obs, 51 | std::vector& measurement_pack_list); 52 | 53 | bool CompareData(std::string filename_gt, 54 | std::vector& result_vec); 55 | }; 56 | 57 | 58 | 59 | 60 | #endif /* HELP_FUNCTIONS_H_ */ 61 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : MarkovLocalization.cpp 3 | // Author : Ramiz Raja 4 | // Version : 5 | // Copyright : MIT Licensed 6 | // Description : Hello World in C++, Ansi-style 7 | //============================================================================ 8 | 9 | #include 10 | #include "map.h" 11 | #include "measurement_package.h" 12 | #include "help_functions.h" 13 | #include "bayesian_filter.h" 14 | 15 | using namespace std; 16 | 17 | int main() { 18 | /****************************************************************************** 19 | * declaration: 20 | *****************************************************************************/ 21 | 22 | //define example: 01, 02, 03, 04 23 | string example_string = "01"; 24 | 25 | //declare map: 26 | Map map_1d; 27 | 28 | //declare measurement list: 29 | std::vector measurement_pack_list; 30 | 31 | //declare helpers: 32 | HelpFunctions helper; 33 | 34 | //define file names: 35 | char in_file_name_ctr[1024]; 36 | char in_file_name_obs[1024]; 37 | char in_file_name_gt[1024]; 38 | 39 | /****************************************************************************** 40 | * read map and measurements: * 41 | *****************************************************************************/ 42 | //read map: 43 | helper.ReadMapData("data/map_1d.txt", map_1d); 44 | 45 | //define file name of controls: 46 | sprintf(in_file_name_ctr, "data/example%s/control_data.txt", 47 | example_string.c_str()); 48 | 49 | //define file name of observations: 50 | sprintf(in_file_name_obs, "data/example%s/observations/", 51 | example_string.c_str()); 52 | 53 | //read in data to measurement package list: 54 | helper.ReadMeasurementData(in_file_name_ctr, 55 | in_file_name_obs, 56 | measurement_pack_list); 57 | 58 | /******************************************************************************* 59 | * start 1d_bayesian filter * 60 | *******************************************************************************/ 61 | 62 | //create instance of 1d_bayesian localization filter: 63 | BayesianFilter localization_1d_bayesian; 64 | 65 | //define number of time steps: 66 | size_t T = measurement_pack_list.size(); 67 | 68 | //cycle: 69 | for (size_t t = 0; t < T; ++t){ 70 | 71 | //Call 1d_bayesian filter: 72 | localization_1d_bayesian.ProcessMeasurement(measurement_pack_list[t], 73 | map_1d, 74 | helper); 75 | } 76 | 77 | /******************************************************************************* 78 | * print/compare results: * 79 | ********************************************************************************/ 80 | //define file name of gt data: 81 | sprintf(in_file_name_gt, "data/example%s/gt_example%s.txt",example_string.c_str(),example_string.c_str() ); 82 | 83 | ///compare gt data with results: 84 | helper.CompareData(in_file_name_gt, localization_1d_bayesian.belief_x); 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /src/map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Map 3 | * 4 | * Created on: Apr 24, 2017 5 | * Author: ramiz 6 | */ 7 | 8 | #ifndef MAP_H_ 9 | #define MAP_H_ 10 | 11 | #include 12 | 13 | class Map { 14 | public: 15 | //definition of single landmark: 16 | struct SingleLandmark { 17 | int id; 18 | float x; 19 | }; 20 | 21 | //list of landmarks 22 | std::vector landmark_list_; 23 | }; 24 | 25 | 26 | 27 | #endif /* MAP_H_ */ 28 | -------------------------------------------------------------------------------- /src/measurement_package.h: -------------------------------------------------------------------------------- 1 | /* 2 | * measurement_package.h 3 | * 4 | * Created on: Apr 24, 2017 5 | * Author: ramiz 6 | */ 7 | 8 | #ifndef MEASUREMENT_PACKAGE_H_ 9 | #define MEASUREMENT_PACKAGE_H_ 10 | 11 | #include 12 | 13 | class MeasurementPackage { 14 | public: 15 | struct Control { 16 | float delta_x; // move to successor along x-axis 17 | }; 18 | 19 | struct Observation { 20 | std::vector distances; //distances to observed landmarks 21 | }; 22 | 23 | Control control_; 24 | Observation observation_; 25 | }; 26 | 27 | 28 | #endif /* MEASUREMENT_PACKAGE_H_ */ 29 | --------------------------------------------------------------------------------