├── .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 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
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 |
83 |
84 |
85 |
86 |
87 |
88 |
89 |
90 |
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 |
--------------------------------------------------------------------------------