├── .gitignore
├── .idea
├── .name
├── codeStyles
│ └── Project.xml
├── misc.xml
├── modules.xml
├── untitled1.iml
├── vcs.xml
└── workspace.xml
├── .travis.yml
├── CMakeLists.txt
├── CMakeLists.txt.in
├── README.md
├── src
├── KalmanFilter.cpp
├── KalmanFilter.h
├── Landmark.cpp
├── Landmark.h
├── Robot.cpp
├── Robot.h
├── defines.h
└── main.cpp
└── tests
└── unit_tests.cpp
/.gitignore:
--------------------------------------------------------------------------------
1 | cmake-build-debug
2 |
3 |
--------------------------------------------------------------------------------
/.idea/.name:
--------------------------------------------------------------------------------
1 | untitled1
--------------------------------------------------------------------------------
/.idea/codeStyles/Project.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 |
27 |
28 |
29 |
--------------------------------------------------------------------------------
/.idea/misc.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/.idea/untitled1.iml:
--------------------------------------------------------------------------------
1 |
2 |
--------------------------------------------------------------------------------
/.idea/vcs.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
--------------------------------------------------------------------------------
/.idea/workspace.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 |
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 | rotating
109 | .y
110 | MatrixXd
111 | VectorXd
112 | phi
113 | x_start
114 | state
115 | final state_new =
116 | inal state_new =
117 | SDL_SCANCODE_UP
118 | contro
119 | measured_landmarks
120 | meas
121 | control =
122 | True x,y,phi:
123 | KF:: state_new
124 | control
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
244 |
248 |
249 |
250 |
252 |
253 |
254 |
255 |
256 |
257 | 1536051927095
258 |
259 |
260 | 1536051927095
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 |
299 |
300 |
301 |
302 |
303 |
304 |
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
328 |
329 |
330 |
331 |
332 |
333 |
334 |
335 |
336 |
337 |
338 | file://$PROJECT_DIR$/src/Robot.cpp
339 | 128
340 |
341 |
342 |
343 | file://$PROJECT_DIR$/src/KalmanFilter.cpp
344 | 204
345 |
346 |
347 |
348 |
349 |
350 |
351 |
352 |
353 |
354 |
355 |
356 | isnan(state(0))
357 | ObjectiveC
358 | EXPRESSION
359 |
360 |
361 |
362 |
363 |
364 |
365 |
366 |
367 |
368 |
369 |
370 |
371 |
372 |
373 |
374 |
375 |
376 |
377 |
378 |
379 |
380 |
381 |
382 |
383 |
384 |
385 |
386 |
387 |
388 |
389 |
390 |
391 |
392 |
393 |
394 |
395 |
396 |
397 |
398 |
399 |
400 |
401 |
402 |
403 |
404 |
405 |
406 |
407 |
408 |
409 |
410 |
411 |
412 |
413 |
414 |
415 |
416 |
417 |
418 |
419 |
420 |
421 |
422 |
423 |
424 |
425 |
426 |
427 |
428 |
429 |
430 |
431 |
432 |
433 |
434 |
435 |
436 |
437 |
438 |
439 |
440 |
441 |
442 |
443 |
444 |
445 |
446 |
447 |
448 |
449 |
450 |
451 |
452 |
453 |
454 |
455 |
456 |
457 |
458 |
459 |
460 |
461 |
462 |
463 |
464 |
465 |
466 |
467 |
468 |
469 |
470 |
471 |
472 |
473 |
474 |
475 |
476 |
477 |
478 |
479 |
480 |
481 |
482 |
483 |
484 |
485 |
486 |
487 |
488 |
489 |
490 |
491 |
492 |
493 |
494 |
495 |
496 |
497 |
498 |
499 |
500 |
501 |
502 |
503 |
504 |
505 |
506 |
507 |
508 |
509 |
510 |
511 |
512 |
513 |
514 |
515 |
516 |
517 |
518 |
519 |
520 |
521 |
522 |
523 |
524 |
525 |
526 |
527 |
528 |
529 |
530 |
531 |
532 |
533 |
534 |
535 |
536 |
537 |
538 |
539 |
540 |
541 |
542 |
543 |
544 |
545 |
546 |
547 |
548 |
549 |
550 |
551 |
552 |
553 |
554 |
555 |
556 |
557 |
558 |
559 |
560 |
561 |
562 |
563 |
564 |
565 |
566 |
567 |
568 |
569 |
570 |
571 |
572 |
573 |
574 |
575 |
576 |
577 |
578 |
579 |
580 |
581 |
582 |
583 |
584 |
585 |
586 |
587 |
588 |
589 |
590 |
591 |
--------------------------------------------------------------------------------
/.travis.yml:
--------------------------------------------------------------------------------
1 | sudo: false
2 |
3 | language:
4 | - cpp
5 |
6 | addons:
7 | apt:
8 | sources:
9 | - ubuntu-toolchain-r-test
10 |
11 | before_install:
12 |
13 | before_install:
14 | - pip install --user cpp-coveralls
15 | - sudo add-apt-repository -y ppa:team-xbmc/ppa
16 | - sudo apt-get update -qq
17 | - sudo apt-get install -y libsdl2-dev
18 |
19 |
20 | script:
21 | - cmake -H. -Bbuild
22 | - cd build
23 | - cmake --build .
24 | - ctest
25 |
26 | after_success:
27 | - coveralls --root .. -E ".*external.*" -E ".*CMakeFiles.*" -E ".*test/.*.cpp.*"
28 |
29 | notifications:
30 | email: false
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.9)
2 | project(untitled1)
3 |
4 | set(CMAKE_CXX_STANDARD 11)
5 |
6 |
7 | find_package(SDL2 REQUIRED)
8 | include_directories(${SDL2_INCLUDE_DIRS})
9 |
10 |
11 | find_package(Eigen3 REQUIRED)
12 | include_directories(${EIGEN3_INCLUDE_DIR})
13 |
14 |
15 | # Download and unpack googletest at configure time
16 | configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt)
17 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
18 | RESULT_VARIABLE result
19 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download )
20 | if(result)
21 | message(FATAL_ERROR "CMake step for googletest failed: ${result}")
22 | endif()
23 | execute_process(COMMAND ${CMAKE_COMMAND} --build .
24 | RESULT_VARIABLE result
25 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download )
26 | if(result)
27 | message(FATAL_ERROR "Build step for googletest failed: ${result}")
28 | endif()
29 |
30 | # Prevent overriding the parent project's compiler/linker
31 | # settings on Windows
32 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
33 |
34 | # Add googletest directly to our build. This defines
35 | # the gtest and gtest_main targets.
36 | add_subdirectory(${CMAKE_BINARY_DIR}/googletest-src
37 | ${CMAKE_BINARY_DIR}/googletest-build
38 | EXCLUDE_FROM_ALL)
39 |
40 | # The gtest/gtest_main targets carry header search path
41 | # dependencies automatically when using CMake 2.8.11 or
42 | # later. Otherwise we have to add them here ourselves.
43 | if (CMAKE_VERSION VERSION_LESS 2.8.11)
44 | include_directories("${gtest_SOURCE_DIR}/include")
45 | endif()
46 |
47 | # Now simply link against gtest or gtest_main as needed. Eg
48 | #add_executable(example example.cpp)
49 | #target_link_libraries(example gtest_main)
50 | add_test(NAME example_test COMMAND example)
51 |
52 | add_executable(untitled1 src/main.cpp src/Robot.cpp src/Robot.h src/defines.h src/KalmanFilter.cpp src/KalmanFilter.h src/Landmark.cpp src/Landmark.h tests/unit_tests.cpp)
53 |
54 |
55 | target_link_libraries(untitled1 gtest_main)
56 | target_link_libraries(untitled1 ${SDL2_LIBRARIES})
57 |
--------------------------------------------------------------------------------
/CMakeLists.txt.in:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.2)
2 |
3 | project(googletest-download NONE)
4 |
5 | include(ExternalProject)
6 | ExternalProject_Add(googletest
7 | GIT_REPOSITORY https://github.com/google/googletest.git
8 | GIT_TAG master
9 | SOURCE_DIR "${CMAKE_BINARY_DIR}/googletest-src"
10 | BINARY_DIR "${CMAKE_BINARY_DIR}/googletest-build"
11 | CONFIGURE_COMMAND ""
12 | BUILD_COMMAND ""
13 | INSTALL_COMMAND ""
14 | TEST_COMMAND ""
15 | )
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | Robot Localization using Extended Kalman Filtering with landmarks
2 |
3 |
4 | [](https://travis-ci.org/jzuern/robot-localization)
5 |
6 | # Prerequesites
7 |
8 | ## Libraries
9 |
10 | - SDL2:
11 | Install using
12 | `$apt install libsdl2-dev`
13 |
14 | - Eigen:
15 | Eigen is a header-only library, i.e. no need to install/compile any code separately. Get it [here](eigen.tuxfamily.org).
16 |
17 |
18 | ## Build system
19 |
20 | We use [cmake](https://cmake.org/) as the build system for this project. If you do not have it installed on your system, do so using
21 | `$apt install cmake`
22 |
23 | Go into the project installation directory and create a build directory:
24 |
25 | `$mkdir build`
26 |
27 | Then, `cd` into the directory and run cmake on the parent directory and build the project:
28 |
29 | `cd build && cmake .. && make`
30 |
31 |
32 |
33 |
34 |
35 |
36 | # How it works
37 |
38 | The Extended Kalman Filter is the nonlinear extension of the standard Kalman Filter
39 |
40 |
41 | *****
42 |
43 | # TODO
44 |
45 | ## style
46 | - Add tests!!
47 | - more README.md
48 |
49 | ## bugs
50 | - covariance explodes
51 |
--------------------------------------------------------------------------------
/src/KalmanFilter.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 04.09.18.
3 | //
4 |
5 | #include "KalmanFilter.h"
6 | #include "Robot.h"
7 | #include /* atan2 */
8 | #include
9 |
10 |
11 | KalmanFilter::KalmanFilter(
12 |
13 | double dt,
14 | const Eigen::MatrixXf& A,
15 | const Eigen::MatrixXf& C,
16 | const Eigen::MatrixXf& Q,
17 | const Eigen::MatrixXf& R,
18 | const Eigen::MatrixXf& covariance)
19 | : A(A), C(C), Q(Q), R(R), P0(covariance),
20 | m(C.rows()), n(A.rows()), dt(dt), initialized(false),
21 | I(n, n), state(n), state_new(n)
22 | {
23 | I.setIdentity();
24 | }
25 |
26 | void KalmanFilter::init(double t0, const Eigen::VectorXf& x0) {
27 | state = x0;
28 | covariance = P0;
29 | this->t0 = t0;
30 | t = t0;
31 | initialized = true;
32 | }
33 |
34 | void KalmanFilter::update(const Eigen::VectorXf& y) {
35 |
36 | if(!initialized)
37 | throw std::runtime_error("Filter is not initialized!");
38 |
39 | state_new = A * state;
40 | covariance = A * covariance * A.transpose() + Q;
41 | K = covariance * C.transpose() * (C * covariance * C.transpose() + R).inverse();
42 |
43 | auto y_hat = C * state_new;
44 | state_new += K * (y - y_hat);
45 | covariance = (I - K*C)*covariance;
46 | state = state_new;
47 |
48 | t += dt;
49 | }
50 |
51 |
52 |
53 | void KalmanFilter::localization_landmarks(const std::vector & observed_landmarks,
54 | const std::vector & true_landmarks,
55 | const Eigen::VectorXf & control)
56 | {
57 |
58 | /* INFO:
59 | *
60 | * x_hat(0) = x
61 | * x_hat(1) = y
62 | * x_hat(2) = phi
63 | *
64 | */
65 |
66 | std::cout << "KF:: state = \n" << state << std::endl;
67 | std::cout << "KF:: control = \n" << control << std::endl;
68 |
69 | float v = control(0);
70 | float omega = control(1);
71 |
72 | float phi = state(2);
73 | float dx, dy, domega;
74 |
75 | if (omega > 1E-2) // robot moves along circle with radius v/omega
76 | {
77 | dx = v / omega * (- sin(phi) + sin(phi + omega*DT));
78 | dy = v / omega * ( cos(phi) - cos(phi + omega*DT));
79 | }
80 | else
81 | {
82 | dx = v*DT*cos(phi);
83 | dy = v*DT*sin(phi);
84 | }
85 | domega = omega * DT;
86 |
87 | // 1 - update state estimate
88 | Eigen::VectorXf state_new(3);
89 | state_new(0) = state(0) + dx;
90 | state_new(1) = state(1) + dy;
91 | state_new(2) = state(2) + domega;
92 |
93 | std::cout << "KF:: state_new = \n" << state_new << std::endl;
94 |
95 |
96 | // Define Jacobian
97 |
98 | float entry_13;
99 | float entry_23;
100 |
101 | if (omega > 1E-2) // robot moves along circle with radius v/omega
102 | {
103 | // entry_13 = v / omega * (- sin(phi) + sin(phi + omega*DT)); // falsch?
104 | // entry_23 = v / omega * ( cos(phi) - cos(phi + omega*DT)); // falsch?
105 | entry_13 = v / omega * ( cos(phi) - cos(phi + omega*DT));
106 | entry_23 = v / omega * ( sin(phi) - sin(phi + omega*DT));
107 | }
108 | else
109 | {
110 | entry_13 = v*DT*sin(phi);
111 | entry_23 = -v*DT*cos(phi);
112 | }
113 |
114 |
115 | Eigen::MatrixXf G(3,3);
116 |
117 | G << 1, 0, entry_13,
118 | 0, 1, entry_23,
119 | 0, 0, 1;
120 |
121 | covariance = G * covariance * G.transpose() + Q;
122 |
123 | // std::cout << "KF:: G = \n" << G << std::endl;
124 | // std::cout << "KF:: Q = \n" << Q << std::endl;
125 | // std::cout << "KF:: R = \n" << R << std::endl;
126 | // std::cout << "KF:: covariance new = \n" << covariance << std::endl;
127 |
128 | // covariances of landmark measurements
129 | float sigma_r = 0.01f;
130 | float sigma_phi = 0.01f;
131 | float sigma_s = 0.001f;
132 |
133 | R << sigma_r, 0, 0,
134 | 0, sigma_phi, 0,
135 | 0, 0, sigma_s;
136 |
137 |
138 | Eigen::VectorXf sum1(3);
139 | Eigen::MatrixXf sum2(3,3);
140 |
141 | sum1 << 0.f, 0.f, 0.f;
142 | sum2 << 0.f, 0.f, 0.f,
143 | 0.f, 0.f, 0.f,
144 | 0.f, 0.f, 0.f;
145 |
146 | int nLandmarks = true_landmarks.size();
147 |
148 |
149 | for (int i = 0; i < nLandmarks; i++)
150 | {
151 | auto true_lm = true_landmarks[i];
152 | auto observed_lm = observed_landmarks[i];
153 |
154 |
155 | // OBSERVED LANDMARKS
156 | Eigen::VectorXf delta_(2);
157 | delta_(0) = observed_lm.pos.x - state_new(0);
158 | delta_(1) = observed_lm.pos.y - state_new(1);
159 |
160 | auto q_ = delta_.dot(delta_);
161 |
162 | Eigen::VectorXf z_i(3);
163 | z_i(0) = sqrt(q_);
164 | z_i(1) = atan2(delta_(1),delta_(0)) - state_new(2);
165 | z_i(2) = i;
166 |
167 |
168 | // TRUE LANDMARKS
169 | Eigen::VectorXf delta(2);
170 | delta(0) = true_lm.pos.x - state_new(0);
171 | delta(1) = true_lm.pos.y - state_new(1);
172 |
173 | auto q = delta.dot(delta);
174 |
175 | Eigen::VectorXf z_hat_i(3);
176 | z_hat_i(0) = sqrt(q);
177 | z_hat_i(1) = atan2(delta(1),delta(0)) - state_new(2);
178 | z_hat_i(2) = i;
179 |
180 | // TEST
181 | // z_i = z_hat_i;
182 |
183 | // END
184 |
185 | Eigen::MatrixXf H_i(3,3);
186 | H_i(0,0) = 1/q * sqrt(q)*delta(0);
187 | H_i(0,1) = -1/q * sqrt(q)*delta(1);
188 | H_i(0,2) = 0;
189 | H_i(1,0) = 1/q * delta(1);
190 | H_i(1,1) = 1/q * delta(0);
191 | H_i(1,2) = -1/q;
192 | H_i(2,0) = 0.;
193 | H_i(2,1) = 0.;
194 | H_i(2,2) = 0.;
195 |
196 |
197 | auto K_i = covariance * H_i.transpose() * (H_i * covariance * H_i.transpose() + R).inverse();
198 |
199 | std::cout << "KF:: H_i = \n" << H_i << std::endl;
200 | std::cout << "KF:: K_i = \n" << K_i << std::endl;
201 | std::cout << "KF:: z_hat_i = \n" << z_hat_i << std::endl;
202 | std::cout << "KF:: z_i = \n" << z_i << std::endl;
203 |
204 |
205 | sum1 += K_i * (z_i - z_hat_i);
206 | sum2 += K_i * H_i;
207 |
208 | }
209 |
210 | state = state_new + sum1;
211 | covariance = (I - sum2)*covariance;
212 |
213 | std::cout << "KF:: final state_new = \n" << state << std::endl;
214 | std::cout << "KF:: final covariance = \n" << covariance << std::endl;
215 | }
216 |
217 |
218 |
219 |
220 | void KalmanFilter::renderSamples(SDL_Renderer * ren)
221 | {
222 | float var_x = covariance(0,0);
223 | float var_y = covariance(1,1);
224 |
225 | float mean_x = state(0);
226 | float mean_y = state(1);
227 |
228 |
229 | unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
230 |
231 | std::default_random_engine generator(seed);
232 | std::normal_distribution dist_x(mean_x, var_x);
233 | std::normal_distribution dist_y(mean_y, var_y);
234 |
235 | int nSamples = 100;
236 | for (int i = 0; i < nSamples; i++)
237 | {
238 | float pos_x = dist_x(generator);
239 | float pos_y = dist_y(generator);
240 |
241 |
242 | SDL_RenderDrawPoint(ren, pos_x, pos_y);
243 |
244 | }
245 |
246 |
247 | }
248 |
249 |
--------------------------------------------------------------------------------
/src/KalmanFilter.h:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 04.09.18.
3 | //
4 |
5 | #ifndef UNTITLED1_KALMANFILTER_H
6 | #define UNTITLED1_KALMANFILTER_H
7 |
8 | #include
9 | #include
10 | #include
11 | #include "Landmark.h"
12 |
13 | #include
14 | #include
15 |
16 | class KalmanFilter {
17 | public:
18 |
19 |
20 | /**
21 | * Create a Kalman filter with the specified matrices.
22 | * A - System dynamics matrix
23 | * C - Output matrix
24 | * Q - Process noise covariance
25 | * R - Measurement noise covariance
26 | * covariance - Estimate error covariance
27 | */
28 |
29 |
30 | KalmanFilter(
31 | double dt,
32 | const Eigen::MatrixXf& A,
33 | const Eigen::MatrixXf& C,
34 | const Eigen::MatrixXf& Q,
35 | const Eigen::MatrixXf& R,
36 | const Eigen::MatrixXf& covariance
37 | );
38 |
39 | /**
40 | * Initialize the filter with a guess for initial states.
41 | */
42 | void init(double t0, const Eigen::VectorXf& x0);
43 |
44 | /**
45 | * Update the estimated state based on measured values. The
46 | * time step is assumed to remain constant.
47 | */
48 | void update(const Eigen::VectorXf& y);
49 |
50 | /**
51 | * Return the current state and time.
52 | */
53 | Eigen::VectorXf get_state() { return state; };
54 |
55 | void renderSamples(SDL_Renderer * ren);
56 |
57 | void localization_landmarks(const std::vector & observed_landmarks,
58 | const std::vector & true_landmarks,
59 | const Eigen::VectorXf & control);
60 |
61 | private:
62 |
63 | // Matrices for computation
64 | Eigen::MatrixXf A, C, Q, R, covariance, K, P0;
65 |
66 | // System dimensions
67 | int m, n;
68 |
69 | // Initial and current time
70 | double t0, t;
71 |
72 | // Discrete time step
73 | double dt;
74 |
75 | // Is the filter initialized?
76 | bool initialized;
77 |
78 | // n-size identity
79 | Eigen::MatrixXf I;
80 |
81 | // Estimated states
82 | Eigen::VectorXf state, state_new;
83 | };
84 |
85 |
86 | #endif //UNTITLED1_KALMANFILTER_H
87 |
--------------------------------------------------------------------------------
/src/Landmark.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 06.09.18.
3 | //
4 |
5 | #include "Landmark.h"
6 |
7 |
8 | Landmark::Landmark(float x, float y, SDL_Color color)
9 | {
10 | pos.x = x;
11 | pos.y = y;
12 | id = color;
13 | }
14 |
15 | Landmark::~Landmark()
16 | {
17 |
18 | }
19 |
20 |
21 | void Landmark::render(SDL_Renderer * ren)
22 | {
23 | int32_t radius = 5;
24 | int32_t x = radius - 1;
25 | int32_t y = 0;
26 | int32_t tx = 1;
27 | int32_t ty = 1;
28 | int32_t err = tx - (radius << 1); // shifting bits left by 1 effectively
29 | // doubles the value. == tx - diameter
30 |
31 | int32_t int_pose_x = int(pos.x);
32 | int32_t int_pose_y = int(pos.y);
33 |
34 |
35 |
36 |
37 | while (x >= y)
38 | {
39 | // Each of the following renders an octant of the circle
40 | SDL_RenderDrawPoint(ren, int_pose_x + x, int_pose_y - y);
41 | SDL_RenderDrawPoint(ren, int_pose_x + x, int_pose_y + y);
42 | SDL_RenderDrawPoint(ren, int_pose_x - x, int_pose_y - y);
43 | SDL_RenderDrawPoint(ren, int_pose_x - x, int_pose_y + y);
44 | SDL_RenderDrawPoint(ren, int_pose_x + y, int_pose_y - x);
45 | SDL_RenderDrawPoint(ren, int_pose_x + y, int_pose_y + x);
46 | SDL_RenderDrawPoint(ren, int_pose_x - y, int_pose_y - x);
47 | SDL_RenderDrawPoint(ren, int_pose_x - y, int_pose_y + x);
48 |
49 | if (err <= 0)
50 | {
51 | y++;
52 | err += ty;
53 | ty += 2;
54 | }
55 | if (err > 0)
56 | {
57 | x--;
58 | tx += 2;
59 | err += tx - (radius << 1);
60 | }
61 | }
62 |
63 | }
--------------------------------------------------------------------------------
/src/Landmark.h:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 06.09.18.
3 | //
4 |
5 | #ifndef UNTITLED1_LANDMARK_H
6 | #define UNTITLED1_LANDMARK_H
7 |
8 | #include
9 |
10 |
11 | typedef struct Position
12 | {
13 | float x, y;
14 | } Position;
15 |
16 |
17 |
18 | class Landmark {
19 | public:
20 | Landmark(float x, float y, SDL_Color id);
21 | ~Landmark();
22 | Position pos;
23 | SDL_Color id;
24 | void render(SDL_Renderer * ren);
25 | };
26 |
27 |
28 | #endif //UNTITLED1_LANDMARK_H
29 |
--------------------------------------------------------------------------------
/src/Robot.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 04.09.18.
3 | //
4 |
5 | #include "Robot.h"
6 | #include "SDL.h"
7 | #include
8 |
9 |
10 | Robot::Robot(int x_start, int y_start, float orientation, int rad, SDL_Color col)
11 | {
12 | // state properties
13 | pose.x = x_start;
14 | pose.y = y_start;
15 | pose.phi = orientation;
16 |
17 | velocity.v = 0.;
18 | velocity.phi = 0.;
19 |
20 | // static properties
21 | radius = rad;
22 | color = col;
23 | }
24 |
25 | Robot::~Robot()
26 | {
27 |
28 | }
29 |
30 |
31 | void Robot::render(SDL_Renderer * ren)
32 | {
33 | int32_t x = radius - 1;
34 | int32_t y = 0;
35 | int32_t tx = 1;
36 | int32_t ty = 1;
37 | int32_t err = tx - (radius << 1); // shifting bits left by 1 effectively
38 | // doubles the value. == tx - diameter
39 |
40 | int32_t int_pose_x = int(pose.x);
41 | int32_t int_pose_y = int(pose.y);
42 |
43 | SDL_SetRenderDrawColor(ren, color.r, color.g, color.b, color.a);
44 |
45 |
46 | while (x >= y)
47 | {
48 | // Each of the following renders an octant of the circle
49 | SDL_RenderDrawPoint(ren, int_pose_x + x, int_pose_y - y);
50 | SDL_RenderDrawPoint(ren, int_pose_x + x, int_pose_y + y);
51 | SDL_RenderDrawPoint(ren, int_pose_x - x, int_pose_y - y);
52 | SDL_RenderDrawPoint(ren, int_pose_x - x, int_pose_y + y);
53 | SDL_RenderDrawPoint(ren, int_pose_x + y, int_pose_y - x);
54 | SDL_RenderDrawPoint(ren, int_pose_x + y, int_pose_y + x);
55 | SDL_RenderDrawPoint(ren, int_pose_x - y, int_pose_y - x);
56 | SDL_RenderDrawPoint(ren, int_pose_x - y, int_pose_y + x);
57 |
58 | if (err <= 0)
59 | {
60 | y++;
61 | err += ty;
62 | ty += 2;
63 | }
64 | if (err > 0)
65 | {
66 | x--;
67 | tx += 2;
68 | err += tx - (radius << 1);
69 | }
70 | }
71 |
72 | int x0 = int_pose_x;
73 | int y0 = int_pose_y;
74 | int x1 = x0 + int(15*(cos(pose.phi)));
75 | int y1 = y0 + int(15*(sin(pose.phi)));
76 |
77 |
78 | SDL_RenderDrawLine(ren, x0, y0, x1, y1);
79 |
80 |
81 | }
82 |
83 |
84 | void Robot::move(const Uint8 * state, Eigen::VectorXf & control)
85 | {
86 | // preallocate control vector
87 | control(0) = 0.f;
88 | control(1) = 0.f;
89 |
90 |
91 | if (state[SDL_SCANCODE_RIGHT])
92 | {
93 |
94 | printf("rotating right\n");
95 | rotateRight(control);
96 | }
97 |
98 |
99 | if (state[SDL_SCANCODE_LEFT])
100 | {
101 | printf("rotating left\n");
102 | rotateLeft(control);
103 | }
104 |
105 |
106 | if (state[SDL_SCANCODE_UP])
107 | {
108 | printf("moving forward\n");
109 | moveForward(control);
110 | }
111 |
112 |
113 | if (state[SDL_SCANCODE_DOWN])
114 | {
115 | printf("moving backward\n");
116 | moveBackward(control);
117 | }
118 |
119 | }
120 |
121 |
122 |
123 | void Robot::moveForward(Eigen::VectorXf & control) {
124 |
125 | velocity.v = 1*DT;
126 |
127 | control(0) = velocity.v;
128 |
129 | pose.x += velocity.v * cos(pose.phi);
130 | pose.y += velocity.v * sin(pose.phi);
131 | }
132 |
133 | void Robot::moveBackward(Eigen::VectorXf & control){
134 |
135 | velocity.v = - 1*DT;
136 |
137 | control(0) = velocity.v;
138 |
139 | pose.x += velocity.v * cos(pose.phi);
140 | pose.y += velocity.v * sin(pose.phi);
141 | }
142 |
143 |
144 | void Robot::rotateLeft(Eigen::VectorXf & control) {
145 |
146 |
147 | velocity.phi = - DT * 2*M_PI/360;
148 |
149 | control(1) = velocity.phi;
150 |
151 | pose.phi += velocity.phi;
152 |
153 |
154 | }
155 |
156 | void Robot::rotateRight(Eigen::VectorXf & control) {
157 |
158 | velocity.phi = DT * 2*M_PI/360;
159 |
160 | control(1) = velocity.phi;
161 |
162 | pose.phi += velocity.phi;
163 | }
164 |
165 |
166 | void Robot::setPose(float x, float y, float phi) {
167 |
168 | pose.x = x;
169 | pose.y = y;
170 | pose.phi = phi;
171 | }
172 |
173 |
174 |
175 | std::vector Robot::measureLandmarks(std::vector landmarks)
176 | {
177 | std::vector measured_landmarks;
178 |
179 | uint32_t time_ui = uint32_t( time(NULL) );
180 | srand( time_ui );
181 |
182 | for (auto lm = landmarks.begin(); lm != landmarks.end(); ++lm)
183 | {
184 | // Define random generator with Gaussian distribution
185 |
186 | float rx = .001*static_cast (rand()) / static_cast (RAND_MAX);
187 | float ry = .001*static_cast (rand()) / static_cast (RAND_MAX);
188 |
189 | // rx = 0.f;
190 | // ry = 0.f;
191 |
192 | float est_pos_x = rx + lm->pos.x;
193 | float est_pos_y = ry + lm->pos.y;
194 |
195 | measured_landmarks.push_back(Landmark(est_pos_x,est_pos_y,lm->id));
196 | }
197 |
198 | return measured_landmarks;
199 |
200 | }
201 |
202 |
203 |
204 | Eigen::VectorXf Robot::get_state()
205 | {
206 | Eigen::VectorXf state(3);
207 |
208 | // without PHI
209 | state(0) = this->pose.x;
210 | state(1) = this->pose.y;
211 | state(2) = this->pose.phi;
212 |
213 |
214 | return state;
215 | }
216 |
--------------------------------------------------------------------------------
/src/Robot.h:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 04.09.18.
3 | //
4 |
5 | #ifndef UNTITLED1_ROBOT_H
6 | #define UNTITLED1_ROBOT_H
7 |
8 | #include
9 | #include "Landmark.h"
10 | #include
11 | #include
12 | #include "defines.h"
13 |
14 |
15 | typedef struct Pose
16 | {
17 | float x, y;
18 | float phi;
19 | } Pose;
20 |
21 |
22 |
23 | typedef struct Velocity
24 | {
25 | float v;
26 | float phi;
27 | } Velocity;
28 |
29 |
30 | class Robot {
31 | public:
32 | Robot(int x_start, int y_start, float orientation, int radius, SDL_Color col);
33 | ~Robot();
34 | Pose pose;
35 | Velocity velocity;
36 | SDL_Color color;
37 |
38 | int radius;
39 | void render(SDL_Renderer * ren);
40 |
41 | void move(const Uint8 * , Eigen::VectorXf & control);
42 |
43 | void moveForward(Eigen::VectorXf & control);
44 | void moveBackward(Eigen::VectorXf & control);
45 | void rotateLeft(Eigen::VectorXf & control);
46 | void rotateRight(Eigen::VectorXf & control);
47 | void setPose(float x, float y, float phi);
48 |
49 | Eigen::VectorXf get_state();
50 |
51 | std::vector measureLandmarks(std::vector landmarks);
52 |
53 | private:
54 |
55 |
56 | };
57 |
58 |
59 | #endif //UNTITLED1_ROBOT_H
60 |
--------------------------------------------------------------------------------
/src/defines.h:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 04.09.18.
3 | //
4 |
5 | #ifndef DEFINES_H
6 | #define DEFINES_H
7 |
8 | #include "cstring"
9 |
10 | #define XSTART 100
11 | #define YSTART 100
12 |
13 | #define WWIDTH 640
14 | #define WHEIGHT 480
15 | #define DT 1
16 |
17 |
18 |
19 | #endif //DEFINES_H
20 |
--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 |
4 | #include "Eigen/Dense"
5 | #include
6 | #include "Robot.h"
7 | #include "Landmark.h"
8 | #include "KalmanFilter.h"
9 |
10 |
11 | std::vector createLandmarks()
12 | {
13 | std::vector lmks;
14 |
15 | SDL_Color color_red = {.r = 255, .g = 0, .b = 0, .a = 255 };
16 | SDL_Color color_green = {.r = 0, .g = 255, .b = 0, .a = 255 };
17 | SDL_Color color_blue = {.r = 0, .g = 0, .b = 255, .a = 255 };
18 | SDL_Color color_purple = {.r = 255, .g = 0, .b = 255, .a = 255 };
19 |
20 | lmks.push_back( Landmark(300.,300.,color_red));
21 | lmks.push_back( Landmark(124.,478.,color_blue));
22 | // lmks.push_back( Landmark(214.,312.,color_purple));
23 |
24 | return lmks;
25 | }
26 |
27 |
28 | int main() {
29 |
30 | if (SDL_Init(SDL_INIT_VIDEO) != 0){
31 | std::cout << "SDL_Init Error: " << SDL_GetError() << std::endl;
32 | return 1;
33 | }
34 |
35 |
36 | SDL_Window *win = SDL_CreateWindow("Robot Program", XSTART, YSTART, WWIDTH, WHEIGHT, SDL_WINDOW_SHOWN);
37 | if (win == nullptr){
38 | std::cout << "SDL_CreateWindow Error: " << SDL_GetError() << std::endl;
39 | SDL_Quit();
40 | return 1;
41 | }
42 |
43 | SDL_Renderer *ren = SDL_CreateRenderer(win, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
44 | if (ren == nullptr){
45 | SDL_DestroyWindow(win);
46 | std::cout << "SDL_CreateRenderer Error: " << SDL_GetError() << std::endl;
47 | SDL_Quit();
48 | return 1;
49 | }
50 |
51 |
52 | std::vector landmarks = createLandmarks();
53 |
54 | SDL_Color orange {.r=255, .g=165, .b=0, .a=255};
55 | SDL_Color red {.r=255, .g=0, .b=0, .a=255};
56 | SDL_Color gray {.r=128, .g=128, .b=128, .a=255};
57 |
58 | Robot robby(200, 200, 0.0, 20, red);
59 |
60 | // Kalman filter stuff
61 | int n = 3; // number of state variables (x,y,phi)
62 | int m = 3; // number of measurements (landmark x, y, index)
63 |
64 | // Best guess of initial states
65 | Eigen::VectorXf x0(n); //[x, y, phi]
66 | x0 << 200., 200., 0.0;
67 | Robot robby_estimate(x0(0), x0(1), x0(2), 18, gray);
68 |
69 | // control vector:
70 | Eigen::VectorXf control(2); // [v, omega]
71 |
72 |
73 | Eigen::MatrixXf A(n, n); // System dynamics matrix
74 | Eigen::MatrixXf C(m, n); // Output matrix
75 | Eigen::MatrixXf Q(n, n); // Process noise covariance
76 | Eigen::MatrixXf R(m, m); // Measurement noise covariance
77 | Eigen::MatrixXf covariance(n, n); // Estimate error covariance
78 |
79 |
80 | // Reasonable covariance matrices
81 | covariance << 5., .0, .0,
82 | .0, 5., .0,
83 | .0, .0, 5.;
84 |
85 | R << 1.0, 0., 0.,
86 | 0., 1.0, 0.,
87 | 0., 0., 0.1;
88 |
89 | Q << 0.1, 0.1, 0.1,
90 | 0.1, 0.1, 0.1,
91 | 0.1, 0.1, 0.1;
92 |
93 | KalmanFilter kf(DT, A, C, Q, R, covariance);
94 |
95 | float t0 = 0.0;
96 | kf.init(t0, x0);
97 |
98 | // rendering loop
99 | int i = 0;
100 | while (i < 10000) {
101 |
102 | //First clear the renderer
103 | SDL_RenderClear(ren);
104 |
105 | //Draw the texture
106 | SDL_SetRenderDrawColor(ren, 200, 200, 255, 255);
107 |
108 | SDL_RenderClear(ren); // fill the scene with white
109 |
110 | SDL_SetRenderDrawColor(ren, 0, 0, 0, 255);
111 |
112 | // update renderer
113 | SDL_RenderPresent(ren);
114 |
115 | // move robot
116 | SDL_PumpEvents();
117 | const Uint8 *key_pressed = SDL_GetKeyboardState(NULL);
118 | robby.move(key_pressed, control);
119 |
120 | // measure landmark positions
121 | auto observed_landmarks = robby.measureLandmarks(landmarks);
122 |
123 | // get robot state
124 | Eigen::VectorXf state = robby.get_state();
125 |
126 | // Localize via Landmarks
127 | kf.localization_landmarks(observed_landmarks, landmarks, control);
128 |
129 | // get estimated state (x,y,phi) as estimated by the EKF
130 | auto x_hat = kf.get_state();
131 |
132 | printf("True x,y,phi: %f, %f, %f\n", state(0), state(1),state(2));
133 | printf("Estimated x,y,phi: %f, %f, %f\n", x_hat(0), x_hat(1), x_hat(2));
134 |
135 | // move robby estimate to new pose as calculated by kalman filter:
136 | robby_estimate.setPose(x_hat(0), x_hat(1), x_hat(2));
137 | robby_estimate.render(ren);
138 |
139 | // render robot
140 | robby.render(ren);
141 |
142 | // render landmarks
143 | for (auto lm = landmarks.begin(); lm != landmarks.end(); ++lm)
144 | {
145 | SDL_SetRenderDrawColor(ren, lm->id.r, lm->id.g, lm->id.b, lm->id.a);
146 | lm->render(ren);
147 | }
148 |
149 | SDL_SetRenderDrawColor(ren, gray.r, gray.g, gray.b, gray.a);
150 |
151 | // render sampled probability distribution
152 | kf.renderSamples(ren);
153 |
154 | SDL_RenderPresent(ren);
155 |
156 | //Take a quick break after all that hard work
157 | SDL_Delay(30);
158 |
159 |
160 | if (key_pressed[SDL_SCANCODE_RETURN])
161 | {
162 | printf("Exiting Program!\n");
163 | break;
164 | }
165 |
166 | i+=1;
167 | }
168 |
169 | SDL_DestroyRenderer(ren);
170 | SDL_DestroyWindow(win);
171 | SDL_Quit();
172 |
173 |
174 | return EXIT_SUCCESS;
175 | }
--------------------------------------------------------------------------------
/tests/unit_tests.cpp:
--------------------------------------------------------------------------------
1 | //
2 | // Created by jannik on 15.09.18.
3 | //
4 |
5 | #include
6 | #include "../src/Robot.h"
7 |
8 |
9 | TEST(TestSuite, RobotTest)
10 | {
11 |
12 | SDL_Color red {.r=255, .g=0, .b=0, .a=255};
13 | Robot robby(200, 200, 0.0, 20, red);
14 |
15 | EXPECT_EQ(1000, 1000);
16 | }
17 |
18 |
--------------------------------------------------------------------------------