├── .github └── workflows │ └── c-cpp.yml ├── .gitignore ├── .vscode └── launch.json ├── CMakeLists.txt ├── README.md ├── bin ├── cbia.lib.blas.dyn.rel.x64.12.dll ├── cbia.lib.blas.dyn.rel.x86.12.dll ├── cbia.lib.lapack.dyn.rel.x64.12.dll ├── cbia.lib.lapack.dyn.rel.x86.12.dll └── clean.bat ├── docs ├── html │ ├── Kalman_concept1.jpg │ ├── Kalman_concept2.jpg │ ├── _formulas.aux │ ├── _formulas.dvi │ ├── _formulas.log │ ├── _formulas.pdf │ ├── _formulas.tex │ ├── annotated.html │ ├── annotated_dup.js │ ├── bc_s.png │ ├── bdwn.png │ ├── class_e_k_f-members.html │ ├── class_e_k_f.html │ ├── class_e_k_f.js │ ├── class_e_k_f2-members.html │ ├── class_e_k_f2.html │ ├── class_e_k_f2.js │ ├── class_f_x-members.html │ ├── class_f_x.html │ ├── class_f_x.js │ ├── class_k_f-members.html │ ├── class_k_f.html │ ├── class_k_f.js │ ├── class_u_k_f-members.html │ ├── class_u_k_f.html │ ├── class_u_k_f.js │ ├── classes.html │ ├── closed.png │ ├── dir_6f9635d4fbfa321d4c00f29df5cd540c.html │ ├── dir_6f9635d4fbfa321d4c00f29df5cd540c.js │ ├── dir_dc133b73f91f0e3b4287924ed00a2a00.html │ ├── dir_dc133b73f91f0e3b4287924ed00a2a00.js │ ├── doc.png │ ├── doxygen.css │ ├── doxygen.svg │ ├── dynsections.js │ ├── ekf2_8cpp.html │ ├── ekf2_8h.html │ ├── ekf2_8h.js │ ├── ekf2_8h_source.html │ ├── ekf_8cpp.html │ ├── ekf_8h.html │ ├── ekf_8h.js │ ├── ekf_8h_source.html │ ├── ex1.png │ ├── ex10.png │ ├── ex4.png │ ├── ex7.png │ ├── files.html │ ├── files_dup.js │ ├── folderclosed.png │ ├── folderopen.png │ ├── functions.html │ ├── functions_func.html │ ├── functions_vars.html │ ├── fx_8cpp.html │ ├── fx_8h.html │ ├── fx_8h.js │ ├── fx_8h_source.html │ ├── globals.html │ ├── globals_defs.html │ ├── globals_func.html │ ├── index.html │ ├── jquery.js │ ├── kf_8cpp.html │ ├── kf_8h.html │ ├── kf_8h.js │ ├── kf_8h_source.html │ ├── main10_8cpp.html │ ├── main10_8cpp.js │ ├── main11_8cpp.html │ ├── main11_8cpp.js │ ├── main1_8cpp.html │ ├── main1_8cpp.js │ ├── main2_8cpp.html │ ├── main2_8cpp.js │ ├── main3_8cpp.html │ ├── main3_8cpp.js │ ├── main4_8cpp.html │ ├── main4_8cpp.js │ ├── main5_8cpp.html │ ├── main5_8cpp.js │ ├── main6_8cpp.html │ ├── main6_8cpp.js │ ├── main7_8cpp.html │ ├── main7_8cpp.js │ ├── main8_8cpp.html │ ├── main8_8cpp.js │ ├── main9_8cpp.html │ ├── main9_8cpp.js │ ├── mainpage_8dox.html │ ├── menu.js │ ├── menudata.js │ ├── nav_f.png │ ├── nav_g.png │ ├── nav_h.png │ ├── navtree.css │ ├── navtree.js │ ├── navtreedata.js │ ├── navtreeindex0.js │ ├── open.png │ ├── resize.js │ ├── splitbar.png │ ├── sync_off.png │ ├── sync_on.png │ ├── tab_a.png │ ├── tab_b.png │ ├── tab_h.png │ ├── tab_s.png │ ├── tabs.css │ ├── ukf_8cpp.html │ ├── ukf_8h.html │ ├── ukf_8h.js │ └── ukf_8h_source.html └── index.html ├── doxyconfig ├── images ├── Kalman_concept1.jpg ├── Kalman_concept2.jpg ├── ex1.png ├── ex10.png ├── ex2.png ├── ex3.png ├── ex4.png ├── ex5.png ├── ex6_outputs.png ├── ex6_states.png └── ex7.png ├── kalman-cpp.kdev4 ├── kalman ├── ekf.cpp ├── ekf.h ├── ekf2.cpp ├── ekf2.h ├── fx.cpp ├── fx.h ├── kf.cpp ├── kf.h ├── ukf.cpp └── ukf.h ├── m-files ├── plot_data1.m ├── plot_data10.m ├── plot_data11.m ├── plot_data2.m ├── plot_data3.m ├── plot_data4.m ├── plot_data5.m ├── plot_data6.m ├── plot_data7.m └── plot_data8.m ├── mainpage.dox ├── samples ├── main0.cpp ├── main1.cpp ├── main10.cpp ├── main11.cpp ├── main2.cpp ├── main3.cpp ├── main4.cpp ├── main5.cpp ├── main6.cpp ├── main7.cpp ├── main8.cpp └── main9.cpp ├── vs2019 ├── clean.bat ├── demo0.vcxproj ├── demo1.vcxproj ├── demo10.vcxproj ├── demo11.vcxproj ├── demo2.vcxproj ├── demo3.vcxproj ├── demo4.vcxproj ├── demo5.vcxproj ├── demo6.vcxproj ├── demo7.vcxproj ├── demo8.vcxproj ├── demo9.vcxproj ├── kalman-cpp.sln └── kalman-cpp.vcxproj └── windows-libs ├── Extract windows-libs-2019 here.txt ├── clean.bat └── windows-libs-2019.zip /.github/workflows/c-cpp.yml: -------------------------------------------------------------------------------- 1 | name: C/C++ CI 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build: 11 | 12 | runs-on: ubuntu-latest 13 | 14 | steps: 15 | - uses: actions/checkout@v2 16 | - name: install libarmadillo-dev 17 | run: sudo apt-get update && sudo apt-get install -yq libarmadillo-dev 18 | - name: run cmake 19 | run: cmake . 20 | - name: run make 21 | run: make 22 | 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .kdev4/ 2 | build/CMakeFiles/ 3 | cmake_install.cmake 4 | CMakeCache.txt 5 | log*.* 6 | Makefile 7 | simple_kalman 8 | *.a 9 | *.lib 10 | *.exp 11 | *.user 12 | *.suo 13 | *.ncb 14 | vs2015/kalman-cpp.sdf 15 | vs2019/.vs 16 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": 4 | [ 5 | { 6 | "name": "(Windows) Launch", 7 | "type": "cppvsdbg", 8 | "request": "launch", 9 | "program": "${workspaceFolder}/bin/debug/demo1.exe", 10 | "args": [], 11 | "stopAtEntry": false, 12 | "cwd": "${workspaceFolder}/bin/", 13 | "environment": [], 14 | "console": "externalTerminal" 15 | } 16 | ] 17 | } 18 | 19 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | project(kalman-cpp) 4 | 5 | if (CMAKE_GENERATOR MATCHES "Visual Studio") 6 | # Do Visual Studio specific stuff 7 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ../bin) 8 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ../bin) 9 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ../bin) 10 | 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 12 | 13 | set(CMAKE_BUILD_TYPE Debug) 14 | 15 | link_directories(${CMAKE_SOURCE_DIR}/windows-libs) 16 | add_library (kalman-cpp ./kalman/kf.cpp ./kalman/ekf.cpp ./kalman/ekf2.cpp ./kalman/ukf.cpp ./kalman/fx.cpp) 17 | 18 | target_include_directories (kalman-cpp PUBLIC ./kalman ./windows-libs/armadillo/include) 19 | 20 | add_executable(demo1 ./samples/main1.cpp) 21 | add_executable(demo2 ./samples/main2.cpp) 22 | add_executable(demo3 ./samples/main3.cpp) 23 | add_executable(demo4 ./samples/main4.cpp) 24 | add_executable(demo5 ./samples/main5.cpp) 25 | add_executable(demo6 ./samples/main6.cpp) 26 | add_executable(demo7 ./samples/main7.cpp) 27 | add_executable(demo8 ./samples/main8.cpp) 28 | add_executable(demo9 ./samples/main9.cpp) 29 | add_executable(demo10 ./samples/main10.cpp) 30 | add_executable(demo11 ./samples/main11.cpp) 31 | 32 | install(TARGETS demo1 RUNTIME DESTINATION ../bin) 33 | install(TARGETS demo2 RUNTIME DESTINATION ../bin) 34 | install(TARGETS demo3 RUNTIME DESTINATION ../bin) 35 | install(TARGETS demo4 RUNTIME DESTINATION ../bin) 36 | install(TARGETS demo5 RUNTIME DESTINATION ../bin) 37 | install(TARGETS demo6 RUNTIME DESTINATION ../bin) 38 | install(TARGETS demo7 RUNTIME DESTINATION ../bin) 39 | install(TARGETS demo8 RUNTIME DESTINATION ../bin) 40 | install(TARGETS demo9 RUNTIME DESTINATION ../bin) 41 | install(TARGETS demo10 RUNTIME DESTINATION ../bin) 42 | install(TARGETS demo11 RUNTIME DESTINATION ../bin) 43 | 44 | target_link_libraries (demo1 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 45 | target_link_libraries (demo2 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 46 | target_link_libraries (demo3 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 47 | target_link_libraries (demo4 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 48 | target_link_libraries (demo5 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 49 | target_link_libraries (demo6 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 50 | target_link_libraries (demo7 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 51 | target_link_libraries (demo8 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 52 | target_link_libraries (demo9 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 53 | target_link_libraries (demo10 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 54 | target_link_libraries (demo11 LINK_PUBLIC kalman-cpp cbia.lib.lapack.dyn.rel.x64.12 cbia.lib.blas.dyn.rel.x64.12) 55 | 56 | else() 57 | # Other stuff 58 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ../bin) 59 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ../bin) 60 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ../bin) 61 | 62 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 63 | 64 | set(CMAKE_BUILD_TYPE Debug) 65 | 66 | add_library (kalman-cpp ./kalman/kf.cpp ./kalman/ekf.cpp ./kalman/ekf2.cpp ./kalman/ukf.cpp ./kalman/fx.cpp) 67 | 68 | target_include_directories (kalman-cpp PUBLIC ./kalman) 69 | 70 | add_executable(demo1 ./samples/main1.cpp) 71 | add_executable(demo2 ./samples/main2.cpp) 72 | add_executable(demo3 ./samples/main3.cpp) 73 | add_executable(demo4 ./samples/main4.cpp) 74 | add_executable(demo5 ./samples/main5.cpp) 75 | add_executable(demo6 ./samples/main6.cpp) 76 | add_executable(demo7 ./samples/main7.cpp) 77 | add_executable(demo8 ./samples/main8.cpp) 78 | add_executable(demo9 ./samples/main9.cpp) 79 | add_executable(demo10 ./samples/main10.cpp) 80 | add_executable(demo11 ./samples/main11.cpp) 81 | 82 | install(TARGETS demo1 RUNTIME DESTINATION ../bin) 83 | install(TARGETS demo2 RUNTIME DESTINATION ../bin) 84 | install(TARGETS demo3 RUNTIME DESTINATION ../bin) 85 | install(TARGETS demo4 RUNTIME DESTINATION ../bin) 86 | install(TARGETS demo5 RUNTIME DESTINATION ../bin) 87 | install(TARGETS demo6 RUNTIME DESTINATION ../bin) 88 | install(TARGETS demo7 RUNTIME DESTINATION ../bin) 89 | install(TARGETS demo8 RUNTIME DESTINATION ../bin) 90 | install(TARGETS demo9 RUNTIME DESTINATION ../bin) 91 | install(TARGETS demo10 RUNTIME DESTINATION ../bin) 92 | install(TARGETS demo11 RUNTIME DESTINATION ../bin) 93 | 94 | target_link_libraries (demo1 LINK_PUBLIC kalman-cpp armadillo) 95 | target_link_libraries (demo2 LINK_PUBLIC kalman-cpp armadillo) 96 | target_link_libraries (demo3 LINK_PUBLIC kalman-cpp armadillo) 97 | target_link_libraries (demo4 LINK_PUBLIC kalman-cpp armadillo) 98 | target_link_libraries (demo5 LINK_PUBLIC kalman-cpp armadillo) 99 | target_link_libraries (demo6 LINK_PUBLIC kalman-cpp armadillo) 100 | target_link_libraries (demo7 LINK_PUBLIC kalman-cpp armadillo) 101 | target_link_libraries (demo8 LINK_PUBLIC kalman-cpp armadillo) 102 | target_link_libraries (demo9 LINK_PUBLIC kalman-cpp armadillo) 103 | target_link_libraries (demo10 LINK_PUBLIC kalman-cpp armadillo) 104 | target_link_libraries (demo11 LINK_PUBLIC kalman-cpp armadillo) 105 | 106 | endif() 107 | 108 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kalman-cpp 2 | 3 | [![C/C++ CI](https://github.com/auralius/kalman-cpp/actions/workflows/c-cpp.yml/badge.svg)](https://github.com/auralius/kalman-cpp/actions/workflows/c-cpp.yml) 4 | 5 | ## Kalman filter and extended Kalman filter implementation in C++ 6 | 7 | Implemented filters so far: 8 | * Kalman filter 9 | * Extended Kalman filter 10 | * Second-order extended Kalman filter 11 | * Unscented Kalman filter 12 | 13 | Please use cmake to build all the codes. 14 | 15 | The steps to compile are: 16 | 17 | ``` 18 | mkdir build 19 | cd build 20 | cmake .. 21 | make 22 | ``` 23 | 24 | ## Windows System 25 | 26 | In a Windows system, a Visual Studio solution file (VS 2019) is provided. 27 | 28 | ## Dependencies 29 | 30 | This library utilizes [Armadillo](http://arma.sourceforge.net). 31 | In Windows system, the armadillo library is provided in "windows-libs" folder. 32 | **The contents of windows-libs.zip need to be first extracted.** 33 | Armadillo itself is very easy to use. 34 | More information on the Armadillo can be found [here](http://arma.sourceforge.net/docs.html). 35 | 36 | ## blas and lapack 37 | By default, now kalman-cpp uses blas and lapack. For Windows machine, working with blas and lapack is a messy stuff. Thus, we will use the precompiled blas and lapack from: https://www.fi.muni.cz/~xsvobod2/misc/lapack/. 38 | 39 | The precompiled blas and lapack libraries are included in **windows-libs.zip**. There are four LIB files. Additionally, in "bin" folder, there are four corresponding DLL files as well. There are four files because two files are for the 32-bit platform, and the other two files are for the 64-bit platform. 40 | 41 | **The compiled binary must always be located in the same folder as these DLL files.** 42 | 43 | 44 | 45 | ## MATLAB m-files for plotting 46 | 47 | MATLAB m-files for each example are provided in 'm-files' folder. Octave can also be used instead of MATLAB. 48 | 49 | ## Documentation 50 | 51 | https://auralius.github.io/kalman-cpp/ 52 | -------------------------------------------------------------------------------- /bin/cbia.lib.blas.dyn.rel.x64.12.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/bin/cbia.lib.blas.dyn.rel.x64.12.dll -------------------------------------------------------------------------------- /bin/cbia.lib.blas.dyn.rel.x86.12.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/bin/cbia.lib.blas.dyn.rel.x86.12.dll -------------------------------------------------------------------------------- /bin/cbia.lib.lapack.dyn.rel.x64.12.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/bin/cbia.lib.lapack.dyn.rel.x64.12.dll -------------------------------------------------------------------------------- /bin/cbia.lib.lapack.dyn.rel.x86.12.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/bin/cbia.lib.lapack.dyn.rel.x86.12.dll -------------------------------------------------------------------------------- /bin/clean.bat: -------------------------------------------------------------------------------- 1 | del *.exe 2 | del *.user 3 | del *.ncb 4 | del *.pdb 5 | del *.lib 6 | del *.ilk 7 | del *.txt 8 | del *.exp 9 | del *.suo -------------------------------------------------------------------------------- /docs/html/Kalman_concept1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/Kalman_concept1.jpg -------------------------------------------------------------------------------- /docs/html/Kalman_concept2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/Kalman_concept2.jpg -------------------------------------------------------------------------------- /docs/html/_formulas.aux: -------------------------------------------------------------------------------- 1 | \relax 2 | -------------------------------------------------------------------------------- /docs/html/_formulas.dvi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/_formulas.dvi -------------------------------------------------------------------------------- /docs/html/_formulas.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/_formulas.pdf -------------------------------------------------------------------------------- /docs/html/_formulas.tex: -------------------------------------------------------------------------------- 1 | \documentclass{article} 2 | \usepackage{ifthen} 3 | \usepackage{epsfig} 4 | \usepackage[utf8]{inputenc} 5 | % Packages requested by user 6 | \usepackage{amsmath} 7 | 8 | \usepackage{newunicodechar} 9 | \newunicodechar{⁻}{${}^{-}$}% Superscript minus 10 | \newunicodechar{²}{${}^{2}$}% Superscript two 11 | \newunicodechar{³}{${}^{3}$}% Superscript three 12 | 13 | \pagestyle{empty} 14 | \begin{document} 15 | $x$ 16 | \pagebreak 17 | 18 | $y$ 19 | \pagebreak 20 | 21 | $\dot{x}$ 22 | \pagebreak 23 | 24 | $\dot{y}$ 25 | \pagebreak 26 | 27 | \[x_k = f(x_{k-1}, u_{k-1}) + v_{k-1}\] 28 | \pagebreak 29 | 30 | \[z_k = h(x_k) + w_k\] 31 | \pagebreak 32 | 33 | $f$ 34 | \pagebreak 35 | 36 | $h$ 37 | \pagebreak 38 | 39 | $v$ 40 | \pagebreak 41 | 42 | $w$ 43 | \pagebreak 44 | 45 | $z$ 46 | \pagebreak 47 | 48 | $u$ 49 | \pagebreak 50 | 51 | $x_k$ 52 | \pagebreak 53 | 54 | $z_k$ 55 | \pagebreak 56 | 57 | $\hat{x}_k$ 58 | \pagebreak 59 | 60 | $\hat{z}_k$ 61 | \pagebreak 62 | 63 | \[y = \begin{bmatrix} f_1(x_1,x_2,x_3) \\ f_2(x_1,x_2,x_3) \end{bmatrix} \] 64 | \pagebreak 65 | 66 | \[Jac(y)=\begin{bmatrix} \frac{\partial f_1}{\partial x_1} \frac{\partial f_1}{\partial x_2} \frac{\partial f_1}{\partial x_3} \\ \frac{\partial f_2}{\partial x_1} \frac{\partial f_2}{\partial x_2} \frac{\partial f_2}{\partial x_3} \end{bmatrix} \] 67 | \pagebreak 68 | 69 | \[Hess(y(1)) =\begin{bmatrix} \frac{\partial^2 f_1}{\partial x_1^2} \frac{\partial^2 f_1}{\partial x_1 \partial x_2} \frac{\partial^2 f_1}{\partial x_1 \partial x_3} \\ \frac{\partial^2 f_1}{\partial x_2 \partial x_1} \frac{\partial^2 f_1}{\partial x_2^2} \frac{\partial^2 f_1}{\partial x_2 \partial x_3} \\ \frac{\partial^2 f_1}{\partial x_3 \partial x_1} \frac{\partial^2 f_1}{\partial x_3 \partial x_2} \frac{\partial^2 f_1}{\partial x_3^2} \end{bmatrix} \] 70 | \pagebreak 71 | 72 | \[Hess(y(2)) =\begin{bmatrix} \frac{\partial^2 f_2}{\partial x_1^2} \frac{\partial^2 f_2}{\partial x_1 \partial x_2} \frac{\partial^2 f_2}{\partial x_1 \partial x_3} \\ \frac{\partial^2 f_2}{\partial x_2 \partial x_1} \frac{\partial^2 f_2}{\partial x_2^2} \frac{\partial^2 f_2}{\partial x_2 \partial x_3} \\ \frac{\partial^2 f_2}{\partial x_3 \partial x_1} \frac{\partial^2 f_2}{\partial x_3 \partial x_2} \frac{\partial^2 f_2}{\partial x_3^2} \end{bmatrix} \] 73 | \pagebreak 74 | 75 | \[x_k = Ax_{k-1} + Bu_{k-1} + v_{k-1}\] 76 | \pagebreak 77 | 78 | \[z_k = Hx_k + w_k\] 79 | \pagebreak 80 | 81 | $A$ 82 | \pagebreak 83 | 84 | $B$ 85 | \pagebreak 86 | 87 | $H$ 88 | \pagebreak 89 | 90 | \[Q = Q^T\] 91 | \pagebreak 92 | 93 | \[R = R^T\] 94 | \pagebreak 95 | 96 | \[x_k = 12 + v_{k-1}\] 97 | \pagebreak 98 | 99 | \[z_k = x_k + w_ḳ\] 100 | \pagebreak 101 | 102 | $A = 0$ 103 | \pagebreak 104 | 105 | $B = 1$ 106 | \pagebreak 107 | 108 | $H = 1$ 109 | \pagebreak 110 | 111 | $u = 12$ 112 | \pagebreak 113 | 114 | $x_0 = 12$ 115 | \pagebreak 116 | 117 | $Q=4$ 118 | \pagebreak 119 | 120 | $R=4$ 121 | \pagebreak 122 | 123 | $Q$ 124 | \pagebreak 125 | 126 | $R$ 127 | \pagebreak 128 | 129 | \[ f = \begin{bmatrix} \sin(x_2(k-1))(k-1) \\ x_2(k-1) \end{bmatrix} \] 130 | \pagebreak 131 | 132 | \[ h = \begin{bmatrix} x_1(k) \\ x_2(k) \end{bmatrix} \] 133 | \pagebreak 134 | 135 | \[ x_0 = \begin{bmatrix} 0 \\ \frac{1 \pi}{500} \end{bmatrix}\] 136 | \pagebreak 137 | 138 | \[ f = \begin{bmatrix} x_2(k) \\ x_3(k) \\ 0.005 \, x_1(k) \bigg(x_2(k) + x_3(k) \bigg) \end{bmatrix} \] 139 | \pagebreak 140 | 141 | \[ h = \begin{bmatrix} x_1(k) \end{bmatrix} \] 142 | \pagebreak 143 | 144 | \[ x_0 = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}\] 145 | \pagebreak 146 | 147 | $x_1$ 148 | \pagebreak 149 | 150 | $x_2$ 151 | \pagebreak 152 | 153 | $x_3$ 154 | \pagebreak 155 | 156 | $u_k$ 157 | \pagebreak 158 | 159 | \[x_k = x_{k-1} + v_{k-1}\] 160 | \pagebreak 161 | 162 | \end{document} 163 | -------------------------------------------------------------------------------- /docs/html/annotated.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: Class List 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
Class List
71 |
72 |
73 |
Here are the classes, structs, unions and interfaces with brief descriptions:
74 | 75 | 76 | 77 | 78 | 79 | 80 |
 CEKFImplemetation of the extended Kalman filter. This class needs to be derived
 CEKF2Implemetation of the extended Kalman filter. This class needs to be derived
 CFX
 CKFKalman filter implementation, for a linear system
 CUKFImplemetation of the Unscented Kalman filter. This class needs to be derived
81 |
82 |
83 |
84 | 85 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/annotated_dup.js: -------------------------------------------------------------------------------- 1 | var annotated_dup = 2 | [ 3 | [ "EKF", "class_e_k_f.html", "class_e_k_f" ], 4 | [ "EKF2", "class_e_k_f2.html", "class_e_k_f2" ], 5 | [ "FX", "class_f_x.html", "class_f_x" ], 6 | [ "KF", "class_k_f.html", "class_k_f" ], 7 | [ "UKF", "class_u_k_f.html", "class_u_k_f" ] 8 | ]; -------------------------------------------------------------------------------- /docs/html/bc_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/bc_s.png -------------------------------------------------------------------------------- /docs/html/bdwn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/bdwn.png -------------------------------------------------------------------------------- /docs/html/class_e_k_f.js: -------------------------------------------------------------------------------- 1 | var class_e_k_f = 2 | [ 3 | [ "EKF", "class_e_k_f.html#ab48b75de3b276773f474e347ebc5b2a0", null ], 4 | [ "~EKF", "class_e_k_f.html#abebda258b361ad3ceb05a70ae453baec", null ], 5 | [ "EKalmanf", "class_e_k_f.html#a135d2b0e0132492693fd5c6ebe650893", null ], 6 | [ "EKalmanf", "class_e_k_f.html#aa8a5e367c077e87c46c63368e5a3fffa", null ], 7 | [ "f", "class_e_k_f.html#a6c084d493687d9ad0cf55d8c97c22633", null ], 8 | [ "GetCurrentEstimatedOutput", "class_e_k_f.html#a12968bc0cfc1e2ff922afedce318dd7b", null ], 9 | [ "GetCurrentEstimatedState", "class_e_k_f.html#ad8dcb50f6cd1dcaa313c8e90706c5dca", null ], 10 | [ "GetCurrentOutput", "class_e_k_f.html#a8ff98a83a909c1a73e7c54fb6bed9c4c", null ], 11 | [ "GetCurrentState", "class_e_k_f.html#af1efb20e6f88cf39cc80d8c83bb41ae5", null ], 12 | [ "h", "class_e_k_f.html#acb8638938ff97e5839e02f24860243e5", null ], 13 | [ "InitSystem", "class_e_k_f.html#a9d8af910c1a469ff5f4d5feaa9a24258", null ], 14 | [ "InitSystemState", "class_e_k_f.html#a4d44ac37102771fa49824b073a532670", null ], 15 | [ "InitSystemStateCovariance", "class_e_k_f.html#a55196ac6b48e2ded166834a0c1df53cf", null ], 16 | [ "nOutputs_", "class_e_k_f.html#a8902f8ab6a6a8d46524e2a501716d09e", null ], 17 | [ "nStates_", "class_e_k_f.html#ae27fd423477a96fd15665ae15a5e55e5", null ], 18 | [ "x_", "class_e_k_f.html#a262cd1dead985261fa576cf4ca7721ff", null ], 19 | [ "z_", "class_e_k_f.html#a4faca49ecd115b274373579090514fcc", null ] 20 | ]; -------------------------------------------------------------------------------- /docs/html/class_e_k_f2.js: -------------------------------------------------------------------------------- 1 | var class_e_k_f2 = 2 | [ 3 | [ "EKF2", "class_e_k_f2.html#a73120213612e130a1b8657a1b9c157b8", null ], 4 | [ "~EKF2", "class_e_k_f2.html#a1410c08c0b6eb0db519a3b94a0b3fb81", null ], 5 | [ "EKalmanf", "class_e_k_f2.html#a4c257671712db2ed50a9bac1b201ebe1", null ], 6 | [ "EKalmanf", "class_e_k_f2.html#ac846a4a4a98bc477d7c8f70250b984e7", null ], 7 | [ "f", "class_e_k_f2.html#af895b6e04cce02a8144190f719167f29", null ], 8 | [ "GetCurrentEstimatedOutput", "class_e_k_f2.html#aa7f75a7bc89e1e652a464b0c68a123af", null ], 9 | [ "GetCurrentEstimatedState", "class_e_k_f2.html#aca6143226980e2cb480b44e549dc79c6", null ], 10 | [ "GetCurrentOutput", "class_e_k_f2.html#a42f2a80683305870c849bb363a4d713b", null ], 11 | [ "GetCurrentState", "class_e_k_f2.html#af180026c5b458924417b9414f8c5957e", null ], 12 | [ "h", "class_e_k_f2.html#a85bf920e64108a76ac586a0163d6e768", null ], 13 | [ "InitSystem", "class_e_k_f2.html#ad67663f816588df77c3580ee22ab10e8", null ], 14 | [ "InitSystemState", "class_e_k_f2.html#a3e4768d55e211a99bc28b6cd7f1ffb93", null ], 15 | [ "InitSystemStateCovariance", "class_e_k_f2.html#a67dffee5f528e1d3aebdcfb874ec39e7", null ], 16 | [ "nOutputs_", "class_e_k_f2.html#ada5ba3c1e9e8dcbfbefeeaad082207a0", null ], 17 | [ "nStates_", "class_e_k_f2.html#a1f3f61b02a3213f4078b7b68b9d28741", null ], 18 | [ "x_", "class_e_k_f2.html#a25cae89d90ad4e89261a6a28c00cbe9c", null ], 19 | [ "z_", "class_e_k_f2.html#ad52e0e3cceeb94e83296503a3e1e9716", null ] 20 | ]; -------------------------------------------------------------------------------- /docs/html/class_f_x-members.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: Member List 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
FX Member List
71 |
72 |
73 | 74 |

This is the complete list of members for FX, including all inherited members.

75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 |
FX(colvec(*f)(colvec &x, colvec &some_constants))FX
HessianAt(colvec &x, colvec &some_constants, int i)FX
JacobianAt(colvec &x, colvec &some_constants)FX
SetEpsilon(double epsilon)FX
SolveAt(colvec &x, colvec &some_constants)FX
~FX()FX
83 |
84 | 85 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/class_f_x.js: -------------------------------------------------------------------------------- 1 | var class_f_x = 2 | [ 3 | [ "FX", "class_f_x.html#a55c1146952f7c792e9d0b73603427093", null ], 4 | [ "~FX", "class_f_x.html#adcbe426b544509f4e3d1b174d3a87c1e", null ], 5 | [ "HessianAt", "class_f_x.html#a54e5d98adac66b1f8e7fb7217b056ec9", null ], 6 | [ "JacobianAt", "class_f_x.html#a99f9d802876f6726360ff8dd97349bab", null ], 7 | [ "SetEpsilon", "class_f_x.html#af5dac6379cad0385dd26219bc9d184d7", null ], 8 | [ "SolveAt", "class_f_x.html#ac1ef2f0229463d1a9d12ee2041972e12", null ] 9 | ]; -------------------------------------------------------------------------------- /docs/html/class_k_f-members.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: Member List 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
KF Member List
71 |
72 |
73 | 74 |

This is the complete list of members for KF, including all inherited members.

75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 |
GetCurrentEstimatedOutput()KF
GetCurrentEstimatedState()KF
GetCurrentOutput()KF
GetCurrentState()KF
InitStateCovariance(const mat &P0)KF
InitSystem(const mat &A, const mat &B, const mat &H, const mat &Q, const mat &R)KF
InitSystemState(const colvec &x0)KF
Kalmanf(const colvec &u)KF
Kalmanf(const colvec &z, const colvec &u)KF
KF()KF
~KF()KF
88 |
89 | 90 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /docs/html/class_k_f.js: -------------------------------------------------------------------------------- 1 | var class_k_f = 2 | [ 3 | [ "KF", "class_k_f.html#ac6906ac2754b4ef3f79f45a5b7886fdd", null ], 4 | [ "~KF", "class_k_f.html#af37a2d3ac0119750e546a895d9e2f279", null ], 5 | [ "GetCurrentEstimatedOutput", "class_k_f.html#a5766b144d9d9d4ff395ed546e6071b76", null ], 6 | [ "GetCurrentEstimatedState", "class_k_f.html#abcfd6d77410e71d6df11f356ac49e6b7", null ], 7 | [ "GetCurrentOutput", "class_k_f.html#a8ec9ee4dc0e5435a2ca9e1d2787c3e9c", null ], 8 | [ "GetCurrentState", "class_k_f.html#a09da06a7f21b8d449833a0a9f130292a", null ], 9 | [ "InitStateCovariance", "class_k_f.html#a351b7f3cea3e81b82c832eb901d7b4bb", null ], 10 | [ "InitSystem", "class_k_f.html#a890367dd9a8888a2ea8bda296b6128dd", null ], 11 | [ "InitSystemState", "class_k_f.html#a4a5efd881097162ae3d76cae2f31da4f", null ], 12 | [ "Kalmanf", "class_k_f.html#acc63e676e77b8a6b1ccbb88dec545d99", null ], 13 | [ "Kalmanf", "class_k_f.html#acf29802c32e4c31adb9a3b4cdfa4eee7", null ] 14 | ]; -------------------------------------------------------------------------------- /docs/html/class_u_k_f.js: -------------------------------------------------------------------------------- 1 | var class_u_k_f = 2 | [ 3 | [ "UKF", "class_u_k_f.html#aa5f9e529068a2cf933b6cc304f75f4fa", null ], 4 | [ "~UKF", "class_u_k_f.html#aebf50d27d280b3f1fb105bbe0338b0ca", null ], 5 | [ "f", "class_u_k_f.html#a3121b2c76798ca333cd2b292daa1a32c", null ], 6 | [ "GetCurrentEstimatedOutput", "class_u_k_f.html#a293ca62563975effafb390fce580d3bf", null ], 7 | [ "GetCurrentEstimatedState", "class_u_k_f.html#ade927ed141ab72355141460875d3dbef", null ], 8 | [ "GetCurrentOutput", "class_u_k_f.html#a70058f98ea44a782a998612b4dde0c2e", null ], 9 | [ "GetCurrentState", "class_u_k_f.html#a5d73d3a72cd13d1aaa90ca776854fd75", null ], 10 | [ "h", "class_u_k_f.html#ae0be58f48525b8a77bfbfe0091615519", null ], 11 | [ "InitSystem", "class_u_k_f.html#a9a4205634f4f347511741eb69337b25b", null ], 12 | [ "InitSystemState", "class_u_k_f.html#ae93f8be9b388c358c65e376530f37364", null ], 13 | [ "InitSystemStateCovariance", "class_u_k_f.html#aba92eae3369d180af50590e01a15c810", null ], 14 | [ "UKalmanf", "class_u_k_f.html#a548dd1501813f70f17d13d83a26ddfc1", null ], 15 | [ "UKalmanf", "class_u_k_f.html#ac6b50286cf4773bb5ea82f5c554fd729", null ], 16 | [ "nOutputs_", "class_u_k_f.html#ad15469068a235d3474bdc6a21762183f", null ], 17 | [ "nStates_", "class_u_k_f.html#a9d21d070be2cc239f6fcc7f72771fc64", null ], 18 | [ "x_", "class_u_k_f.html#ae354cb3833947d9cac8e8a540283d913", null ], 19 | [ "z_", "class_u_k_f.html#a8a0aa571ba18f83d5b507d4d0e4cc2ab", null ] 20 | ]; -------------------------------------------------------------------------------- /docs/html/classes.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: Class Index 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
Class Index
71 |
72 |
73 |
E | F | K | U
74 |
75 |
76 |
E
77 |
EKF
EKF2
78 |
79 |
F
80 |
FX
81 |
82 |
K
83 |
KF
84 |
85 |
U
86 |
UKF
87 |
88 |
89 |
90 | 91 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /docs/html/closed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/closed.png -------------------------------------------------------------------------------- /docs/html/dir_6f9635d4fbfa321d4c00f29df5cd540c.js: -------------------------------------------------------------------------------- 1 | var dir_6f9635d4fbfa321d4c00f29df5cd540c = 2 | [ 3 | [ "main1.cpp", "main1_8cpp.html", "main1_8cpp" ], 4 | [ "main10.cpp", "main10_8cpp.html", "main10_8cpp" ], 5 | [ "main11.cpp", "main11_8cpp.html", "main11_8cpp" ], 6 | [ "main2.cpp", "main2_8cpp.html", "main2_8cpp" ], 7 | [ "main3.cpp", "main3_8cpp.html", "main3_8cpp" ], 8 | [ "main4.cpp", "main4_8cpp.html", "main4_8cpp" ], 9 | [ "main5.cpp", "main5_8cpp.html", "main5_8cpp" ], 10 | [ "main6.cpp", "main6_8cpp.html", "main6_8cpp" ], 11 | [ "main7.cpp", "main7_8cpp.html", "main7_8cpp" ], 12 | [ "main8.cpp", "main8_8cpp.html", "main8_8cpp" ], 13 | [ "main9.cpp", "main9_8cpp.html", "main9_8cpp" ] 14 | ]; -------------------------------------------------------------------------------- /docs/html/dir_dc133b73f91f0e3b4287924ed00a2a00.js: -------------------------------------------------------------------------------- 1 | var dir_dc133b73f91f0e3b4287924ed00a2a00 = 2 | [ 3 | [ "ekf.cpp", "ekf_8cpp.html", null ], 4 | [ "ekf.h", "ekf_8h.html", "ekf_8h" ], 5 | [ "ekf2.cpp", "ekf2_8cpp.html", null ], 6 | [ "ekf2.h", "ekf2_8h.html", "ekf2_8h" ], 7 | [ "fx.cpp", "fx_8cpp.html", null ], 8 | [ "fx.h", "fx_8h.html", "fx_8h" ], 9 | [ "kf.cpp", "kf_8cpp.html", null ], 10 | [ "kf.h", "kf_8h.html", "kf_8h" ], 11 | [ "ukf.cpp", "ukf_8cpp.html", null ], 12 | [ "ukf.h", "ukf_8h.html", "ukf_8h" ] 13 | ]; -------------------------------------------------------------------------------- /docs/html/doc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/doc.png -------------------------------------------------------------------------------- /docs/html/dynsections.js: -------------------------------------------------------------------------------- 1 | /* 2 | @licstart The following is the entire license notice for the JavaScript code in this file. 3 | 4 | The MIT License (MIT) 5 | 6 | Copyright (C) 1997-2020 by Dimitri van Heesch 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 9 | and associated documentation files (the "Software"), to deal in the Software without restriction, 10 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 11 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all copies or 15 | substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 18 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | 23 | @licend The above is the entire license notice for the JavaScript code in this file 24 | */ 25 | function toggleVisibility(linkObj) 26 | { 27 | var base = $(linkObj).attr('id'); 28 | var summary = $('#'+base+'-summary'); 29 | var content = $('#'+base+'-content'); 30 | var trigger = $('#'+base+'-trigger'); 31 | var src=$(trigger).attr('src'); 32 | if (content.is(':visible')===true) { 33 | content.hide(); 34 | summary.show(); 35 | $(linkObj).addClass('closed').removeClass('opened'); 36 | $(trigger).attr('src',src.substring(0,src.length-8)+'closed.png'); 37 | } else { 38 | content.show(); 39 | summary.hide(); 40 | $(linkObj).removeClass('closed').addClass('opened'); 41 | $(trigger).attr('src',src.substring(0,src.length-10)+'open.png'); 42 | } 43 | return false; 44 | } 45 | 46 | function updateStripes() 47 | { 48 | $('table.directory tr'). 49 | removeClass('even').filter(':visible:even').addClass('even'); 50 | } 51 | 52 | function toggleLevel(level) 53 | { 54 | $('table.directory tr').each(function() { 55 | var l = this.id.split('_').length-1; 56 | var i = $('#img'+this.id.substring(3)); 57 | var a = $('#arr'+this.id.substring(3)); 58 | if (l 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: kalman/ekf2.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
ekf2.cpp File Reference
71 |
72 |
73 | 74 |

Second order extended Kalman filter implementation, for a nonlinear system. 75 | More...

76 |
#include "ekf2.h"
77 |

Detailed Description

78 |

Second order extended Kalman filter implementation, for a nonlinear system.

79 |
Author
Auralius Manurung
80 |
Date
18 Apr 2015
81 |
82 |
83 | 84 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/ekf2_8h.js: -------------------------------------------------------------------------------- 1 | var ekf2_8h = 2 | [ 3 | [ "EKF2", "class_e_k_f2.html", "class_e_k_f2" ], 4 | [ "_USE_MATH_DEFINES", "ekf2_8h.html#a525335710b53cb064ca56b936120431e", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/ekf_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: kalman/ekf.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
ekf.cpp File Reference
71 |
72 |
73 | 74 |

Extended Kalman filter implementation, for a nonlinear system. 75 | More...

76 |
#include "ekf.h"
77 |

Detailed Description

78 |

Extended Kalman filter implementation, for a nonlinear system.

79 |
Author
Auralius Manurung
80 |
Date
18 Apr 2015
81 |
82 |
83 | 84 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/ekf_8h.js: -------------------------------------------------------------------------------- 1 | var ekf_8h = 2 | [ 3 | [ "EKF", "class_e_k_f.html", "class_e_k_f" ], 4 | [ "_USE_MATH_DEFINES", "ekf_8h.html#a525335710b53cb064ca56b936120431e", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/ex1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/ex1.png -------------------------------------------------------------------------------- /docs/html/ex10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/ex10.png -------------------------------------------------------------------------------- /docs/html/ex4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/ex4.png -------------------------------------------------------------------------------- /docs/html/ex7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/ex7.png -------------------------------------------------------------------------------- /docs/html/files_dup.js: -------------------------------------------------------------------------------- 1 | var files_dup = 2 | [ 3 | [ "kalman", "dir_dc133b73f91f0e3b4287924ed00a2a00.html", "dir_dc133b73f91f0e3b4287924ed00a2a00" ], 4 | [ "samples", "dir_6f9635d4fbfa321d4c00f29df5cd540c.html", "dir_6f9635d4fbfa321d4c00f29df5cd540c" ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/folderclosed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/folderclosed.png -------------------------------------------------------------------------------- /docs/html/folderopen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/folderopen.png -------------------------------------------------------------------------------- /docs/html/functions_vars.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: Class Members - Variables 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |  
    70 |
  • nOutputs_ 71 | : EKF2 72 | , EKF 73 | , UKF 74 |
    • 75 |
  • nStates_ 76 | : EKF2 77 | , EKF 78 | , UKF 79 |
    • 80 |
  • x_ 81 | : EKF2 82 | , EKF 83 | , UKF 84 |
    • 85 |
  • z_ 86 | : EKF2 87 | , EKF 88 | , UKF 89 |
    • 90 |
91 |
92 |
93 | 94 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /docs/html/fx_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: kalman/fx.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
fx.cpp File Reference
71 |
72 |
73 | 74 |

Implement a user defined mathematical function. 75 | More...

76 |
#include "fx.h"
77 |

Detailed Description

78 |

Implement a user defined mathematical function.

79 |
Author
Auralius Manurung
80 |
Date
28 Jan 2017
81 |
82 |
83 | 84 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/fx_8h.js: -------------------------------------------------------------------------------- 1 | var fx_8h = 2 | [ 3 | [ "FX", "class_f_x.html", "class_f_x" ], 4 | [ "_USE_MATH_DEFINES", "fx_8h.html#a525335710b53cb064ca56b936120431e", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/globals.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: File Members 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
Here is a list of all file members with links to the files they belong to:
94 |
95 |
96 | 97 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /docs/html/globals_defs.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: File Members 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |   78 |
79 |
80 | 81 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /docs/html/globals_func.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: File Members 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |   87 |
88 |
89 | 90 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /docs/html/kf_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: kalman/kf.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
kf.cpp File Reference
71 |
72 |
73 | 74 |

Kalman filter implementation, for a linear system. 75 | More...

76 |
#include "kf.h"
77 |

Detailed Description

78 |

Kalman filter implementation, for a linear system.

79 |
Author
Auralius Manurung
80 |
Date
18 Apr 2015
81 |
82 |
83 | 84 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/kf_8h.js: -------------------------------------------------------------------------------- 1 | var kf_8h = 2 | [ 3 | [ "KF", "class_k_f.html", "class_k_f" ], 4 | [ "_USE_MATH_DEFINES", "kf_8h.html#a525335710b53cb064ca56b936120431e", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/main10_8cpp.js: -------------------------------------------------------------------------------- 1 | var main10_8cpp = 2 | [ 3 | [ "main", "main10_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main11_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: samples/main11.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 | Functions
71 |
72 |
main11.cpp File Reference
73 |
74 |
75 | 76 |

An example for an Unscented Kalman filter. 77 | More...

78 |
#include <fstream>
79 | #include "ukf.h"
80 |
81 | 83 | 84 | 85 |

82 | Functions

int main (int argc, char **argv)
 
86 |

Detailed Description

87 |

An example for an Unscented Kalman filter.

88 |
Author
Auralius Manurung
89 |
Date
15 Peb 2021
90 |

91 | DESCRIPTION

92 |

This example is taken from here, section 3.2: "Tracking a random sine signal".

93 |

Function Documentation

94 | 95 |

◆ main()

96 | 97 |
98 |
99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 |
int main (int argc,
char ** argv 
)
118 |
119 | 120 |
121 |
122 |
123 |
124 | 125 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /docs/html/main11_8cpp.js: -------------------------------------------------------------------------------- 1 | var main11_8cpp = 2 | [ 3 | [ "main", "main11_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main1_8cpp.js: -------------------------------------------------------------------------------- 1 | var main1_8cpp = 2 | [ 3 | [ "main", "main1_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main2_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: samples/main2.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 | Functions
71 |
72 |
main2.cpp File Reference
73 |
74 |
75 | 76 |

Example for the Kalman filter. 77 | More...

78 |
#include <fstream>
79 | #include "kf.h"
80 |
81 | 83 | 84 | 85 |

82 | Functions

int main (int argc, char **argv)
 
86 |

Detailed Description

87 |

Example for the Kalman filter.

88 |
Author
Auralius Manurung
89 |
Date
18 Apr 2015
90 |

91 | DESCRIPTION

92 |

A kinematic system, with position and velocity as the states. Measured output is position.

93 |

Function Documentation

94 | 95 |

◆ main()

96 | 97 |
98 |
99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 |
int main (int argc,
char ** argv 
)
118 |
119 | 120 |
121 |
122 |
123 |
124 | 125 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /docs/html/main2_8cpp.js: -------------------------------------------------------------------------------- 1 | var main2_8cpp = 2 | [ 3 | [ "main", "main2_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main3_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: samples/main3.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 | Functions
71 |
72 |
main3.cpp File Reference
73 |
74 |
75 | 76 |

Example for the Kalman filter. 77 | More...

78 |
#include <fstream>
79 | #include "kf.h"
80 |
81 | 83 | 84 | 85 |

82 | Functions

int main (int argc, char **argv)
 
86 |

Detailed Description

87 |

Example for the Kalman filter.

88 |
Author
Auralius Manurung
89 |
Date
18 Apr 2015
90 |

91 | DESCRIPTION

92 |

A system in x-y plane, a kinematic system, with position and velocity as the states. The system has 4 input states ( \(x\), \(y\), \(\dot{x}\), \(\dot{y}\)). and 2 output states ( \(x\), \(y\)).

93 |

Function Documentation

94 | 95 |

◆ main()

96 | 97 |
98 |
99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 |
int main (int argc,
char ** argv 
)
118 |
119 | 120 |
121 |
122 |
123 |
124 | 125 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /docs/html/main3_8cpp.js: -------------------------------------------------------------------------------- 1 | var main3_8cpp = 2 | [ 3 | [ "main", "main3_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main4_8cpp.js: -------------------------------------------------------------------------------- 1 | var main4_8cpp = 2 | [ 3 | [ "main", "main4_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main5_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: samples/main5.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 | Functions
71 |
72 |
main5.cpp File Reference
73 |
74 |
75 | 76 |

Example for the extended Kalman filter. 77 | More...

78 |
#include <fstream>
79 | #include "ekf.h"
80 |
81 | 83 | 84 | 85 |

82 | Functions

int main (int argc, char **argv)
 
86 |

Detailed Description

87 |

Example for the extended Kalman filter.

88 |
Author
Auralius Manurung
89 |
Date
12 Aug 2015
90 |

91 | DESCRIPTION

92 |

This example is taken from here.

93 |

Function Documentation

94 | 95 |

◆ main()

96 | 97 |
98 |
99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 |
int main (int argc,
char ** argv 
)
118 |
119 | 120 |
121 |
122 |
123 |
124 | 125 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /docs/html/main5_8cpp.js: -------------------------------------------------------------------------------- 1 | var main5_8cpp = 2 | [ 3 | [ "main", "main5_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main6_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: samples/main6.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 | Functions
71 |
72 |
main6.cpp File Reference
73 |
74 |
75 | 76 |

Example for the extended Kalman filter. 77 | More...

78 |
#include <fstream>
79 | #include "ekf.h"
80 |
81 | 83 | 84 | 85 |

82 | Functions

int main (int argc, char **argv)
 
86 |

Detailed Description

87 |

Example for the extended Kalman filter.

88 |
Author
Auralius Manurung
89 |
Date
12 Aug 2015
90 |

91 | DESCRIPTION

92 |

This example is taken from here, section 3.2: "Tracking a random sine signal".

93 |

Function Documentation

94 | 95 |

◆ main()

96 | 97 |
98 |
99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 |
int main (int argc,
char ** argv 
)
118 |
119 | 120 |
121 |
122 |
123 |
124 | 125 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /docs/html/main6_8cpp.js: -------------------------------------------------------------------------------- 1 | var main6_8cpp = 2 | [ 3 | [ "main", "main6_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main7_8cpp.js: -------------------------------------------------------------------------------- 1 | var main7_8cpp = 2 | [ 3 | [ "main", "main7_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main8_8cpp.js: -------------------------------------------------------------------------------- 1 | var main8_8cpp = 2 | [ 3 | [ "main", "main8_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 4 | ]; -------------------------------------------------------------------------------- /docs/html/main9_8cpp.js: -------------------------------------------------------------------------------- 1 | var main9_8cpp = 2 | [ 3 | [ "foo", "main9_8cpp.html#ad977f3cfa6915bbd8e66d197068ac946", null ], 4 | [ "main", "main9_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/html/mainpage_8dox.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: mainpage.dox File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
27 |
28 | 29 | 30 | 31 | 36 | 37 | 38 |
32 |
kalman-cpp 33 |
34 |
Implementation of Kalman Filter in C++
35 |
39 |
40 | 41 | 42 | 43 | 44 | 50 | 51 |
52 |
53 | 58 |
60 |
61 |
62 | 67 |
68 |
69 |
70 |
mainpage.dox File Reference
71 |
72 |
73 |
74 |
75 | 76 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /docs/html/menu.js: -------------------------------------------------------------------------------- 1 | /* 2 | @licstart The following is the entire license notice for the JavaScript code in this file. 3 | 4 | The MIT License (MIT) 5 | 6 | Copyright (C) 1997-2020 by Dimitri van Heesch 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 9 | and associated documentation files (the "Software"), to deal in the Software without restriction, 10 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 11 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all copies or 15 | substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 18 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | 23 | @licend The above is the entire license notice for the JavaScript code in this file 24 | */ 25 | function initMenu(relPath,searchEnabled,serverSide,searchPage,search) { 26 | function makeTree(data,relPath) { 27 | var result=''; 28 | if ('children' in data) { 29 | result+=''; 36 | } 37 | return result; 38 | } 39 | 40 | $('#main-nav').append(makeTree(menudata,relPath)); 41 | $('#main-nav').children(':first').addClass('sm sm-dox').attr('id','main-menu'); 42 | if (searchEnabled) { 43 | if (serverSide) { 44 | $('#main-menu').append('
    • '); 45 | } else { 46 | $('#main-menu').append('
    • '); 47 | } 48 | } 49 | $('#main-menu').smartmenus(); 50 | } 51 | /* @license-end */ 52 | -------------------------------------------------------------------------------- /docs/html/menudata.js: -------------------------------------------------------------------------------- 1 | /* 2 | @licstart The following is the entire license notice for the JavaScript code in this file. 3 | 4 | The MIT License (MIT) 5 | 6 | Copyright (C) 1997-2020 by Dimitri van Heesch 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 9 | and associated documentation files (the "Software"), to deal in the Software without restriction, 10 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 11 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all copies or 15 | substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 18 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | 23 | @licend The above is the entire license notice for the JavaScript code in this file 24 | */ 25 | var menudata={children:[ 26 | {text:"Main Page",url:"index.html"}, 27 | {text:"Classes",url:"annotated.html",children:[ 28 | {text:"Class List",url:"annotated.html"}, 29 | {text:"Class Index",url:"classes.html"}, 30 | {text:"Class Members",url:"functions.html",children:[ 31 | {text:"All",url:"functions.html",children:[ 32 | {text:"e",url:"functions.html#index_e"}, 33 | {text:"f",url:"functions.html#index_f"}, 34 | {text:"g",url:"functions.html#index_g"}, 35 | {text:"h",url:"functions.html#index_h"}, 36 | {text:"i",url:"functions.html#index_i"}, 37 | {text:"j",url:"functions.html#index_j"}, 38 | {text:"k",url:"functions.html#index_k"}, 39 | {text:"n",url:"functions.html#index_n"}, 40 | {text:"s",url:"functions.html#index_s"}, 41 | {text:"u",url:"functions.html#index_u"}, 42 | {text:"x",url:"functions.html#index_x"}, 43 | {text:"z",url:"functions.html#index_z"}, 44 | {text:"~",url:"functions.html#index__7E"}]}, 45 | {text:"Functions",url:"functions_func.html",children:[ 46 | {text:"e",url:"functions_func.html#index_e"}, 47 | {text:"f",url:"functions_func.html#index_f"}, 48 | {text:"g",url:"functions_func.html#index_g"}, 49 | {text:"h",url:"functions_func.html#index_h"}, 50 | {text:"i",url:"functions_func.html#index_i"}, 51 | {text:"j",url:"functions_func.html#index_j"}, 52 | {text:"k",url:"functions_func.html#index_k"}, 53 | {text:"s",url:"functions_func.html#index_s"}, 54 | {text:"u",url:"functions_func.html#index_u"}, 55 | {text:"~",url:"functions_func.html#index__7E"}]}, 56 | {text:"Variables",url:"functions_vars.html"}]}]}, 57 | {text:"Files",url:"files.html",children:[ 58 | {text:"File List",url:"files.html"}, 59 | {text:"File Members",url:"globals.html",children:[ 60 | {text:"All",url:"globals.html"}, 61 | {text:"Functions",url:"globals_func.html"}, 62 | {text:"Macros",url:"globals_defs.html"}]}]}]} 63 | -------------------------------------------------------------------------------- /docs/html/nav_f.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/nav_f.png -------------------------------------------------------------------------------- /docs/html/nav_g.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/nav_g.png -------------------------------------------------------------------------------- /docs/html/nav_h.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/nav_h.png -------------------------------------------------------------------------------- /docs/html/navtree.css: -------------------------------------------------------------------------------- 1 | #nav-tree .children_ul { 2 | margin:0; 3 | padding:4px; 4 | } 5 | 6 | #nav-tree ul { 7 | list-style:none outside none; 8 | margin:0px; 9 | padding:0px; 10 | } 11 | 12 | #nav-tree li { 13 | white-space:nowrap; 14 | margin:0px; 15 | padding:0px; 16 | } 17 | 18 | #nav-tree .plus { 19 | margin:0px; 20 | } 21 | 22 | #nav-tree .selected { 23 | background-image: url('tab_a.png'); 24 | background-repeat:repeat-x; 25 | color: #fff; 26 | text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0); 27 | } 28 | 29 | #nav-tree img { 30 | margin:0px; 31 | padding:0px; 32 | border:0px; 33 | vertical-align: middle; 34 | } 35 | 36 | #nav-tree a { 37 | text-decoration:none; 38 | padding:0px; 39 | margin:0px; 40 | outline:none; 41 | } 42 | 43 | #nav-tree .label { 44 | margin:0px; 45 | padding:0px; 46 | font: 12px 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; 47 | } 48 | 49 | #nav-tree .label a { 50 | padding:2px; 51 | } 52 | 53 | #nav-tree .selected a { 54 | text-decoration:none; 55 | color:#fff; 56 | } 57 | 58 | #nav-tree .children_ul { 59 | margin:0px; 60 | padding:0px; 61 | } 62 | 63 | #nav-tree .item { 64 | margin:0px; 65 | padding:0px; 66 | } 67 | 68 | #nav-tree { 69 | padding: 0px 0px; 70 | background-color: #FAFAFF; 71 | font-size:14px; 72 | overflow:auto; 73 | } 74 | 75 | #doc-content { 76 | overflow:auto; 77 | display:block; 78 | padding:0px; 79 | margin:0px; 80 | -webkit-overflow-scrolling : touch; /* iOS 5+ */ 81 | } 82 | 83 | #side-nav { 84 | padding:0 6px 0 0; 85 | margin: 0px; 86 | display:block; 87 | position: absolute; 88 | left: 0px; 89 | width: 250px; 90 | } 91 | 92 | .ui-resizable .ui-resizable-handle { 93 | display:block; 94 | } 95 | 96 | .ui-resizable-e { 97 | background-image:url("splitbar.png"); 98 | background-size:100%; 99 | background-repeat:repeat-y; 100 | background-attachment: scroll; 101 | cursor:ew-resize; 102 | height:100%; 103 | right:0; 104 | top:0; 105 | width:6px; 106 | } 107 | 108 | .ui-resizable-handle { 109 | display:none; 110 | font-size:0.1px; 111 | position:absolute; 112 | z-index:1; 113 | } 114 | 115 | #nav-tree-contents { 116 | margin: 6px 0px 0px 0px; 117 | } 118 | 119 | #nav-tree { 120 | background-image:url('nav_h.png'); 121 | background-repeat:repeat-x; 122 | background-color: #F9FAFC; 123 | -webkit-overflow-scrolling : touch; /* iOS 5+ */ 124 | } 125 | 126 | #nav-sync { 127 | position:absolute; 128 | top:5px; 129 | right:24px; 130 | z-index:0; 131 | } 132 | 133 | #nav-sync img { 134 | opacity:0.3; 135 | } 136 | 137 | #nav-sync img:hover { 138 | opacity:0.9; 139 | } 140 | 141 | @media print 142 | { 143 | #nav-tree { display: none; } 144 | div.ui-resizable-handle { display: none; position: relative; } 145 | } 146 | 147 | -------------------------------------------------------------------------------- /docs/html/navtreedata.js: -------------------------------------------------------------------------------- 1 | /* 2 | @licstart The following is the entire license notice for the JavaScript code in this file. 3 | 4 | The MIT License (MIT) 5 | 6 | Copyright (C) 1997-2020 by Dimitri van Heesch 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 9 | and associated documentation files (the "Software"), to deal in the Software without restriction, 10 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 11 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all copies or 15 | substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 18 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | 23 | @licend The above is the entire license notice for the JavaScript code in this file 24 | */ 25 | var NAVTREE = 26 | [ 27 | [ "kalman-cpp", "index.html", [ 28 | [ "Kalman Filter for Linear Systems", "index.html#linear-kalman", [ 29 | [ "Definition", "index.html#definition_kf", null ], 30 | [ "Examples", "index.html#example_kf", null ] 31 | ] ], 32 | [ "Extended Kalman Filter (EKF) for nonlinear systems", "index.html#non-linear-kalman", [ 33 | [ "Definition", "index.html#definition_ekf", null ], 34 | [ "Example", "index.html#example_ekf", null ] 35 | ] ], 36 | [ "Unscented Kalman Filter (UKF) for a nonlinear system", "index.html#unscented-kalman", [ 37 | [ "Example", "index.html#example_ukf", null ] 38 | ] ], 39 | [ "Practical application: Kalman filter for noisy measurements", "index.html#practical-application", [ 40 | [ "Example", "index.html#practical_example", null ] 41 | ] ], 42 | [ "Classes", "annotated.html", [ 43 | [ "Class List", "annotated.html", "annotated_dup" ], 44 | [ "Class Index", "classes.html", null ], 45 | [ "Class Members", "functions.html", [ 46 | [ "All", "functions.html", null ], 47 | [ "Functions", "functions_func.html", null ], 48 | [ "Variables", "functions_vars.html", null ] 49 | ] ] 50 | ] ], 51 | [ "Files", "files.html", [ 52 | [ "File List", "files.html", "files_dup" ], 53 | [ "File Members", "globals.html", [ 54 | [ "All", "globals.html", null ], 55 | [ "Functions", "globals_func.html", null ], 56 | [ "Macros", "globals_defs.html", null ] 57 | ] ] 58 | ] ] 59 | ] ] 60 | ]; 61 | 62 | var NAVTREEINDEX = 63 | [ 64 | "annotated.html" 65 | ]; 66 | 67 | var SYNCONMSG = 'click to disable panel synchronisation'; 68 | var SYNCOFFMSG = 'click to enable panel synchronisation'; -------------------------------------------------------------------------------- /docs/html/open.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/open.png -------------------------------------------------------------------------------- /docs/html/resize.js: -------------------------------------------------------------------------------- 1 | /* 2 | @licstart The following is the entire license notice for the JavaScript code in this file. 3 | 4 | The MIT License (MIT) 5 | 6 | Copyright (C) 1997-2020 by Dimitri van Heesch 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 9 | and associated documentation files (the "Software"), to deal in the Software without restriction, 10 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 11 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all copies or 15 | substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 18 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | 23 | @licend The above is the entire license notice for the JavaScript code in this file 24 | */ 25 | function initResizable() 26 | { 27 | var cookie_namespace = 'doxygen'; 28 | var sidenav,navtree,content,header,collapsed,collapsedWidth=0,barWidth=6,desktop_vp=768,titleHeight; 29 | 30 | function readCookie(cookie) 31 | { 32 | var myCookie = cookie_namespace+"_"+cookie+"="; 33 | if (document.cookie) { 34 | var index = document.cookie.indexOf(myCookie); 35 | if (index != -1) { 36 | var valStart = index + myCookie.length; 37 | var valEnd = document.cookie.indexOf(";", valStart); 38 | if (valEnd == -1) { 39 | valEnd = document.cookie.length; 40 | } 41 | var val = document.cookie.substring(valStart, valEnd); 42 | return val; 43 | } 44 | } 45 | return 0; 46 | } 47 | 48 | function writeCookie(cookie, val, expiration) 49 | { 50 | if (val==undefined) return; 51 | if (expiration == null) { 52 | var date = new Date(); 53 | date.setTime(date.getTime()+(10*365*24*60*60*1000)); // default expiration is one week 54 | expiration = date.toGMTString(); 55 | } 56 | document.cookie = cookie_namespace + "_" + cookie + "=" + val + "; expires=" + expiration+"; path=/"; 57 | } 58 | 59 | function resizeWidth() 60 | { 61 | var windowWidth = $(window).width() + "px"; 62 | var sidenavWidth = $(sidenav).outerWidth(); 63 | content.css({marginLeft:parseInt(sidenavWidth)+"px"}); 64 | writeCookie('width',sidenavWidth-barWidth, null); 65 | } 66 | 67 | function restoreWidth(navWidth) 68 | { 69 | var windowWidth = $(window).width() + "px"; 70 | content.css({marginLeft:parseInt(navWidth)+barWidth+"px"}); 71 | sidenav.css({width:navWidth + "px"}); 72 | } 73 | 74 | function resizeHeight() 75 | { 76 | var headerHeight = header.outerHeight(); 77 | var footerHeight = footer.outerHeight(); 78 | var windowHeight = $(window).height() - headerHeight - footerHeight; 79 | content.css({height:windowHeight + "px"}); 80 | navtree.css({height:windowHeight + "px"}); 81 | sidenav.css({height:windowHeight + "px"}); 82 | var width=$(window).width(); 83 | if (width!=collapsedWidth) { 84 | if (width=desktop_vp) { 85 | if (!collapsed) { 86 | collapseExpand(); 87 | } 88 | } else if (width>desktop_vp && collapsedWidth0) { 103 | restoreWidth(0); 104 | collapsed=true; 105 | } 106 | else { 107 | var width = readCookie('width'); 108 | if (width>200 && width<$(window).width()) { restoreWidth(width); } else { restoreWidth(200); } 109 | collapsed=false; 110 | } 111 | } 112 | 113 | header = $("#top"); 114 | sidenav = $("#side-nav"); 115 | content = $("#doc-content"); 116 | navtree = $("#nav-tree"); 117 | footer = $("#nav-path"); 118 | $(".side-nav-resizable").resizable({resize: function(e, ui) { resizeWidth(); } }); 119 | $(sidenav).resizable({ minWidth: 0 }); 120 | $(window).resize(function() { resizeHeight(); }); 121 | var device = navigator.userAgent.toLowerCase(); 122 | var touch_device = device.match(/(iphone|ipod|ipad|android)/); 123 | if (touch_device) { /* wider split bar for touch only devices */ 124 | $(sidenav).css({ paddingRight:'20px' }); 125 | $('.ui-resizable-e').css({ width:'20px' }); 126 | $('#nav-sync').css({ right:'34px' }); 127 | barWidth=20; 128 | } 129 | var width = readCookie('width'); 130 | if (width) { restoreWidth(width); } else { resizeWidth(); } 131 | resizeHeight(); 132 | var url = location.href; 133 | var i=url.indexOf("#"); 134 | if (i>=0) window.location.hash=url.substr(i); 135 | var _preventDefault = function(evt) { evt.preventDefault(); }; 136 | $("#splitbar").bind("dragstart", _preventDefault).bind("selectstart", _preventDefault); 137 | $(".ui-resizable-handle").dblclick(collapseExpand); 138 | $(window).on('load',resizeHeight); 139 | } 140 | /* @license-end */ 141 | -------------------------------------------------------------------------------- /docs/html/splitbar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/splitbar.png -------------------------------------------------------------------------------- /docs/html/sync_off.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/sync_off.png -------------------------------------------------------------------------------- /docs/html/sync_on.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/sync_on.png -------------------------------------------------------------------------------- /docs/html/tab_a.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/tab_a.png -------------------------------------------------------------------------------- /docs/html/tab_b.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/tab_b.png -------------------------------------------------------------------------------- /docs/html/tab_h.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/tab_h.png -------------------------------------------------------------------------------- /docs/html/tab_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/docs/html/tab_s.png -------------------------------------------------------------------------------- /docs/html/ukf_8cpp.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | kalman-cpp: kalman/ukf.cpp File Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 22 | 23 | 24 | 25 | 26 |
    27 |
    28 | 29 | 30 | 31 | 36 | 37 | 38 |
    32 |
    kalman-cpp 33 |
    34 |
    Implementation of Kalman Filter in C++
    35 |
    39 |
    40 | 41 | 42 | 43 | 44 | 50 | 51 |
    52 |
    53 | 58 |
    60 |
    61 |
    62 | 67 |
    68 |
    69 |
    70 |
    ukf.cpp File Reference
    71 |
    72 |
    73 | 74 |

    Unscented Kalman filter implementation, for a nonlinear system. 75 | More...

    76 |
    #include "ukf.h"
    77 |

    Detailed Description

    78 |

    Unscented Kalman filter implementation, for a nonlinear system.

    79 |
    Author
    Auralius Manurung
    80 |
    Date
    13 Peb 2021
    81 |
    82 |
    83 | 84 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /docs/html/ukf_8h.js: -------------------------------------------------------------------------------- 1 | var ukf_8h = 2 | [ 3 | [ "UKF", "class_u_k_f.html", "class_u_k_f" ], 4 | [ "_USE_MATH_DEFINES", "ukf_8h.html#a525335710b53cb064ca56b936120431e", null ] 5 | ]; -------------------------------------------------------------------------------- /docs/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /images/Kalman_concept1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/Kalman_concept1.jpg -------------------------------------------------------------------------------- /images/Kalman_concept2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/Kalman_concept2.jpg -------------------------------------------------------------------------------- /images/ex1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex1.png -------------------------------------------------------------------------------- /images/ex10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex10.png -------------------------------------------------------------------------------- /images/ex2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex2.png -------------------------------------------------------------------------------- /images/ex3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex3.png -------------------------------------------------------------------------------- /images/ex4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex4.png -------------------------------------------------------------------------------- /images/ex5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex5.png -------------------------------------------------------------------------------- /images/ex6_outputs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex6_outputs.png -------------------------------------------------------------------------------- /images/ex6_states.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex6_states.png -------------------------------------------------------------------------------- /images/ex7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/images/ex7.png -------------------------------------------------------------------------------- /kalman-cpp.kdev4: -------------------------------------------------------------------------------- 1 | [Project] 2 | Manager=KDevCMakeManager 3 | Name=kalman-cpp 4 | -------------------------------------------------------------------------------- /kalman/ekf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ekf.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * @brief Extended Kalman filter implementation, for a nonlinear system. 6 | */ 7 | 8 | #include "ekf.h" 9 | 10 | EKF::EKF() 11 | { 12 | 13 | } 14 | 15 | EKF::~EKF() 16 | { 17 | 18 | } 19 | 20 | void EKF::InitSystem(int n_states, int n_outputs, const mat& Q, const mat& R) 21 | { 22 | assert(Q.is_square() && "Whoops, Q must be a square matrix"); 23 | assert(R.is_square() && "Whoops, R must be a square matrix (n_outputs x n_outputs)"); 24 | 25 | // Epsilon for computing the Jacobian numerically 26 | epsilon_= 1e-8; 27 | 28 | nStates_ = n_states; 29 | nOutputs_ = n_outputs; 30 | 31 | Q_ = Q; 32 | R_ = R; 33 | 34 | // Stdev is sqrt of variance 35 | sqrt_Q_ = sqrt(Q_); 36 | sqrt_R_ = sqrt(R_); 37 | 38 | x_.resize(n_states); 39 | F_.resize(n_states, n_states); 40 | H_.resize(n_outputs, n_states); 41 | 42 | x_p_.resize(n_states); 43 | x_m_.resize(n_states); 44 | 45 | P_p_.resize(n_states, n_states); 46 | P_m_.resize(n_states, n_states); 47 | 48 | v_.resize(n_states); 49 | w_.resize(n_outputs); 50 | 51 | // Apply intial states 52 | x_.resize(n_states); 53 | x_ = x_.zeros(); 54 | 55 | // Inital values for the Kalman iterations 56 | P_m_ = P_m_.eye(); 57 | x_m_ = x_m_.zeros(); 58 | } 59 | 60 | colvec EKF::f(const colvec &x, const colvec &u) 61 | { 62 | colvec xk(nOutputs_); 63 | xk = xk.zeros(); 64 | return xk; 65 | } 66 | 67 | colvec EKF::h(const colvec &x) 68 | { 69 | colvec zk(nOutputs_); 70 | zk = zk.zeros(); 71 | return zk; 72 | } 73 | 74 | void EKF::CalcF(const colvec &x, const colvec &u) 75 | { 76 | colvec f0 = f(x, u); 77 | 78 | colvec fn; 79 | 80 | for (int j = 0; j < nStates_; j ++) { 81 | colvec x_eps = x; 82 | x_eps(j) = x_eps(j) + epsilon_; 83 | fn = f(x_eps, u); 84 | 85 | for (int i = 0; i < nStates_; i ++) { 86 | F_(i, j) = (fn(i) - f0(i)) / epsilon_; 87 | } 88 | } 89 | 90 | //F_.print(); 91 | } 92 | 93 | void EKF::CalcH(const colvec &x) 94 | { 95 | colvec h0 = h(x); 96 | 97 | colvec hn; 98 | 99 | for (int j = 0; j < nStates_; j ++) { 100 | colvec x_eps = x; 101 | x_eps(j) = x_eps(j) + epsilon_; 102 | hn = h(x_eps); 103 | 104 | for (int i = 0; i < nOutputs_; i ++) 105 | H_(i, j) = (hn(i) - h0(i)) / epsilon_; 106 | } 107 | 108 | //H_.print(); 109 | } 110 | 111 | void EKF::InitSystemState(const colvec& x0) 112 | { 113 | arma_assert_same_size(x_.n_rows, x_.n_cols, x0.n_rows, x0.n_cols, "Whoops, error initializing system states"); 114 | x_ = x0; 115 | x_m_ = x0; 116 | } 117 | 118 | void EKF::InitSystemStateCovariance(const mat& P0) 119 | { 120 | arma_assert_same_size(P0.n_rows, P0.n_cols, P_m_.n_rows, P_m_.n_cols, "Whoops, error initializing state covariance"); 121 | P_m_ = P0; 122 | } 123 | 124 | 125 | void EKF::EKalmanf(const colvec& u) 126 | { 127 | // Simulate true system, with noise 128 | // randn uses a normal/Gaussian distribution with zero mean and unit variance 129 | v_.randn(F_.n_rows); 130 | w_.randn(H_.n_rows); 131 | v_ = sqrt_Q_ * v_; 132 | w_ = sqrt_R_ * w_; 133 | x_ = f(x_, u) + v_; 134 | z_ = h(x_) + w_; 135 | 136 | CalcF(x_m_, u); 137 | 138 | // Prior update: 139 | x_p_ = f(x_m_, u); 140 | P_p_ = F_ * P_m_ * trans(F_) + Q_; 141 | 142 | CalcH(x_p_); 143 | 144 | // Measurement update: 145 | mat K = P_p_ * trans(H_) * inv(H_ * P_p_ * trans(H_) + R_); 146 | x_m_ = x_p_ + K * (z_ - h(x_p_)); 147 | P_m_ = P_p_ - K * H_ * P_p_; 148 | 149 | // Estimated output is the projection of etimated states to the output function 150 | z_m_ = h(x_m_); 151 | } 152 | 153 | void EKF::EKalmanf(const colvec& z, const colvec& u) 154 | { 155 | CalcF(x_m_, u); 156 | 157 | // Prior update: 158 | x_p_ = f(x_m_, u); 159 | P_p_ = F_ * P_m_ * trans(F_) + Q_; 160 | 161 | CalcH(x_p_); 162 | 163 | // Measurement update: 164 | mat K = P_p_ * trans(H_) * inv(H_ * P_p_ * trans(H_) + R_); 165 | x_m_ = x_p_ + K * (z - h(x_p_)); 166 | P_m_ = P_p_ - K * H_ * P_p_; 167 | 168 | // Estimated output is the projection of etimated states to the output function 169 | z_m_ = h(x_m_); 170 | } 171 | 172 | colvec* EKF::GetCurrentState() 173 | { 174 | return & x_; 175 | } 176 | 177 | colvec* EKF::GetCurrentOutput() 178 | { 179 | return &z_; 180 | } 181 | 182 | colvec* EKF::GetCurrentEstimatedState() 183 | { 184 | return &x_m_; 185 | } 186 | 187 | 188 | colvec* EKF::GetCurrentEstimatedOutput() 189 | { 190 | return &z_m_; 191 | } 192 | 193 | 194 | 195 | -------------------------------------------------------------------------------- /kalman/ekf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ekf.h 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * @brief Header file for the extended Kalman filter implementation, for a nonlinear system. 6 | * 7 | * @section DESCRIPTION 8 | * Define a non-linear discrete-time process: 9 | * \f[x_k = f(x_{k-1}, u_{k-1}) + v_{k-1}\f] 10 | * \f[z_k = h(x_k) + w_k\f] 11 | * where:\n 12 | * \f$f\f$ is the dynamic model of the system\n 13 | * \f$h\f$ is the measurement model of the system\n 14 | * \f$v\f$ is the process noise (Gaussian with covariance Q)\n 15 | * \f$w\f$ is the measurement noise (Gaussian with covariance R)\n 16 | * \f$x\f$ is the state vector\n 17 | * \f$z\f$ is the output vector\n 18 | * \f$u\f$ is the input vector\n 19 | */ 20 | 21 | #ifndef EKF_H 22 | #define EKF_H 23 | 24 | #define ARMA_USE_LAPACK // This definition must be before the include to the armadillo 25 | 26 | 27 | #define _USE_MATH_DEFINES 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | using namespace std; 35 | using namespace arma; 36 | 37 | /*! 38 | * @brief Implemetation of the extended Kalman filter. 39 | * This class needs to be derived. 40 | */ 41 | class EKF { 42 | public: 43 | /*! 44 | * \brief Constructor, nothing happens here. 45 | */ 46 | EKF(); 47 | 48 | /*! 49 | * \brief Destructur, nothing happens here. 50 | */ 51 | ~EKF(); 52 | 53 | /*! 54 | * \brief Tell me how many states and outputs you have! 55 | * @param n_states Number of the elements on the input vector x 56 | * @param n_outputs Number of the elements on the otput vector z 57 | * @param Q Process noise covariance 58 | * @param R Measurement noise covariance 59 | */ 60 | void InitSystem(int n_states, int n_outputs, const mat& Q, const mat& R); 61 | /*! 62 | * \brief Define model of your system. 63 | * @param x System states 64 | * @param u System inputs 65 | */ 66 | virtual colvec f(const colvec &x, const colvec &u); 67 | 68 | /*! 69 | * \brief Define the output model of your system. 70 | * @param x System states 71 | */ 72 | virtual colvec h(const colvec &x); 73 | /*! 74 | * \brief Initialize the system states. 75 | * Must be called after InitSystem. 76 | * If not called, system state is initialized to zero. 77 | * @param x0 Inital value for the system state 78 | */ 79 | void InitSystemState(const colvec& x0); 80 | 81 | /*! 82 | * \brief Initialize the system state covariance. 83 | * Must be called after InitSystem. 84 | * If not called, state covariance is initialized to an identity matrix. 85 | * @param P0 Inital value for the state covariance 86 | */ 87 | void InitSystemStateCovariance(const mat& P0); 88 | 89 | /*! 90 | * \brief Do the extended Kalman iteration step-by-step while simulating the system. 91 | * Simulating the system is done to calculate system states and outputs. 92 | * @param u The applied input to the system 93 | */ 94 | void EKalmanf(const colvec& u); 95 | 96 | /*! 97 | * \brief Do the extended Kalman iteration step-by-step without simulating the system. 98 | * Use this if measurement is available and simulating the system is unnecessary. 99 | * Here, true system states and system outputs do not matter. 100 | * The only thing that matters is the estimated states. 101 | * @param z The measurement outputs, this is a returned value 102 | * @param u The applied input to the system 103 | */ 104 | void EKalmanf(const colvec& z, const colvec& u); 105 | 106 | /*! 107 | * @brief Get current simulated true state. 108 | * @return Current simulated state of the system \f$x_k\f$ 109 | */ 110 | colvec* GetCurrentState(); 111 | 112 | /*! 113 | * @brief Get current simulated true output. 114 | * This is analogous to the measurements. 115 | * @return Current simulated output \f$z_k\f$ 116 | */ 117 | colvec* GetCurrentOutput(); 118 | 119 | /*! 120 | * @brief Get current estimated state. 121 | * This is analogous to the filtered measurements. 122 | * @return Current estimated state \f$\hat{x}_k\f$ 123 | */ 124 | colvec* GetCurrentEstimatedState(); 125 | 126 | /*! 127 | * @brief Get current estimated output. 128 | * @return Current estimated output \f$\hat{z}_k\f$ 129 | */ 130 | colvec* GetCurrentEstimatedOutput(); 131 | 132 | private: 133 | /*! 134 | * \brief Compute the Jacobian of f numerically using a small 135 | * finite-difference perturbation magnitude. 136 | * @param x System states 137 | * @param u Input vector 138 | */ 139 | void CalcF(const colvec &x, const colvec &u); 140 | 141 | /*! 142 | * \brief Compute the Jacobian of h numerically using a small 143 | * finite-difference perturbation magnitude. 144 | * @param x System states 145 | */ 146 | void CalcH(const colvec &x); 147 | 148 | mat F_; ///< Jacobian of F 149 | mat H_; ///< Jacobian of H 150 | mat Q_; ///< Process noise covariance 151 | mat R_; ///< Measurement noise covariance 152 | colvec v_; ///< Gaussian process noise 153 | colvec w_; ///< Gaussian measurement noise 154 | 155 | mat sqrt_Q_; ///< Process noise stdev 156 | mat sqrt_R_; ///< Measurement noise stdev 157 | 158 | 159 | 160 | colvec x_m_; ///< State vector after measurement update 161 | colvec x_p_; ///< State vector after a priori update 162 | 163 | mat P_p_; ///< State covariance after a priori update 164 | mat P_m_; ///< State covariance after measurement update 165 | 166 | colvec z_m_; ///< Estimated output 167 | 168 | double epsilon_; ///< Very small number 169 | 170 | protected: 171 | 172 | int nStates_; ///< Number of the states 173 | int nOutputs_; ///< Number of outputs 174 | 175 | colvec x_; ///< State vector 176 | colvec z_; ///< Output matrix 177 | }; 178 | 179 | 180 | #endif 181 | -------------------------------------------------------------------------------- /kalman/fx.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fx.cpp 3 | * @author Auralius Manurung 4 | * @date 28 Jan 2017 5 | * @brief Implement a user defined mathematical function. 6 | */ 7 | 8 | #include "fx.h" 9 | 10 | FX::FX(colvec(*f) (colvec &x, colvec &some_constants)) 11 | { 12 | // Epsilon for computing the Jacobian numerically 13 | Epsilon_ = 1e-5; 14 | 15 | // Remember the callback function 16 | F_ = f; 17 | } 18 | 19 | FX::~FX() 20 | { 21 | } 22 | 23 | colvec FX::SolveAt(colvec &x, colvec &some_constants) 24 | { 25 | colvec ret = F_(x, some_constants); 26 | return ret; 27 | } 28 | 29 | mat FX::JacobianAt(colvec &x, colvec &some_constants) 30 | { 31 | colvec f0 = F_(x, some_constants); 32 | colvec fn; 33 | 34 | uword c = x.size(); 35 | uword r = f0.size(); 36 | mat jac; 37 | jac.resize(r, c); 38 | 39 | for (uword j = 0; j < c; j++) { 40 | colvec x_eps = x; 41 | x_eps(j) = x_eps(j) + Epsilon_; 42 | fn = F_(x_eps, some_constants); 43 | 44 | for (uword i = 0; i < r; i++) { 45 | jac(i, j) = (fn(i) - f0(i)) / Epsilon_; 46 | } 47 | } 48 | 49 | //jac.print("Jac="); 50 | return jac; 51 | } 52 | 53 | mat FX::HessianAt(colvec &x, colvec &some_constants, int i) 54 | { 55 | uword n = x.size(); 56 | mat hess; 57 | hess.resize(n, n); 58 | 59 | for (uword j = 0; j < n; j++) { 60 | colvec x0 = x; 61 | 62 | x0(j) = x0(j) + Epsilon_; 63 | mat hess_plus = JacobianAt(x0, some_constants); 64 | 65 | x0 = x; 66 | x0(j) = x0(j) - Epsilon_; 67 | mat hess_min = JacobianAt(x0, some_constants); 68 | 69 | rowvec delta = (hess_plus.row(i) - hess_min.row(i)) / (2 * Epsilon_); 70 | hess.row(j) = delta; 71 | } 72 | 73 | //hess.print("Hessian="); 74 | return hess; 75 | } 76 | 77 | void FX::SetEpsilon(double epsilon) 78 | { 79 | Epsilon_ = epsilon; 80 | } 81 | -------------------------------------------------------------------------------- /kalman/fx.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file fx.h 3 | * @author Auralius Manurung 4 | * @date 28 Jan 2017 5 | * @brief A header file for a user defined mathematical function. 6 | * 7 | * @section DESCRIPTION 8 | * Example: 9 | * A function of vector, with 3 input parameters and 2 output parameters. 10 | * \f[y = \begin{bmatrix} f_1(x_1,x_2,x_3) \\ f_2(x_1,x_2,x_3) \end{bmatrix} \f] 11 | * \f[Jac(y)=\begin{bmatrix} \frac{\partial f_1}{\partial x_1} \frac{\partial f_1}{\partial x_2} \frac{\partial f_1}{\partial x_3} \\ 12 | * \frac{\partial f_2}{\partial x_1} \frac{\partial f_2}{\partial x_2} \frac{\partial f_2}{\partial x_3} 13 | * \end{bmatrix} \f] 14 | * 15 | * \f[Hess(y(1)) =\begin{bmatrix} \frac{\partial^2 f_1}{\partial x_1^2} \frac{\partial^2 f_1}{\partial x_1 \partial x_2} \frac{\partial^2 f_1}{\partial x_1 \partial x_3} \\ 16 | * \frac{\partial^2 f_1}{\partial x_2 \partial x_1} \frac{\partial^2 f_1}{\partial x_2^2} \frac{\partial^2 f_1}{\partial x_2 \partial x_3} \\ 17 | * \frac{\partial^2 f_1}{\partial x_3 \partial x_1} \frac{\partial^2 f_1}{\partial x_3 \partial x_2} \frac{\partial^2 f_1}{\partial x_3^2} 18 | * \end{bmatrix} \f] 19 | * 20 | * \f[Hess(y(2)) =\begin{bmatrix} \frac{\partial^2 f_2}{\partial x_1^2} \frac{\partial^2 f_2}{\partial x_1 \partial x_2} \frac{\partial^2 f_2}{\partial x_1 \partial x_3} \\ 21 | * \frac{\partial^2 f_2}{\partial x_2 \partial x_1} \frac{\partial^2 f_2}{\partial x_2^2} \frac{\partial^2 f_2}{\partial x_2 \partial x_3} \\ 22 | * \frac{\partial^2 f_2}{\partial x_3 \partial x_1} \frac{\partial^2 f_2}{\partial x_3 \partial x_2} \frac{\partial^2 f_2}{\partial x_3^2} 23 | * \end{bmatrix} \f] 24 | * 25 | */ 26 | 27 | #ifndef FX_H 28 | #define FX_H 29 | 30 | #define ARMA_USE_LAPACK // This definition must be before the include to the armadillo 31 | 32 | 33 | #define _USE_MATH_DEFINES 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | using namespace std; 41 | using namespace arma; 42 | 43 | class FX 44 | { 45 | public: 46 | /*! 47 | * \brief Constructor, create a mathematical function. 48 | * \param f Address to the user defined mathematical function. 49 | */ 50 | FX(colvec(*f) (colvec &x, colvec &some_constants)); 51 | 52 | /*! 53 | * \brief Destructor, nothing happens here. 54 | */ 55 | ~FX(); 56 | 57 | /*! 58 | * \brief Calculate the Jacobian at certain inputs. 59 | * \param x Location where the Jacobian is computed. 60 | * \param some_constants Optional constants used in the function. 61 | * \return Jacobian at location x 62 | */ 63 | mat JacobianAt(colvec &x, colvec &some_constants); 64 | 65 | /*! 66 | * \brief Calculate the Hessian, at certain inputs. 67 | * \param x Location where the Hessian is computed. 68 | * \param some_constants Optional constants used in the function. 69 | * \param i For a function of vector, do Hessian at i-th element of the vector. 70 | * \return Hessian at location x. 71 | */ 72 | mat HessianAt(colvec &x, colvec &some_constants, int i); 73 | 74 | /*! 75 | * \brief Solve the function at certain inputs. 76 | * \param x Location where the function is solved. 77 | * \param some_constants Optional constants used in the function. 78 | * \return Result from solving the function. 79 | */ 80 | colvec SolveAt(colvec &x, colvec &some_constants); 81 | 82 | /*! 83 | * \brief A very small number. 84 | * \param epsilon A very small number. 85 | */ 86 | void SetEpsilon(double epsilon); 87 | 88 | private: 89 | colvec (*F_) (colvec &x, colvec &some_constants); ///< Remember the address of the callback function. 90 | 91 | double Epsilon_; ///< Number of the states 92 | }; 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /kalman/kf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file kf.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * @brief Kalman filter implementation, for a linear system. 6 | */ 7 | 8 | #include "kf.h" 9 | 10 | KF::KF() 11 | { 12 | 13 | } 14 | 15 | KF::~KF() 16 | { 17 | 18 | } 19 | 20 | void KF::InitSystem(const mat& A, const mat& B, const mat& H, const mat& Q, const mat& R) 21 | { 22 | A_ = A; 23 | B_ = B; 24 | H_ = H; 25 | Q_ = Q; 26 | R_ = R; 27 | 28 | // Stdev is sqrt of variance 29 | sqrt_Q_ = sqrt(Q_); 30 | sqrt_R_ = sqrt(R_); 31 | 32 | int n_states = A.n_cols; 33 | int n_outputs = H.n_rows; 34 | 35 | assert(A.is_square() && "Whoops, A must be a square matrix (n_outputs x n_outputs)"); 36 | assert(B.n_rows == A.n_rows && "Whoops, B has wrong dimension"); 37 | assert(Q.is_square() && "Whoops, Q must be a square matrix"); 38 | assert(R.is_square() && "Whoops, R must be a square matrix (n_outputs x n_outputs)"); 39 | 40 | // Apply intial states 41 | x_.resize(n_states); 42 | x_ = x_.zeros(); 43 | 44 | x_p_.resize(n_states); 45 | x_m_.resize(n_states); 46 | 47 | P_p_.resize(n_states, n_states); 48 | P_m_.resize(n_states, n_states); 49 | 50 | v_.resize(n_states); 51 | w_.resize(n_outputs); 52 | 53 | // Inital values: 54 | P_m_ = P_m_.eye(); 55 | x_m_ = x_m_.zeros(); 56 | } 57 | 58 | void KF::InitSystemState(const colvec& x0) 59 | { 60 | arma_assert_same_size(x_.n_rows, x_.n_cols, x0.n_rows, x0.n_cols, "Whoops, error initializing system states"); 61 | x_ = x0; 62 | x_m_ = x0; 63 | } 64 | 65 | void KF::InitStateCovariance(const mat& P0) 66 | { 67 | arma_assert_same_size(P_m_.n_rows, P_m_.n_cols, P0.n_rows, P0.n_cols, "Whoops, error initializing state covariance"); 68 | P_m_ = P0; 69 | } 70 | 71 | void KF::Kalmanf(const colvec& u) 72 | { 73 | // Simulate true system, with noise 74 | // randn uses a normal/Gaussian distribution with zero mean and unit variance 75 | v_.randn(A_.n_rows); 76 | w_.randn(H_.n_rows); 77 | v_ = sqrt_Q_ * v_; 78 | w_ = sqrt_R_ * w_; 79 | x_ = A_ * x_ + B_ * u + v_; 80 | z_ = H_ * x_ + w_; 81 | 82 | // Prior update: 83 | x_p_ = A_ * x_m_ + B_ * u; 84 | P_p_ = A_ * P_m_ * trans(A_) + Q_; 85 | 86 | // Measurement update: 87 | mat K = P_p_ * trans(H_) * inv(H_ * P_p_ * trans(H_) + R_); 88 | x_m_ = x_p_ + K * (z_ - H_ * x_p_); 89 | P_m_ = P_p_ - K * H_ * P_p_; 90 | 91 | // Estimated output is the projection of etimated states to the output function 92 | z_m_ = H_ * x_m_; 93 | } 94 | 95 | void KF::Kalmanf(const colvec& z, const colvec& u) 96 | { 97 | // Prior update: 98 | x_p_ = A_ * x_m_ + B_ * u; 99 | P_p_ = A_ * P_m_ * trans(A_) + Q_; 100 | 101 | // Measurement update: 102 | mat K = P_p_ * trans(H_) * inv(H_ * P_p_ * trans(H_) + R_); 103 | x_m_ = x_p_ + K * (z - H_ * x_p_); 104 | P_m_ = P_p_ - K * H_ * P_p_; 105 | 106 | // Estimated output is the projection of etimated states to the output function 107 | z_m_ = H_ * x_m_; 108 | } 109 | 110 | colvec* KF::GetCurrentState() 111 | { 112 | return & x_; 113 | } 114 | 115 | colvec* KF::GetCurrentOutput() 116 | { 117 | return &z_; 118 | } 119 | 120 | colvec* KF::GetCurrentEstimatedState() 121 | { 122 | return &x_m_; 123 | } 124 | 125 | colvec* KF::GetCurrentEstimatedOutput() 126 | { 127 | return &z_m_; 128 | } 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /kalman/kf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file kf.h 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * @brief Header file for the Kalman filter, for a linear system. 6 | * 7 | * @section DESCRIPTION 8 | * Define a system: 9 | * \f[x_k = Ax_{k-1} + Bu_{k-1} + v_{k-1}\f] 10 | * \f[z_k = Hx_k + w_k\f] 11 | * where:\n 12 | * \f$v\f$ is the process noise (Gaussian with covariance Q)\n 13 | * \f$w\f$ is the measurement noise (Gaussian with covariance R)\n 14 | * \f$A\f$ is the system matrix\n 15 | * \f$B\f$ is the input matrix\n 16 | * \f$H\f$ is the output matrix\n 17 | * \f$x\f$ is the state vector\n 18 | * \f$z\f$ is the output vector\n 19 | * \f$u\f$ is the input vector\n 20 | */ 21 | 22 | #ifndef KF_H 23 | #define KF_H 24 | 25 | #define ARMA_USE_LAPACK // This definition must be before the include to the armadillo 26 | 27 | 28 | #define _USE_MATH_DEFINES 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | using namespace std; 36 | using namespace arma; 37 | 38 | /*! 39 | * @brief Kalman filter implementation, for a linear system. 40 | */ 41 | class KF { 42 | public: 43 | /*! 44 | * \brief Constructor, nothing happens here. 45 | */ 46 | KF(); 47 | /*! 48 | * \brief Destructor, nothing happens here. 49 | */ 50 | ~KF(); 51 | 52 | /*! 53 | * @brief Define the system. 54 | * @param A System matrix 55 | * @param B Input matrix 56 | * @param H Output matrix 57 | * @param Q Process noise covariance 58 | * @param R Measurement noise covariance 59 | */ 60 | void InitSystem (const mat& A, const mat& B, const mat& H, const mat& Q, const mat& R); 61 | 62 | /*! 63 | * @brief Initialize the system states. 64 | * Must be called after InitSystem. 65 | * If not, called, system states are initialized to zero. 66 | * @param x0 Inital value for the system state 67 | */ 68 | void InitSystemState(const colvec& x0); 69 | 70 | /*! 71 | * @brief Initialize the state covariance. 72 | * Must be called after InitSystem. 73 | * If not called, covariance state is Initialized to an identity matrix. 74 | * @param P0 Inital value for the state covariance 75 | */ 76 | void InitStateCovariance(const mat& P0); 77 | 78 | /*! 79 | * @brief Do Kalman filter iteration step-by-step while simulating the system. 80 | * Simulating the system is done to calculate system states and outputs. 81 | * @param u tThe applied input to the system 82 | */ 83 | void Kalmanf(const colvec& u); 84 | 85 | /*! 86 | * @brief Do Kalman filter iteration step-by-step without simulating the system. 87 | * Use this if measurement is available and simulating the system is unnecessary. 88 | * Here, true system states and system outputs do not matter. 89 | * The only thing that matters is the estimated states. 90 | * @param z The values of the output from measurement 91 | * @param u The applied input to the system 92 | */ 93 | void Kalmanf(const colvec& z, const colvec& u); 94 | 95 | /*! 96 | * @brief Get current simulated true state. 97 | * @return Current simulated state $x_k$ 98 | */ 99 | colvec* GetCurrentState(); 100 | 101 | /*! 102 | * @brief Get current simulated true output. 103 | * This is analogous to the measurements. 104 | * @return Current simulated output $z_k$ 105 | */ 106 | colvec* GetCurrentOutput(); 107 | 108 | /*! 109 | * @brief Get current estimated state. 110 | * @return Current estimated state $\hat{x}_k$ 111 | */ 112 | colvec* GetCurrentEstimatedState(); 113 | 114 | /*! 115 | * @brief Get current estimated output. 116 | * This is the filtered measurements, with less noise. 117 | * @return Current estimated output $\hat{z}_k$ 118 | */ 119 | colvec* GetCurrentEstimatedOutput(); 120 | 121 | private: 122 | 123 | mat A_; ///< System matrix 124 | mat B_; ///< Input matrix 125 | mat H_; ///< Output matrix 126 | mat Q_; ///< Process noise covariance 127 | mat R_; ///< Measurement noise covariance 128 | colvec v_; ///< Gaussian process noise 129 | colvec w_; ///< Gaussian measurement noise 130 | 131 | mat sqrt_Q_; ///< Process noise stdev 132 | mat sqrt_R_; ///< Measurement noise stdev 133 | 134 | colvec x_; ///< State vector 135 | colvec z_; ///< Output matrix 136 | 137 | colvec x_m_; ///< State vector after measurement update 138 | colvec x_p_; ///< State vector after a priori update 139 | 140 | mat P_p_; ///< State covariance after a priori update 141 | mat P_m_; ///< State covariance after measurement update 142 | 143 | colvec z_m_; ///< Estimated output 144 | }; 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /kalman/ukf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ukf.h 3 | * @author Auralius Manurung 4 | * @date 13 Peb 2021 5 | * @brief Header file for the Unscented Kalman filter (UKF). 6 | * 7 | * @section DESCRIPTION 8 | * The implementation is based on the paper: Wan, E. A., & Van Der Merwe, 9 | * R. (2006). The unscented Kalman filter for nonlinear estimation. 10 | * Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, 11 | * Communications, and Control Symposium (Cat. No.00EX373), 31(2), 12 | * 153–158. https://doi.org/10.1109/ASSPCC.2000.882463\n\n 13 | * Define a non-linear discrete-time process: 14 | * \f[x_k = f(x_{k-1}, u_{k-1}) + v_{k-1}\f] 15 | * \f[z_k = h(x_k) + w_k\f] 16 | * where:\n 17 | * \f$f\f$ is the dynamic model of the system\n 18 | * \f$h\f$ is the measurement model of the system\n 19 | * \f$v\f$ is the process noise (Gaussian with covariance Q)\n 20 | * \f$w\f$ is the measurement noise (Gaussian with covariance R)\n 21 | * \f$x\f$ is the state vector\n 22 | * \f$z\f$ is the output vector\n 23 | * \f$u\f$ is the input vector\n 24 | */ 25 | 26 | #ifndef EKF_H 27 | #define EKF_H 28 | 29 | #define ARMA_USE_LAPACK // This definition must be before the include to the armadillo 30 | 31 | 32 | #define _USE_MATH_DEFINES 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | using namespace std; 40 | using namespace arma; 41 | 42 | /*! 43 | * @brief Implemetation of the Unscented Kalman filter. 44 | * This class needs to be derived. 45 | */ 46 | class UKF { 47 | public: 48 | /*! 49 | * \brief Constructor, nothing happens here. 50 | */ 51 | UKF(); 52 | 53 | /*! 54 | * \brief Destructur, nothing happens here. 55 | */ 56 | ~UKF(); 57 | 58 | /*! 59 | * \brief Tell me how many states and outputs you have! 60 | * @param n_states Number of the elements on the input vector x 61 | * @param n_outputs Number of the elements on the otput vector z 62 | * @param Q Process noise covariance 63 | * @param R Measurement noise covariance 64 | */ 65 | void InitSystem(int n_states, int n_outputs, const mat& Q, const mat& R); 66 | /*! 67 | * \brief Define model of your system. 68 | * @param x System states 69 | * @param u System inputs 70 | */ 71 | virtual colvec f(const colvec &x, const colvec &u); 72 | 73 | /*! 74 | * \brief Define the output model of your system. 75 | * @param x System states 76 | */ 77 | virtual colvec h(const colvec &x); 78 | /*! 79 | * \brief Initialize the system states. 80 | * Must be called after InitSystem. 81 | * If not called, system state is initialized to zero. 82 | * @param x0 Inital value for the system state 83 | */ 84 | void InitSystemState(const colvec& x0); 85 | 86 | /*! 87 | * \brief Initialize the system state covariance. 88 | * Must be called after InitSystem. 89 | * If not called, state covariance is initialized to an identity matrix. 90 | * @param P0 Inital value for the state covariance 91 | */ 92 | void InitSystemStateCovariance(const mat& P0); 93 | 94 | /*! 95 | * \brief Do the extended Kalman iteration step-by-step while simulating the system. 96 | * Simulating the system is done to calculate system states and outputs. 97 | * @param u The applied input to the system 98 | */ 99 | void UKalmanf(const colvec& u); 100 | 101 | /*! 102 | * \brief Do the extended Kalman iteration step-by-step without simulating the system. 103 | * Use this if measurement is available and simulating the system is unnecessary. 104 | * Here, true system states and system outputs do not matter. 105 | * The only thing that matters is the estimated states. 106 | * @param z The measurement outputs, this is a returned value 107 | * @param u The applied input to the system 108 | */ 109 | void UKalmanf(const colvec& z, const colvec& u); 110 | 111 | /*! 112 | * @brief Get current simulated true state. 113 | * @return Current simulated state of the system \f$x_k\f$ 114 | */ 115 | colvec* GetCurrentState(); 116 | 117 | /*! 118 | * @brief Get current simulated true output. 119 | * This is analogous to the measurements. 120 | * @return Current simulated output \f$z_k\f$ 121 | */ 122 | colvec* GetCurrentOutput(); 123 | 124 | /*! 125 | * @brief Get current estimated state. 126 | * This is analogous to the filtered measurements. 127 | * @return Current estimated state \f$\hat{x}_k\f$ 128 | */ 129 | colvec* GetCurrentEstimatedState(); 130 | 131 | /*! 132 | * @brief Get current estimated output. 133 | * @return Current estimated output \f$\hat{z}_k\f$ 134 | */ 135 | colvec* GetCurrentEstimatedOutput(); 136 | 137 | private: 138 | 139 | mat Q_; ///< Process noise covariance 140 | mat R_; ///< Measurement noise covariance 141 | colvec v_; ///< Gaussian process noise 142 | colvec w_; ///< Gaussian measurement noise 143 | 144 | mat sqrt_Q_; ///< Process noise standard deviation 145 | mat sqrt_R_; ///< Measurement noise standard deviation 146 | 147 | mat P_; ///< Covariance matrix 148 | colvec x_m_; ///< Estimated states 149 | colvec z_m_; ///< Estimated outputs 150 | 151 | protected: 152 | 153 | int nStates_; ///< Number of the states 154 | int nOutputs_; ///< Number of outputs 155 | 156 | colvec x_; ///< State vector, by simulaiton, true value 157 | colvec z_; ///< Output matrix, by simulation, true value 158 | }; 159 | 160 | 161 | #endif 162 | -------------------------------------------------------------------------------- /m-files/plot_data1.m: -------------------------------------------------------------------------------- 1 | close all; 2 | load ../bin/log_file1.txt; 3 | 4 | h = figure; 5 | 6 | hold; 7 | plot(log_file1(:,1), log_file1(:,2), 'b','LineWidth',2); 8 | plot(log_file1(:,1), log_file1(:,3), 'r','LineWidth',2); 9 | plot(log_file1(:,1), log_file1(:,4), 'g','LineWidth',2); 10 | 11 | legend('measurement', 'true', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data10.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | load ../bin/log_file10.txt; 4 | 5 | % This is the data format: 6 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] 7 | % true2 [TAB] estimate2 [TAB] 8 | % true3 [TAB] estimate3[TAB] [EOL] 9 | 10 | figure; 11 | 12 | subplot(3, 1, 1); 13 | hold on; 14 | plot(log_file10(:,1), log_file10(:,2), 'b'); 15 | plot(log_file10(:,1), log_file10(:,3), '--r'); 16 | ylabel('x_1'); 17 | 18 | 19 | subplot(3, 1, 2); 20 | hold on; 21 | plot(log_file10(:,1), log_file10(:,4), 'b'); 22 | plot(log_file10(:,1), log_file10(:,5), '--r'); 23 | ylabel('x_2'); 24 | 25 | subplot(3, 1, 3); 26 | hold on; 27 | plot(log_file10(:,1), log_file10(:,6), 'b'); 28 | plot(log_file10(:,1), log_file10(:,7), '--r'); 29 | xlabel('n-th iteration'); 30 | ylabel('x_3'); 31 | 32 | legend('true', 'estimate'); 33 | 34 | figure 35 | hold on; 36 | plot(log_file10(:,1), log_file10(:,8), 'b'); 37 | plot(log_file10(:,1), log_file10(:,9), '--r'); 38 | xlabel('n-th iteration'); 39 | ylabel('z'); 40 | 41 | legend('measurement', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data11.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | load ../bin/log_file11.txt; 4 | 5 | % This is the data format: 6 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] 7 | % true2 [TAB] estimate2 [TAB] 8 | % true3 [TAB] estimate3[TAB] [EOL] 9 | 10 | figure; 11 | 12 | subplot(3, 1, 1); 13 | hold on; 14 | plot(log_file11(:,1), log_file11(:,2), 'b'); 15 | plot(log_file11(:,1), log_file11(:,3), '--r'); 16 | ylabel('x_1'); 17 | 18 | 19 | subplot(3, 1, 2); 20 | hold on; 21 | plot(log_file11(:,1), log_file11(:,4), 'b'); 22 | plot(log_file11(:,1), log_file11(:,5), '--r'); 23 | ylabel('x_2'); 24 | 25 | subplot(3, 1, 3); 26 | hold on; 27 | plot(log_file11(:,1), log_file11(:,6), 'b'); 28 | plot(log_file11(:,1), log_file11(:,7), '--r'); 29 | xlabel('n-th iteration'); 30 | ylabel('x_3'); 31 | 32 | legend('true', 'estimate'); 33 | 34 | figure 35 | hold on; 36 | plot(log_file11(:,1), log_file11(:,8), 'b'); 37 | plot(log_file11(:,1), log_file11(:,9), '--r', 'LineWidth', 2); 38 | xlabel('n-th iteration'); 39 | ylabel('z'); 40 | 41 | legend('measurement', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data2.m: -------------------------------------------------------------------------------- 1 | close all; 2 | load ../bin/log_file2.txt; 3 | 4 | subplot(2,1,1); 5 | hold; 6 | plot(log_file2(:,1), log_file2(:,2), 'b'); 7 | plot(log_file2(:,1), log_file2(:,3), 'r'); 8 | plot(log_file2(:,1), log_file2(:,6), 'g'); 9 | xlabel('position'); 10 | legend('true', 'estimate', 'measured'); 11 | 12 | subplot(2,1,2); 13 | hold 14 | plot(log_file2(:,1), log_file2(:,4), 'b'); 15 | plot(log_file2(:,1), log_file2(:,5), 'r'); 16 | xlabel('velocity'); 17 | 18 | legend('true', 'estimate'); 19 | 20 | -------------------------------------------------------------------------------- /m-files/plot_data3.m: -------------------------------------------------------------------------------- 1 | close all; 2 | load ../bin/log_file3.txt; 3 | 4 | figure; 5 | 6 | subplot(3,1,1); 7 | hold; 8 | plot(log_file3(:,1), log_file3(:,2), 'b'); 9 | plot(log_file3(:,5), log_file3(:,6), 'r'); 10 | xlabel('x1'); 11 | ylabel('x2'); 12 | title('position'); 13 | legend('true', 'estimate'); 14 | 15 | subplot(3,1,2); 16 | hold; 17 | plot(log_file3(:,3), log_file3(:,4), 'b'); 18 | plot(log_file3(:,7), log_file3(:,8), 'r'); 19 | xlabel('x3'); 20 | ylabel('x4'); 21 | title('velocity'); 22 | legend('true', 'estimate'); 23 | 24 | subplot(3,1,3); 25 | hold; 26 | plot(log_file3(:,9), log_file3(:,10), 'b'); 27 | xlabel('x1'); 28 | ylabel('x2'); 29 | title('measured position'); 30 | -------------------------------------------------------------------------------- /m-files/plot_data4.m: -------------------------------------------------------------------------------- 1 | close all; 2 | load ../bin/log_file4.txt; 3 | 4 | % This is the data format: 5 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] measured1 [TAB] 6 | % true2 [TAB] estimate2 [TAB] measured2 [EOL] 7 | 8 | h = figure; 9 | 10 | subplot(2, 1, 1); 11 | hold; 12 | plot(log_file4(:,1), log_file4(:,2), '+b'); 13 | plot(log_file4(:,1), log_file4(:,3), 'r'); 14 | plot(log_file4(:,1), log_file4(:,4), 'g'); 15 | xlabel('Time'); 16 | ylabel('Position'); 17 | title('Extended Kalman Filtering of a Sine Wave - Position'); 18 | legend('Position Measurements', 'True Position', 'Position Estimates'); 19 | 20 | subplot(2, 1, 2); 21 | hold; 22 | plot(log_file4(:,1), log_file4(:,5), '+b'); 23 | plot(log_file4(:,1), log_file4(:,6), 'r'); 24 | plot(log_file4(:,1), log_file4(:,7), 'g'); 25 | xlabel('Time'); 26 | ylabel('Frequency'); 27 | title('Extended Kalman Filtering of a Sine Wave - Frequency'); 28 | legend('Frequency Measurements', 'True Frequency', 'Frequency Estimates'); 29 | 30 | saveas(h, 'ex4.png'); -------------------------------------------------------------------------------- /m-files/plot_data5.m: -------------------------------------------------------------------------------- 1 | close all; 2 | load ../bin/log_file5.txt; 3 | 4 | % This is the data format: 5 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] 6 | % true2 [TAB] estimate2 [TAB] 7 | % true3 [TAB] estimate3[TAB] [EOL] 8 | 9 | figure; 10 | 11 | subplot(3, 1, 1); 12 | hold on; 13 | plot(log_file5(:,1), log_file5(:,2), 'b'); 14 | plot(log_file5(:,1), log_file5(:,3), '--r'); 15 | ylabel('x_1'); 16 | 17 | 18 | subplot(3, 1, 2); 19 | hold on; 20 | plot(log_file5(:,1), log_file5(:,4), 'b'); 21 | plot(log_file5(:,1), log_file5(:,5), '--r'); 22 | ylabel('x_2'); 23 | 24 | subplot(3, 1, 3); 25 | hold on; 26 | plot(log_file5(:,1), log_file5(:,6), 'b'); 27 | plot(log_file5(:,1), log_file5(:,7), '--r'); 28 | xlabel('# iterations'); 29 | ylabel('x_3'); 30 | 31 | legend('true', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data6.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | load ../bin/log_file6.txt; 4 | 5 | % This is the data format: 6 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] 7 | % true2 [TAB] estimate2 [TAB] 8 | % true3 [TAB] estimate3[TAB] [EOL] 9 | 10 | figure; 11 | 12 | subplot(3, 1, 1); 13 | hold on; 14 | plot(log_file6(:,1), log_file6(:,2), 'b'); 15 | plot(log_file6(:,1), log_file6(:,3), '--r'); 16 | ylabel('x_1'); 17 | 18 | 19 | subplot(3, 1, 2); 20 | hold on; 21 | plot(log_file6(:,1), log_file6(:,4), 'b'); 22 | plot(log_file6(:,1), log_file6(:,5), '--r'); 23 | ylabel('x_2'); 24 | 25 | subplot(3, 1, 3); 26 | hold on; 27 | plot(log_file6(:,1), log_file6(:,6), 'b'); 28 | plot(log_file6(:,1), log_file6(:,7), '--r'); 29 | xlabel('# iterations'); 30 | ylabel('x_3'); 31 | 32 | legend('true', 'estimate'); 33 | 34 | figure 35 | hold on; 36 | plot(log_file6(:,1), log_file6(:,8), 'b'); 37 | plot(log_file6(:,1), log_file6(:,9), '--r'); 38 | xlabel('# iterations'); 39 | ylabel('z'); 40 | 41 | legend('measurement', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data7.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | load ../bin/log_file7.txt; 4 | 5 | % This is the data format: 6 | % iteration# [TAB] measurement [TAB] estimated [EOL] 7 | 8 | figure 9 | hold on; 10 | plot(log_file7(:,1), log_file7(:,2), 'b'); 11 | plot(log_file7(:,1), log_file7(:,3), '--r', 'LineWidth', 2); 12 | xlabel('# iterations'); 13 | ylabel('z'); 14 | 15 | legend('measurement', 'estimate'); -------------------------------------------------------------------------------- /m-files/plot_data8.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | load ../bin/log_file8.txt; 4 | 5 | % This is the data format: 6 | % iteration# [TAB] true1 [TAB] estimate1 [TAB] 7 | % true2 [TAB] estimate2 [TAB] 8 | % true3 [TAB] estimate3[TAB] [EOL] 9 | 10 | figure; 11 | 12 | subplot(3, 1, 1); 13 | hold on; 14 | plot(log_file8(:,1), log_file8(:,2), 'b'); 15 | plot(log_file8(:,1), log_file8(:,3), '--r', 'LineWidth',1.5); 16 | ylabel('x_1'); 17 | 18 | 19 | subplot(3, 1, 2); 20 | hold on; 21 | plot(log_file8(:,1), log_file8(:,4), 'b'); 22 | plot(log_file8(:,1), log_file8(:,5), '--r', 'LineWidth',1.5); 23 | ylabel('x_2'); 24 | 25 | subplot(3, 1, 3); 26 | hold on; 27 | plot(log_file8(:,1), log_file8(:,6), 'b'); 28 | plot(log_file8(:,1), log_file8(:,7), '--r', 'LineWidth',1.5); 29 | xlabel('# iterations'); 30 | ylabel('x_3'); 31 | 32 | legend('true', 'estimate'); 33 | 34 | figure 35 | hold on; 36 | plot(log_file8(:,1), log_file8(:,8), 'b'); 37 | plot(log_file8(:,1), log_file8(:,9), '--r', 'LineWidth',1.5); 38 | xlabel('# iterations'); 39 | ylabel('z'); 40 | 41 | legend('measurement', 'estimate'); -------------------------------------------------------------------------------- /samples/main0.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This is the smole test that comes with the Armadillo lib. 3 | * The purpose is to ensure that LAPACK is working well together with the Armadillo. 4 | * Make sure you can runs this test without any problems! 5 | */ 6 | 7 | #define ARMA_USE_LAPACK // This definition must be before the include to the armadillo 8 | 9 | #include 10 | #include 11 | 12 | using namespace arma; 13 | 14 | int 15 | main() 16 | { 17 | std::cout << "*** smoke test start" << std::endl; 18 | 19 | uword N = 5; 20 | 21 | mat A = reshape(regspace(1, N * N), N, N); 22 | 23 | A.diag() += randu(N); 24 | 25 | mat B; 26 | 27 | bool status = expmat(B, A); 28 | 29 | A.print("A:"); 30 | B.print("B:"); 31 | 32 | std::cout << ((status) ? "*** smoke test okay" : "*** smoke test failed") << std::endl; 33 | 34 | return (status) ? 0 : -1; 35 | } 36 | -------------------------------------------------------------------------------- /samples/main1.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main1.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * 6 | * @brief Example for the Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * A constant voltage of 12 V is measured as output of a system. This example is taken from 10 | * here. 11 | * 12 | */ 13 | 14 | #include 15 | 16 | #include "kf.h" 17 | 18 | 19 | int main(int argc, char** argv) 20 | { 21 | /* 22 | * Log the result into a tab delimitted file, later we can open 23 | * it with Matlab. Use: plot_data1.m to plot the results. 24 | */ 25 | ofstream log_file; 26 | #ifdef _WIN32 27 | log_file.open("..\\bin\\log_file1.txt"); 28 | #else 29 | log_file.open("log_file1.txt"); 30 | #endif 31 | 32 | /* 33 | * Define the system 34 | */ 35 | mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1); 36 | colvec u(1); 37 | colvec x0(1); 38 | 39 | A << 0; 40 | B << 1; 41 | Q << 2*2; 42 | H << 1; 43 | R << 2*2; 44 | u << 12.0; 45 | 46 | KF kalman; 47 | kalman.InitSystem(A, B, H, Q, R); 48 | 49 | for (int k = 0; k < 100; k++) { 50 | kalman.Kalmanf(u); 51 | 52 | colvec *x = kalman.GetCurrentState(); 53 | colvec *z = kalman.GetCurrentOutput(); 54 | colvec *x_m = kalman.GetCurrentEstimatedState(); 55 | log_file << k << '\t' << z->at(0,0) << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) << '\n'; 56 | } 57 | 58 | log_file.close(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /samples/main10.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main10.cpp 3 | * @author Auralius Manurung 4 | * @date 14 Peb 2021 5 | * 6 | * @brief An example for an Unscented Kalman filter (UKF). 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from here, "Learning the Unscented Kalman Filter" by Yi Cao. 10 | */ 11 | 12 | #include 13 | 14 | #include "ukf.h" 15 | 16 | /// @cond DEV 17 | /* 18 | * Class UKF needs to be derived, two virtual functions are provided in 19 | * which system model and output model are described. 20 | */ 21 | class MyUKF : public UKF 22 | { 23 | public: 24 | virtual colvec f(const colvec& x, const colvec& u) { 25 | colvec xk(nStates_); 26 | xk.at(0) = x(1); 27 | xk.at(1) = x(2); 28 | xk.at(2) = 0.05*x(0)*(x(1)+x(2)); 29 | //xk.print("xk="); 30 | return xk; 31 | } 32 | 33 | virtual colvec h(const colvec& x) { 34 | colvec zk(nOutputs_); 35 | zk(0) = x(0); 36 | return zk; 37 | } 38 | }; 39 | 40 | 41 | /// @endcond 42 | 43 | ///////////////////////////////////////////////////////////////////////////// 44 | 45 | int main(int argc, char** argv) 46 | { 47 | /* 48 | * Log the result into a tab delimitted file, later we can open 49 | * it with Matlab. Use: plot_data10.m to plot the results. 50 | */ 51 | ofstream log_file; 52 | #ifdef _WIN32 53 | log_file.open("..\\bin\\log_file10.txt"); 54 | #else 55 | log_file.open("log_file10.txt"); 56 | #endif 57 | 58 | mat Q(3, 3); 59 | mat R(1, 1); 60 | 61 | 62 | MyUKF myukf; 63 | 64 | double r = 0.1; 65 | double q = 0.1; 66 | 67 | Q = eye(3,3)*q*q; 68 | R << r*r << endr; 69 | 70 | colvec x0(3); 71 | x0 << 0 << 0 << 1; 72 | 73 | mat P0 = eye(3, 3); 74 | P0 = P0; 75 | 76 | colvec u; 77 | 78 | // No inputs 79 | u = u.zeros(); 80 | 81 | myukf.InitSystem(3, 1, Q, R); 82 | myukf.InitSystemState(x0); 83 | myukf.InitSystemStateCovariance(P0); 84 | 85 | for (int k = 0; k < 20; k++) { 86 | myukf.UKalmanf(u); 87 | 88 | colvec *x = myukf.GetCurrentState(); 89 | colvec *x_m = myukf.GetCurrentEstimatedState(); 90 | colvec *z = myukf.GetCurrentOutput(); 91 | colvec *z_m = myukf.GetCurrentEstimatedOutput(); 92 | 93 | log_file << k << '\t' << x->at(0, 0) << '\t' << x_m->at(0, 0) 94 | << '\t' << x->at(1, 0) << '\t' << x_m->at(1, 0) 95 | << '\t' << x->at(2, 0) << '\t' << x_m->at(2, 0) 96 | << '\t' << z->at(0, 0) << '\t' << z_m->at(0, 0) << '\t' 97 | << '\n'; 98 | } 99 | 100 | log_file.close(); 101 | 102 | return 0; 103 | } 104 | -------------------------------------------------------------------------------- /samples/main11.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main11.cpp 3 | * @author Auralius Manurung 4 | * @date 15 Peb 2021 5 | * 6 | * @brief An example for an Unscented Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from here, 10 | * section 3.2: "Tracking a random sine signal". 11 | */ 12 | 13 | #include 14 | 15 | #include "ukf.h" 16 | 17 | /// @cond DEV 18 | /* 19 | * Class UKF needs to be derived, two virtual functions are provided in 20 | * which system model and output model are described. 21 | */ 22 | class MyUKF: public UKF 23 | { 24 | public: 25 | virtual colvec f(const colvec& x, const colvec& u) { 26 | colvec xk(nStates_); 27 | mat A(3, 3); 28 | 29 | A << 1 << dt << 0 << endr 30 | << 0 << 1 << 0 << endr 31 | << 0 << 0 << 1 << endr; 32 | 33 | xk = A * x;; 34 | //xk.print("xk="); 35 | return xk; 36 | } 37 | 38 | virtual colvec h(const colvec& x) { 39 | colvec zk(nOutputs_); 40 | zk(0) = x(2)*sin(x(0)); 41 | return zk; 42 | } 43 | 44 | static const double dt; 45 | 46 | }; 47 | 48 | const double MyUKF::dt = 0.01; 49 | 50 | /// @endcond 51 | 52 | ///////////////////////////////////////////////////////////////////////////// 53 | 54 | int main(int argc, char** argv) 55 | { 56 | /* 57 | * Log the result into a tab delimitted file, later we can open 58 | * it with Matlab. Use: plot_data8.m to plot the results. 59 | */ 60 | ofstream log_file; 61 | #ifdef _WIN32 62 | log_file.open("..\\bin\\log_file11.txt"); 63 | #else 64 | log_file.open("log_file11.txt"); 65 | #endif 66 | 67 | int n_states = 3; 68 | int n_outputs = 1; 69 | mat Q(n_states, n_states); 70 | mat R(n_outputs, n_outputs); 71 | 72 | 73 | MyUKF myukf; 74 | 75 | double q1 = 0.2; 76 | double q2 = 0.1; 77 | double dt = myukf.dt; 78 | 79 | Q << q1*dt*dt*dt/3 << q1*dt*dt/2 << 0 << endr 80 | << q1*dt*dt/2 << q1*dt << 0 << endr 81 | << 0 << 0 << dt*q2 << endr; 82 | 83 | R << 1 << endr; 84 | 85 | colvec x0(n_states); 86 | x0 << 0 << 10 << 1; 87 | 88 | mat P0 = eye (3,3); 89 | P0 = 3 * P0; 90 | 91 | colvec u; 92 | 93 | // No inputs 94 | u = u.zeros(); 95 | 96 | myukf.InitSystem(n_states, n_outputs, Q, R); 97 | myukf.InitSystemState(x0); 98 | myukf.InitSystemStateCovariance(P0); 99 | 100 | for (int k = 0; k < 500; k ++) { 101 | myukf.UKalmanf(u); 102 | 103 | colvec *x = myukf.GetCurrentState(); 104 | colvec *x_m = myukf.GetCurrentEstimatedState(); 105 | colvec *z = myukf.GetCurrentOutput(); 106 | colvec *z_m = myukf.GetCurrentEstimatedOutput(); 107 | 108 | log_file << k << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) 109 | << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) 110 | << '\t' << x->at(2,0) << '\t' << x_m->at(2,0) 111 | << '\t' << z->at(0,0) << '\t' << z_m->at(0,0) << '\t' 112 | << '\n'; 113 | } 114 | 115 | log_file.close(); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /samples/main2.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main2.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * 6 | * @brief Example for the Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * A kinematic system, with position and velocity as the states. 10 | * Measured output is position. 11 | */ 12 | 13 | #include 14 | 15 | #include "kf.h" 16 | 17 | 18 | int main(int argc, char** argv) 19 | { 20 | /* 21 | * Log the result into a tab delimitted file, later we can open 22 | * it with Matlab. Use: plot_data2.m to plot the results. 23 | */ 24 | ofstream log_file; 25 | #ifdef _WIN32 26 | log_file.open("..\\bin\\log_file2.txt"); 27 | #else 28 | log_file.open("log_file2.txt"); 29 | #endif 30 | 31 | mat A(2,2), B(2,1), H(1,1), Q(2,2), R(1,1); 32 | 33 | A << 1 << 1 << endr 34 | << 0 << 1 << endr; 35 | 36 | B << 0 << endr 37 | << 1 << endr; 38 | 39 | H << 1 << 0; 40 | 41 | Q << 0 << 0 << endr 42 | << 0 << 0.1 << endr; 43 | 44 | R << 5; // Very noisy :-) 45 | 46 | KF kalman; 47 | kalman.InitSystem(A, B, H, Q, R); 48 | 49 | colvec z(1); 50 | colvec x(2), x_m(2); 51 | colvec u(1); 52 | 53 | for (int i = 0; i < 30 ; i ++) { 54 | if (i < 10) 55 | u << 1; 56 | else if (i >= 10 && i < 20) 57 | u << -1; 58 | else 59 | u << 0; 60 | 61 | kalman.Kalmanf(u); 62 | 63 | colvec *x = kalman.GetCurrentState(); 64 | colvec *z = kalman.GetCurrentOutput(); 65 | colvec *x_m = kalman.GetCurrentEstimatedState(); 66 | 67 | log_file << i 68 | << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) << '\t' 69 | << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) << '\t' 70 | << z->at(0,0) 71 | << '\n'; 72 | 73 | } 74 | 75 | log_file.close(); 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /samples/main3.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main3.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * 6 | * @brief Example for the Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * A system in x-y plane, a kinematic system, with position and velocity as the states. 10 | * The system has 4 input states (\f$x\f$, \f$y\f$, \f$\dot{x}\f$, \f$\dot{y}\f$). 11 | * and 2 output states (\f$x\f$, \f$y\f$). 12 | */ 13 | 14 | #include 15 | 16 | #include "kf.h" 17 | 18 | 19 | int main(int argc, char** argv) 20 | { 21 | /* 22 | * Log the result into a tab delimitted file, later we can open 23 | * it with Matlab. Use: plot_data3.m to plot the results. 24 | */ 25 | ofstream log_file; 26 | #ifdef _WIN32 27 | log_file.open("..\\bin\\log_file3.txt"); 28 | #else 29 | log_file.open("log_file3.txt"); 30 | #endif 31 | 32 | /* 33 | 34 | */ 35 | mat A(4,4), B(4,1), H(2,4), Q(4,4), R(2,2); 36 | 37 | A << 1 << 0 << 1 << 0 << endr 38 | << 0 << 1 << 0 << 1 << endr 39 | << 0 << 0 << 1 << 0 << endr 40 | << 0 << 0 << 0 << 1 << endr; 41 | 42 | B = B.zeros(); 43 | 44 | H << 1 << 0 << 0 << 0 << endr 45 | << 0 << 1 << 0 << 0 << endr; 46 | 47 | Q = Q.eye(); 48 | 49 | R = 10 * R.eye(); 50 | 51 | colvec x0(4); 52 | x0 << 10 << 10 << 1 << 0; 53 | 54 | mat P0(4,4); 55 | P0 = 10 * P0.eye(); 56 | 57 | KF kalman; 58 | kalman.InitSystem(A, B, H, Q, R); 59 | kalman.InitSystemState(x0); 60 | kalman.InitStateCovariance(P0); 61 | 62 | colvec z(2); 63 | colvec x(4), x_m(4); 64 | colvec u(1); 65 | 66 | // No inputs, system subjects only to random perturbation 67 | u = u.zeros(); 68 | 69 | for (int i = 0; i < 100 ; i ++) { 70 | kalman.Kalmanf(u); 71 | 72 | colvec *x = kalman.GetCurrentState(); 73 | colvec *z = kalman.GetCurrentOutput(); 74 | colvec *x_m = kalman.GetCurrentEstimatedState(); 75 | 76 | log_file << x->at(0,0) << '\t' << x->at(1,0) << '\t' << x->at(2,0) << '\t' << x->at(3,0) << '\t' 77 | << x_m->at(0,0) << '\t' << x_m->at(1,0) << '\t' << x_m->at(2,0) << '\t' << x_m->at(3,0) << '\t' 78 | << z->at(0,0) << '\t' << z->at(1,0) 79 | << '\n'; 80 | } 81 | 82 | log_file.close(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /samples/main4.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main4.cpp 3 | * @author Auralius Manurung 4 | * @date 18 Apr 2015 5 | * 6 | * @brief Example for the extended Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from here. 10 | */ 11 | 12 | #include 13 | 14 | #include "ekf.h" 15 | 16 | /// @cond DEV 17 | /* 18 | * Class EKF needs to be derived, two virtual functions are provided in 19 | * which system model and output model are described. 20 | */ 21 | class MyEKF: public EKF 22 | { 23 | public: 24 | virtual colvec f(const colvec& x, const colvec& u) { 25 | colvec xk(nOutputs_); 26 | xk(0) = sin(x(1) * u(0)); 27 | xk(1) = x(1); 28 | return xk; 29 | } 30 | 31 | virtual colvec h(const colvec& x) { 32 | colvec zk(nOutputs_); 33 | zk(0) = x(0); 34 | zk(1) = x(1); 35 | return zk; 36 | } 37 | }; 38 | /// @endcond 39 | 40 | ///////////////////////////////////////////////////////////////////////////// 41 | 42 | int main(int argc, char** argv) 43 | { 44 | /* 45 | * Log the result into a tab delimitted file, later we can open 46 | * it with Matlab. Use: plot_data4.m to plot the results. 47 | */ 48 | ofstream log_file; 49 | #ifdef _WIN32 50 | log_file.open("..\\bin\\log_file4.txt"); 51 | #else 52 | log_file.open("log_file4.txt"); 53 | #endif 54 | 55 | int n_states = 2; 56 | int n_outputs = 2; 57 | mat Q(2, 2); 58 | mat R(2, 2); 59 | 60 | Q << 0.001 << 0 << endr 61 | << 0 << 0 << endr; 62 | 63 | R << 0.1 << 0 << endr 64 | << 0 << 0.01 << endr; 65 | 66 | colvec x0(2); 67 | x0 << 0 << 1 * M_PI / 500; 68 | 69 | colvec u(1); 70 | 71 | MyEKF myekf; 72 | myekf.InitSystem(n_states, n_outputs, Q, R); 73 | myekf.InitSystemState(x0); 74 | 75 | for (int k = 0; k < 1000; k ++) { 76 | u(0) = k; 77 | myekf.EKalmanf(u); 78 | 79 | colvec *x = myekf.GetCurrentState(); 80 | colvec *x_m = myekf.GetCurrentEstimatedState(); 81 | colvec *z = myekf.GetCurrentOutput(); 82 | 83 | log_file << k 84 | << '\t' << z->at(0,0) << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) 85 | << '\t' << z->at(1,0) << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) 86 | << '\n'; 87 | } 88 | 89 | log_file.close(); 90 | 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /samples/main5.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main5.cpp 3 | * @author Auralius Manurung 4 | * @date 12 Aug 2015 5 | * 6 | * @brief Example for the extended Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from 10 | * here. 11 | */ 12 | 13 | #include 14 | 15 | #include "ekf.h" 16 | 17 | /// @cond DEV 18 | /* 19 | * Class EKF needs to be derived, two virtual functions are provided in 20 | * which system model and output model are described. 21 | */ 22 | class MyEKF: public EKF 23 | { 24 | public: 25 | virtual colvec f(const colvec& x, const colvec& u) { 26 | colvec xk(nStates_); 27 | xk << x(1) << x(2) << 0.05*x(0)*(x(1)+x(2)); 28 | return xk; 29 | } 30 | 31 | virtual colvec h(const colvec& x) { 32 | colvec zk(nOutputs_); 33 | zk(0) = x(0); 34 | return zk; 35 | } 36 | }; 37 | /// @endcond 38 | 39 | ///////////////////////////////////////////////////////////////////////////// 40 | 41 | int main(int argc, char** argv) 42 | { 43 | /* 44 | * Log the result into a tab delimitted file, later we can open 45 | * it with Matlab. Use: plot_data5.m to plot the results. 46 | */ 47 | ofstream log_file; 48 | #ifdef _WIN32 49 | log_file.open("..\\bin\\log_file5.txt"); 50 | #else 51 | log_file.open("log_file5.txt"); 52 | #endif 53 | 54 | int n_states = 3; 55 | int n_outputs = 1; 56 | mat Q(n_states, n_states); 57 | mat R(n_outputs, n_outputs); 58 | 59 | double q = 0.1; 60 | double r = 0.1; 61 | 62 | Q << q*q << 0 << 0 << endr 63 | << 0 << q*q << 0 << endr 64 | << 0 << 0 << q*q << endr; 65 | 66 | R << r*r << endr; 67 | 68 | colvec x0(n_states); 69 | x0 << 0 << 0 << 1; 70 | 71 | colvec u; 72 | 73 | // No inputs 74 | u = u.zeros(); 75 | 76 | MyEKF myekf; 77 | myekf.InitSystem(n_states, n_outputs, Q, R); 78 | myekf.InitSystemState(x0); 79 | 80 | for (int k = 0; k < 20; k ++) { 81 | myekf.EKalmanf(u); 82 | 83 | colvec *x = myekf.GetCurrentState(); 84 | colvec *x_m = myekf.GetCurrentEstimatedState(); 85 | colvec *z = myekf.GetCurrentOutput(); 86 | 87 | log_file << k << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) 88 | << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) 89 | << '\t' << x->at(2,0) << '\t' << x_m->at(2,0) 90 | << '\n'; 91 | } 92 | 93 | log_file.close(); 94 | 95 | return 0; 96 | } 97 | -------------------------------------------------------------------------------- /samples/main6.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main6.cpp 3 | * @author Auralius Manurung 4 | * @date 12 Aug 2015 5 | * 6 | * @brief Example for the extended Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from here, 10 | * section 3.2: "Tracking a random sine signal". 11 | */ 12 | 13 | #include 14 | 15 | #include "ekf.h" 16 | 17 | /// @cond DEV 18 | /* 19 | * Class EKF needs to be derived, two virtual functions are provided in 20 | * which system model and output model are described. 21 | */ 22 | class MyEKF: public EKF 23 | { 24 | public: 25 | virtual colvec f(const colvec& x, const colvec& u) { 26 | colvec xk(nStates_); 27 | mat A(3, 3); 28 | 29 | A << 1 << dt << 0 << endr 30 | << 0 << 1 << 0 << endr 31 | << 0 << 0 << 1 << endr; 32 | 33 | xk = A * x;; 34 | //xk.print("xk="); 35 | return xk; 36 | } 37 | 38 | virtual colvec h(const colvec& x) { 39 | colvec zk(nOutputs_); 40 | zk(0) = x(2)*sin(x(0)); 41 | return zk; 42 | } 43 | 44 | static const double dt; 45 | 46 | }; 47 | 48 | const double MyEKF::dt = 0.01; 49 | 50 | /// @endcond 51 | 52 | ///////////////////////////////////////////////////////////////////////////// 53 | 54 | int main(int argc, char** argv) 55 | { 56 | /* 57 | * Log the result into a tab delimitted file, later we can open 58 | * it with Matlab. Use: plot_data6.m to plot the results. 59 | */ 60 | ofstream log_file; 61 | #ifdef _WIN32 62 | log_file.open("..\\bin\\log_file6.txt"); 63 | #else 64 | log_file.open("log_file6.txt"); 65 | #endif 66 | 67 | int n_states = 3; 68 | int n_outputs = 1; 69 | mat Q(n_states, n_states); 70 | mat R(n_outputs, n_outputs); 71 | 72 | 73 | MyEKF myekf; 74 | 75 | double q1 = 0.2; 76 | double q2 = 0.1; 77 | double dt = myekf.dt; 78 | 79 | Q << q1*dt*dt*dt/3 << q1*dt*dt/2 << 0 << endr 80 | << q1*dt*dt/2 << q1*dt << 0 << endr 81 | << 0 << 0 << dt*q2 << endr; 82 | 83 | R << 1 << endr; 84 | 85 | colvec x0(n_states); 86 | x0 << 0 << 10 << 1; 87 | 88 | mat P0 = eye (3,3); 89 | P0 = 3 * P0; 90 | 91 | colvec u; 92 | 93 | // No inputs 94 | u = u.zeros(); 95 | 96 | myekf.InitSystem(n_states, n_outputs, Q, R); 97 | myekf.InitSystemState(x0); 98 | myekf.InitSystemStateCovariance(P0); 99 | 100 | for (int k = 0; k < 500; k ++) { 101 | myekf.EKalmanf(u); 102 | 103 | colvec *x = myekf.GetCurrentState(); 104 | colvec *x_m = myekf.GetCurrentEstimatedState(); 105 | colvec *z = myekf.GetCurrentOutput(); 106 | colvec *z_m = myekf.GetCurrentEstimatedOutput(); 107 | 108 | log_file << k << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) 109 | << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) 110 | << '\t' << x->at(2,0) << '\t' << x_m->at(2,0) 111 | << '\t' << z->at(0,0) << '\t' << z_m->at(0,0) << '\t' 112 | << '\n'; 113 | } 114 | 115 | log_file.close(); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /samples/main7.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main7.cpp 3 | * @author Auralius Manurung 4 | * @date 16 Jan 2017 5 | * 6 | * @brief An example of a Kalman filter practical application. 7 | * 8 | * @section DESCRIPTION 9 | * An example on using Kalman fiter for smoothing measurement data. 10 | * Assume we have a measurment data. 11 | * We will use the second scenario of using Kalman filter. 12 | * In this scenario, there is no need for the Kalman precdure to simulate the system. 13 | * We then tune Q and R to have the best filtering performance. 14 | */ 15 | 16 | #include "kf.h" 17 | 18 | int main(int argc, char** argv) 19 | { 20 | /* 21 | * Log the result into a tab delimitted file, later we can open 22 | * it with Matlab. Use: plot_data7.m to plot the results. 23 | */ 24 | ofstream log_file; 25 | #ifdef _WIN32 26 | log_file.open("..\\bin\\log_file7.txt"); 27 | #else 28 | log_file.open("log_file7.txt"); 29 | #endif 30 | 31 | /* 32 | * Define the system and initialize the Kalman filter 33 | */ 34 | mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1); 35 | colvec u(1); 36 | colvec x0(1); 37 | 38 | A << 1; 39 | B << 1; 40 | Q << 0.01; // Heuristic tuning parameter 41 | H << 1; 42 | R << 1; // Heuristic tuning parameter 43 | u << 0; 44 | 45 | KF kalman; 46 | kalman.InitSystem(A, B, H, Q, R); 47 | 48 | int N = 500; 49 | 50 | for (int k = 0; k < N; k++) { 51 | 52 | // Assume we have a noisy signal that is acquired from a measurement tool 53 | double w = 2; // Stdev of the noise, in reality we don't know this 54 | colvec z_measurement(1); 55 | z_measurement.randn(1); 56 | z_measurement = z_measurement * w + 12.0; 57 | 58 | // Put the z_measurment to the Kalman filter 59 | kalman.Kalmanf(z_measurement, u); 60 | 61 | colvec *z_m = kalman.GetCurrentEstimatedOutput(); 62 | colvec *x_m = kalman.GetCurrentEstimatedState(); 63 | 64 | log_file << k << '\t' << z_measurement.at(0,0) << '\t' << z_m->at(0,0) 65 | << '\n'; 66 | } 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /samples/main8.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main8.cpp 3 | * @author Auralius Manurung 4 | * @date 27 Jan 2017 5 | * 6 | * @brief Example for the second order extended Kalman filter. 7 | * 8 | * @section DESCRIPTION 9 | * This example is taken from here, 10 | * section 3.2: "Tracking a random sine signal". 11 | */ 12 | 13 | #include 14 | 15 | #include "ekf2.h" 16 | 17 | /// @cond DEV 18 | /* 19 | * Class EKF needs to be derived, two virtual functions are provided in 20 | * which system model and output model are described. 21 | */ 22 | class MyEKF: public EKF2 23 | { 24 | public: 25 | virtual colvec f(const colvec& x, const colvec& u) { 26 | colvec xk(nStates_); 27 | mat A(3, 3); 28 | 29 | A << 1 << dt << 0 << endr 30 | << 0 << 1 << 0 << endr 31 | << 0 << 0 << 1 << endr; 32 | 33 | xk = A * x;; 34 | //xk.print("xk="); 35 | return xk; 36 | } 37 | 38 | virtual colvec h(const colvec& x) { 39 | colvec zk(nOutputs_); 40 | zk(0) = x(2)*sin(x(0)); 41 | return zk; 42 | } 43 | 44 | static const double dt; 45 | 46 | }; 47 | 48 | const double MyEKF::dt = 0.01; 49 | 50 | /// @endcond 51 | 52 | ///////////////////////////////////////////////////////////////////////////// 53 | 54 | int main(int argc, char** argv) 55 | { 56 | /* 57 | * Log the result into a tab delimitted file, later we can open 58 | * it with Matlab. Use: plot_data8.m to plot the results. 59 | */ 60 | ofstream log_file; 61 | #ifdef _WIN32 62 | log_file.open("..\\bin\\log_file8.txt"); 63 | #else 64 | log_file.open("log_file8.txt"); 65 | #endif 66 | 67 | int n_states = 3; 68 | int n_outputs = 1; 69 | mat Q(n_states, n_states); 70 | mat R(n_outputs, n_outputs); 71 | 72 | 73 | MyEKF myekf; 74 | 75 | double q1 = 0.2; 76 | double q2 = 0.1; 77 | double dt = myekf.dt; 78 | 79 | Q << q1*dt*dt*dt/3 << q1*dt*dt/2 << 0 << endr 80 | << q1*dt*dt/2 << q1*dt << 0 << endr 81 | << 0 << 0 << dt*q2 << endr; 82 | 83 | R << 1 << endr; 84 | 85 | colvec x0(n_states); 86 | x0 << 0 << 10 << 1; 87 | 88 | mat P0 = eye (3,3); 89 | P0 = 3 * P0; 90 | 91 | colvec u; 92 | 93 | // No inputs 94 | u = u.zeros(); 95 | 96 | myekf.InitSystem(n_states, n_outputs, Q, R); 97 | myekf.InitSystemState(x0); 98 | myekf.InitSystemStateCovariance(P0); 99 | 100 | for (int k = 0; k < 500; k ++) { 101 | myekf.EKalmanf(u); 102 | 103 | colvec *x = myekf.GetCurrentState(); 104 | colvec *x_m = myekf.GetCurrentEstimatedState(); 105 | colvec *z = myekf.GetCurrentOutput(); 106 | colvec *z_m = myekf.GetCurrentEstimatedOutput(); 107 | 108 | log_file << k << '\t' << x->at(0,0) << '\t' << x_m->at(0,0) 109 | << '\t' << x->at(1,0) << '\t' << x_m->at(1,0) 110 | << '\t' << x->at(2,0) << '\t' << x_m->at(2,0) 111 | << '\t' << z->at(0,0) << '\t' << z_m->at(0,0) << '\t' 112 | << '\n'; 113 | } 114 | 115 | log_file.close(); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /samples/main9.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main9.cpp 3 | * @author Auralius Manurung 4 | * @date 27 Jan 2017 5 | * 6 | * @brief This is to test the class FX to compute Jacobian and Hessian of function(s). 7 | * 8 | */ 9 | 10 | #include "fx.h" 11 | 12 | 13 | colvec foo(colvec &x, colvec &a) 14 | { 15 | colvec ret(1); 16 | double y = a.at(0)*x.at(0)*x.at(1)*x.at(2); 17 | ret << y; 18 | return ret; 19 | } 20 | 21 | ///////////////////////////////////////////////////////////////////////////// 22 | 23 | int main(int argc, char** argv) 24 | { 25 | FX f(&foo); 26 | colvec x(1); 27 | colvec a(1); 28 | x << 1.0 << 2.0 << 3.0; 29 | a << 2.0; 30 | 31 | colvec y = f.SolveAt(x, a); 32 | mat jac = f.JacobianAt(x, a); 33 | mat hess = f.HessianAt(x, a, 0); 34 | 35 | y.print("y"); 36 | jac.print("jac"); 37 | hess.print("hess"); 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /vs2019/clean.bat: -------------------------------------------------------------------------------- 1 | del *.exe 2 | del *.user 3 | del *.ncb 4 | del *.pdb 5 | del *.lib 6 | del *.ilk 7 | del *.txt 8 | del *.exp 9 | del *.suo 10 | del *.filters 11 | del *.db 12 | 13 | rd /S /Q .vs 14 | 15 | rd /S /Q x64 16 | 17 | for /f %%i in ('dir /a:d /s /b Debug*') do rd /S /Q %%i 18 | 19 | for /f %%i in ('dir /a:d /s /b Release*') do rd /S /Q %%i -------------------------------------------------------------------------------- /windows-libs/Extract windows-libs-2019 here.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/windows-libs/Extract windows-libs-2019 here.txt -------------------------------------------------------------------------------- /windows-libs/clean.bat: -------------------------------------------------------------------------------- 1 | rmdir /S /Q armadillo 2 | del *.lib -------------------------------------------------------------------------------- /windows-libs/windows-libs-2019.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/auralius/kalman-cpp/c46e4edeab3a726ce1d16b7ea1e582d05fdbb950/windows-libs/windows-libs-2019.zip --------------------------------------------------------------------------------