├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── InEKF ├── CMakeLists.txt ├── inekf_se2.cpp └── inekf_se2.h ├── LICENSE ├── README.md ├── cmake └── Config.cmake.in ├── doc ├── doc.pdf ├── doc.tex ├── figs │ ├── se2_example_error_plots.eps │ └── se2_example_trajectories.eps └── sections │ ├── appendices.tex │ └── titlepage.tex └── examples ├── CMakeLists.txt ├── InEKF ├── CMakeLists.txt ├── config.yml ├── example_SE2.cpp ├── readme.md └── sensor_data │ ├── est_xhat.txt │ ├── gt_states.mat │ ├── gt_states.txt │ ├── meas_gps.txt │ ├── meas_gyro.txt │ ├── meas_prior.txt │ ├── meas_vel.txt │ └── readme.md └── KalmanFilter ├── CMakeLists.txt ├── kalman_filter.cpp └── playground.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | ######################################################## 2 | # CMake 3 | ######################################################## 4 | **CMakeLists.txt.user 5 | **CMakeCache.txt 6 | **CMakeFiles 7 | **CMakeScripts 8 | **Testing 9 | **Makefile 10 | **cmake_install.cmake 11 | **install_manifest.txt 12 | **compile_commands.json 13 | **CTestTestfile.cmake 14 | **_deps 15 | 16 | .vscode/ 17 | 18 | build/ 19 | 20 | 21 | ######################################################## 22 | # Latex 23 | ######################################################## 24 | *.ilg 25 | *.gz 26 | *-eps-converted-to.pdf 27 | *.bib 28 | *.aux 29 | 30 | *TODO.txt 31 | 32 | ######################################################## 33 | # C++ 34 | ######################################################## 35 | # Prerequisites 36 | *.d 37 | 38 | # Compiled Object files 39 | *.slo 40 | *.lo 41 | *.o 42 | *.obj 43 | 44 | # Precompiled Headers 45 | *.gch 46 | *.pch 47 | 48 | # Compiled Dynamic libraries 49 | *.so 50 | *.dylib 51 | *.dll 52 | 53 | # Fortran module files 54 | *.mod 55 | *.smod 56 | 57 | # Compiled Static libraries 58 | *.lai 59 | *.la 60 | *.a 61 | *.lib 62 | 63 | # Executables 64 | *.exe 65 | *.out 66 | *.app 67 | 68 | **/*.cache 69 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "include/RandomVariable"] 2 | path = extern/RandomVariable 3 | url = git@github.com:aalbaa/RandomVariable.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | project(InEKF VERSION 0.1) 3 | 4 | # make cache variables for install destinations 5 | include(GNUInstallDirs) 6 | 7 | include(CTest) 8 | enable_testing() 9 | 10 | # SE(2) InEKF library 11 | add_subdirectory(InEKF) 12 | 13 | if(BUILD_EXAMPLES) 14 | # Add examples subdirectory 15 | message(STATUS "Building examples") 16 | add_subdirectory( examples) 17 | endif() 18 | 19 | ######################################## 20 | # Configuration 21 | ######################################## 22 | # include CMakePackageConfigHelpers macro 23 | include(CMakePackageConfigHelpers) 24 | add_subdirectory(extern/RandomVariable) 25 | 26 | # generate the version file for the config file 27 | write_basic_package_version_file( 28 | "${CMAKE_CURRENT_BINARY_DIR}/InEKFConfigVersion.cmake" 29 | VERSION ${CMAKE_PROJECT_VERSION} 30 | COMPATIBILITY AnyNewerVersion 31 | ) 32 | 33 | # create config file 34 | configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in 35 | "${CMAKE_CURRENT_BINARY_DIR}/InEKFConfig.cmake" 36 | INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InEKF 37 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 38 | ) 39 | 40 | # install config files 41 | install(FILES 42 | "${CMAKE_CURRENT_BINARY_DIR}/InEKFConfig.cmake" 43 | "${CMAKE_CURRENT_BINARY_DIR}/InEKFConfigVersion.cmake" 44 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InEKF 45 | ) 46 | 47 | 48 | # generate and install export file 49 | install(EXPORT InEKFTargets 50 | FILE InEKFTargets.cmake 51 | NAMESPACE InEKF:: 52 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InEKF 53 | ) 54 | 55 | 56 | option(BUILD_EXAMPLES "Build all examples." OFF) 57 | -------------------------------------------------------------------------------- /InEKF/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | # Find the Eigen library 3 | find_package(Eigen3 REQUIRED) 4 | # # Find the manif library 5 | find_package(manif REQUIRED) 6 | 7 | # Add the RandomVariable header class 8 | find_package(RandomVariable) 9 | 10 | # Build the InEKF library 11 | add_library(SE2 12 | STATIC 13 | "${CMAKE_CURRENT_LIST_DIR}/inekf_se2.cpp" 14 | "${CMAKE_CURRENT_LIST_DIR}/inekf_se2.h" 15 | ) 16 | 17 | # Create alias 18 | add_library(InEKF::SE2 ALIAS SE2) 19 | 20 | target_include_directories( 21 | SE2 22 | PUBLIC 23 | ${EIGEN_INCLUDE_DIRS} 24 | "$" 25 | "$" 26 | ) 27 | 28 | target_link_libraries( 29 | SE2 30 | PUBLIC 31 | yaml-cpp 32 | Eigen3::Eigen 33 | ${MANIF_LIBRARY} 34 | RandomVariable::RandomVariable 35 | ) 36 | 37 | set_target_properties( 38 | SE2 39 | PROPERTIES 40 | CXX_STANDARD 11 41 | ) 42 | 43 | 44 | # install the target and create export-set 45 | install(TARGETS SE2 46 | EXPORT InEKFTargets 47 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" 48 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" 49 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" 50 | INCLUDES DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/InEKF" 51 | ) 52 | 53 | # install header file 54 | install(FILES 55 | "inekf_se2.h" 56 | DESTINATION 57 | "${CMAKE_INSTALL_INCLUDEDIR}/InEKF" 58 | ) 59 | 60 | 61 | -------------------------------------------------------------------------------- /InEKF/inekf_se2.cpp: -------------------------------------------------------------------------------- 1 | #include "inekf_se2.h" 2 | 3 | std::vector GetSe2InekfEstimates(PoseEstimate meas_prior, 4 | std::vector meas_gyro, 5 | std::vector meas_vel, 6 | std::vector meas_gps) { 7 | 8 | // Number of poses 9 | const unsigned int K = meas_gyro.size(); 10 | // Lambda function that extracts the sample time (dt) 11 | auto dt_func = [&meas_gyro](int k) { 12 | return meas_gyro[k].time() - meas_gyro[k - 1].time(); 13 | }; 14 | 15 | // Vector of estimated poses 16 | std::vector X_hat(K); 17 | 18 | // Make sure meas_prior is a valid measurement (using manif) 19 | { 20 | // Eigen matrix 21 | EPose X0_mat = meas_prior.mean(); 22 | // Manif 23 | Pose X0_man(X0_mat(0, 2), X0_mat(1, 2), atan2(X0_mat(1, 0), X0_mat(0, 0))); 24 | // Normalize 25 | X0_man.normalize(); 26 | // Export the SE2 object 27 | meas_prior.setMean(X0_man.transform()); 28 | } 29 | // Set initial estimate 30 | X_hat[0] = meas_prior; 31 | 32 | // Index that keeps track of the gps measurements 33 | size_t idx_gps = 0; 34 | // P_hat_km1 (convert from [theta; pos] to [pos; theta]) 35 | CovPose P_hat_km1 = CovThetaPosToCovPosTheta(X_hat[0].cov()); 36 | 37 | // Filtering 38 | for (size_t k = 1; k < K; k++) { 39 | // Store time steps 40 | X_hat[k].setTime(meas_vel[k].time()); 41 | 42 | // Interoceptive measurements at k - 1 43 | // Velocity 44 | auto u_v_km1 = meas_vel[k - 1].mean(); 45 | // Gyro 46 | auto u_g_km1 = meas_gyro[k - 1].mean(); 47 | // Get the tangent coordinates (twist) 48 | double dt_km1 = dt_func(k); 49 | LieAlg u_km1(u_v_km1[0], u_v_km1[1], u_g_km1[0]); 50 | // Covariance on process noise 51 | CovQ Q_km1 = CovQ::Zero(); 52 | Q_km1.block<2, 2>(0, 0) = meas_vel[k - 1].cov(); 53 | Q_km1.block<1, 1>(2, 2) = meas_gyro[k - 1].cov(); 54 | 55 | // Get matrix at k - 1 56 | EPose Xkm1_mat = X_hat[k - 1].mean(); 57 | // Note: covariance is already updated from previous iteration 58 | 59 | // Set pose at previous estimate 60 | Pose Xkm1(Xkm1_mat(0, 2), Xkm1_mat(1, 2), Xkm1_mat(0, 0), Xkm1_mat(1, 0)); 61 | 62 | // Predict 63 | Pose X_k = Xkm1.plus(dt_km1 * u_km1); 64 | // (LI) Jacobian w.r.t. state 65 | JacF_Xkm1 J_F_xkm1 = (-dt_km1 * u_km1).exp().adj(); 66 | // (LI) Jacobian w.r.t. process noise (w_km1) 67 | JacF_wkm1 J_F_wkm1 = -dt_km1 * JacF_wkm1::Identity(); 68 | 69 | // Compute covariance on X_k 70 | CovPose P_k = J_F_xkm1 * P_hat_km1 * J_F_xkm1.transpose() + 71 | J_F_wkm1 * Q_km1 * J_F_wkm1.transpose(); 72 | 73 | // Correction 74 | if (idx_gps < meas_gps.size() && 75 | (meas_gps[idx_gps].time() <= X_hat[k].time())) { 76 | // Implement a correction 77 | CovGps R_k = meas_gps[idx_gps].cov(); 78 | EMeasGps y_k = meas_gps[idx_gps].mean(); 79 | // Jacobian of measurement function w.r.t. state 80 | JacYgps_Xk H_k; 81 | // Jacobian of innovation w.r.t. measurement noise 82 | Eigen::Matrix2d M_k = X_k.rotation().transpose(); 83 | H_k.topLeftCorner<2, 2>() = -Eigen::Matrix2d::Identity(); 84 | H_k.topRightCorner<2, 1>().setZero(); 85 | 86 | // Predicted measurement 87 | EMeasGps y_check_k = X_k.translation(); 88 | 89 | // Innovation 90 | EMeasGps z = X_k.rotation().transpose() * (y_k - y_check_k); 91 | 92 | // Compute S_k 93 | JacYgps_nk S_k = 94 | H_k * P_k * H_k.transpose() + M_k * R_k * M_k.transpose(); 95 | 96 | // Ensure symmetry 97 | JacYgps_nk S_k_tmp = 0.5 * (S_k + S_k.transpose()); 98 | S_k = S_k_tmp; 99 | // Compute Kalman gain 100 | Eigen::Matrix K_k = 101 | P_k * H_k.transpose() * S_k.inverse(); 102 | 103 | // Update state estimate (xhat) using a LI error 104 | // X_k = X_k.plus( LieAlg( - K_k * z)); 105 | X_k += LieAlg(-K_k * z); 106 | // Pose X_hat_k = X_k.plus( LieAlg( - K_k * z)); 107 | // X_k = X_hat_k; 108 | 109 | // Update covariance 110 | P_k = ((CovPose::Identity()) - K_k * H_k) * P_k * 111 | ((CovPose::Identity()) - K_k * H_k).transpose() + 112 | K_k * M_k * R_k * M_k.transpose() * K_k.transpose(); 113 | // Ensure symmetry 114 | CovPose P_k_tmp = 0.5 * (P_k + P_k.transpose()); 115 | P_k = P_k_tmp; 116 | 117 | idx_gps++; 118 | } 119 | 120 | // Ensure symmetry 121 | P_k = 0.5 * (P_k + P_k.transpose().eval()); 122 | 123 | // Update covariance at "previous" time step 124 | P_hat_km1 = P_k; 125 | 126 | // // Ensure symmetry 127 | // P_k_th_r = 0.5 * ( P_k_th_r + P_k_th_r.transpose().eval()); 128 | 129 | // Store estimates 130 | X_hat[k].setMean(X_k.transform()); 131 | // Store the covariance in the appropriate format [th; pos] 132 | // Note: switch covariance type from covariance on [position; theta] to 133 | // [theta; position] (to do the analysis) 134 | X_hat[k].setCov(CovPosThetaToCovThetaPos(P_k)); 135 | X_hat[k].setCovIsGlobal(false); 136 | } 137 | 138 | return X_hat; 139 | } 140 | 141 | std::vector 142 | GetSe2InekfEstimates(const std::string filename_config) { 143 | // @param[in] filename_config 144 | // Filename (including path) of the .yml configuration file. 145 | // @return std::vector of PoseEstimate (instance of RandomVariable class). 146 | // Assumptions: 147 | // - Assumes the files include GPS measurements as exteroceptive 148 | // measurements. 149 | 150 | // Read .yml configuration file 151 | YAML::Node config = YAML::LoadFile(filename_config); 152 | #ifndef NDEBUG 153 | // Display message if in debug model 154 | std::cout << "Reading .yml configuration from '" << filename_config << "'" 155 | << std::endl; 156 | #endif 157 | 158 | // Read sensor files 159 | // Prior 160 | const std::string filename_prior = config["filename_prior"].as(); 161 | // Gyro 162 | const std::string filename_gyro = config["filename_gyro"].as(); 163 | // Velocity 164 | const std::string filename_vel = config["filename_vel"].as(); 165 | // GPS 166 | const std::string filename_gps = config["filename_gps"].as(); 167 | 168 | // Import data 169 | // Prior 170 | PoseEstimate meas_prior = RV::IO::import (filename_prior)[0]; 171 | // Gyro 172 | std::vector meas_gyro = RV::IO::import (filename_gyro); 173 | // Velocity 174 | std::vector meas_vel = RV::IO::import (filename_vel); 175 | // GPS 176 | std::vector meas_gps = RV::IO::import (filename_gps); 177 | 178 | return GetSe2InekfEstimates(meas_prior, meas_gyro, meas_vel, meas_gps); 179 | } 180 | 181 | CovPose CovPosThetaToCovThetaPos(CovPose P_pos_th) { 182 | CovPose P_th_r; 183 | P_th_r(0, 0) = P_pos_th(2, 2); 184 | // X-cov pos-heading 185 | P_th_r.bottomLeftCorner<2, 1>() = P_pos_th.topRightCorner<2, 1>(); 186 | // X-cov heading-pos 187 | P_th_r.topRightCorner<1, 2>() = P_pos_th.bottomLeftCorner<1, 2>(); 188 | // Cov pos 189 | P_th_r.bottomRightCorner<2, 2>() = P_pos_th.topLeftCorner<2, 2>(); 190 | 191 | return P_th_r; 192 | } 193 | 194 | CovPose CovThetaPosToCovPosTheta(CovPose P_th_pos) { 195 | CovPose P_pos_th; 196 | P_pos_th(2, 2) = P_th_pos(0, 0); 197 | // X-cov pos-heading 198 | P_pos_th.topRightCorner<2, 1>() = P_th_pos.bottomLeftCorner<2, 1>(); 199 | // X-cov heading-pos 200 | P_pos_th.bottomLeftCorner<1, 2>() = P_th_pos.topRightCorner<1, 2>(); 201 | // Cov pos 202 | P_pos_th.topLeftCorner<2, 2>() = P_th_pos.bottomRightCorner<2, 2>(); 203 | 204 | return P_pos_th; 205 | } 206 | -------------------------------------------------------------------------------- /InEKF/inekf_se2.h: -------------------------------------------------------------------------------- 1 | #ifndef INEKF_SE2_H 2 | #define INEKF_SE2_H 3 | #ifndef NDEBUG 4 | #include 5 | #endif 6 | #include 7 | #include 8 | 9 | // Eigen 10 | #include "Eigen/Dense" 11 | 12 | // Manif 13 | #include "manif/SE2.h" 14 | 15 | // YAML C++ 16 | #include "yaml-cpp/yaml.h" 17 | 18 | // // My custom RandomVariable class 19 | #include "RV.h" 20 | #include "RVIO.h" 21 | 22 | // Degrees of freedom of pose (SE(2)) 23 | static const size_t dof_x = 3; 24 | // Embedded dimension (SE(2) -> robot lives in 2D) 25 | static const size_t dim_x = 2; 26 | static const size_t dof_gyro = 1; // DOF of gyro measurement 27 | static const size_t dof_vel = 2; // DOF of velocity measurement 28 | static const size_t dof_gps = 2; // DOF of GPS measurement 29 | 30 | // Random variables 31 | typedef RandomVariable MeasGyro; 32 | typedef RandomVariable MeasVel; 33 | typedef RandomVariable MeasGps; 34 | typedef RandomVariable<3, 3, dof_x> PoseEstimate; 35 | 36 | // Lie Group classes 37 | typedef manif::SE2d Pose; // Pose variable 38 | typedef manif::SE2Tangentd LieAlg; // Tangent vectors 39 | 40 | // Eigen matrix sizes 41 | // SE2 matrix 42 | typedef Eigen::Matrix EPose; 43 | // Covariances 44 | typedef Eigen::Matrix CovPose; 45 | // Process model 46 | // Jacobian w.r.t. previous state 47 | typedef Eigen::Matrix JacF_Xkm1; 48 | // Jacobian w.r.t. noise 49 | typedef Eigen::Matrix JacF_wkm1; 50 | // Covariance on process noise 51 | typedef Eigen::Matrix CovQ; 52 | // GPS measurement model 53 | // GPS measurement Eigen type 54 | typedef Eigen::Matrix EMeasGps; 55 | // Covariance on measurement noise 56 | typedef Eigen::Matrix CovGps; 57 | // Jacobian w.r.t. state 58 | typedef Eigen::Matrix JacYgps_Xk; 59 | // Jacobian w.r.t. measurement noise 60 | typedef Eigen::Matrix JacYgps_nk; 61 | 62 | // Function that takes the .yml filename (including path) and returns the InEKF 63 | // estiamtes. 64 | std::vector 65 | GetSe2InekfEstimates(const std::string filename_config); 66 | 67 | // Function that takes measurement RV variables and returns the InEKF estimates 68 | // (it's a different function signature) 69 | std::vector GetSe2InekfEstimates(PoseEstimate meas_prior, 70 | std::vector meas_gyro, 71 | std::vector meas_vel, 72 | std::vector meas_gps); 73 | 74 | // Take covariance on [position; theta] and return covariance on [theta; 75 | // position] 76 | CovPose CovPosThetaToCovThetaPos(CovPose P_pos_th); 77 | 78 | // Take covariance on [theta; position] and return covariance on [position; 79 | // theta] 80 | CovPose CovThetaPosToCovPosTheta(CovPose P_th_pos); 81 | #endif 82 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 aalbaa 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![License: MIT](https://camo.githubusercontent.com/2ff6a06f2f6e08b17783133ca7ebc23ce1f8ac4415eee8e835647b57048a8f0d/68747470733a2f2f696d672e736869656c64732e696f2f6769746875622f6c6963656e73652f6d6173686170652f6170697374617475732e737667)](LICENSE) 2 | 3 | 4 | # Dependencies 5 | * Eigen3. 6 | * Manif (required for the filters operating on Lie groups). 7 | * Custom `RandomVariable` header-only library (this is included in the `extern` directory). 8 | 9 | # In this repo 10 | * Implementation of a standard Kalman filter. 11 | * Implementation of a left-invariant extended Kalman filter (L-InEKF) on *SE(2)*. 12 | -------------------------------------------------------------------------------- /cmake/Config.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | @PACKAGE_INIT@ 3 | 4 | include("${CMAKE_CURRENT_LIST_DIR}/InEKFTargets.cmake") 5 | 6 | #check_required_components(InEKF) -------------------------------------------------------------------------------- /doc/doc.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aalbaali/cpp_filter/8a1b6745769b2d5d346e3f08f81a080028eb6dfd/doc/doc.pdf -------------------------------------------------------------------------------- /doc/doc.tex: -------------------------------------------------------------------------------- 1 | \documentclass[nobib, nofonts, notoc, justified]{tufte-handout} 2 | \usepackage{amro-common} 3 | \usepackage{amro-tufte} 4 | \usepackage[toc,page]{appendix} 5 | \addbibresource{references.bib} 6 | 7 | % Spacing between lines 8 | \setstretch{1} 9 | 10 | % \makeatletter 11 | % % % Remove paragraph indentation 12 | % % \renewcommand{\@tufte@reset@par}{% 13 | % % \setlength{\RaggedRightParindent}{0.0pc}% 14 | % % \setlength{\JustifyingParindent}{0.0pc}% 15 | % % \setlength{\parindent}{0pc}% 16 | % % \setlength{\parskip}{\baselineskip}% 17 | % % } 18 | % \@tufte@reset@par 19 | % \makeatother 20 | 21 | % Renew the PDF symbol 22 | \renewcommand{\symPdf}{p} 23 | 24 | \newcommand{\cpp}{C\texttt{++}\xspace} 25 | 26 | % Might prevent marginnotes from jumping to another page 27 | \def\mathnote#1{% 28 | \tag*{\rlap{\hspace\marginparsep\smash{\parbox[t]{\marginparwidth}{% 29 | \footnotesize#1}}}} 30 | } 31 | 32 | 33 | % % This command is needed for building in linux 34 | % \captionsetup{compatibility=false} 35 | 36 | \newcommand{\lplus}{\overset{{\scriptscriptstyle\mathrm{L}}}{\oplus}} 37 | \newcommand{\liplus}{\overset{{\scriptscriptstyle\mathrm{LI}}}{\oplus}} 38 | \newcommand{\rplus}{\overset{{\scriptscriptstyle\mathrm{R}}}{\oplus}} 39 | \newcommand{\riplus}{\overset{{\scriptscriptstyle\mathrm{RI}}}{\oplus}} 40 | % Ominus 41 | \newcommand{\lminus}{\overset{{\scriptscriptstyle\mathrm{L}}}{\ominus}} 42 | \newcommand{\liminus}{\overset{{\scriptscriptstyle\mathrm{LI}}}{\ominus}} 43 | \newcommand{\rminus}{\overset{{\scriptscriptstyle\mathrm{R}}}{\ominus}} 44 | \newcommand{\riminus}{\overset{{\scriptscriptstyle\mathrm{RI}}}{\ominus}} 45 | 46 | 47 | 48 | \title{Invariant Filtering} 49 | \author{Amro Al~Baali} 50 | 51 | % Generates the index 52 | \usepackage{makeidx} 53 | \makeindex 54 | 55 | \setcounter{tocdepth}{2} 56 | 57 | 58 | \begin{document} 59 | % \frontmatter 60 | { 61 | \input{sections/titlepage.tex} 62 | % \forceheader{Contents} 63 | \tableofcontents 64 | \clearpage 65 | % \thispagestyle{empty} 66 | % \addtocontents{toc}{\protect\thispagestyle{empty}} 67 | } 68 | % \mainmatter 69 | 70 | \section{Why this document?} 71 | This document is provided to explain and clarify the code uploaded with it. 72 | The repository includes examples of implementing filters, usually Kalman fitlers, in \cpp. 73 | The filters will be mainly implemented on 74 | \begin{packed_enum} 75 | \item a linear system, and 76 | \item a non-Euclidean nonlinear system (usually defined on a Lie group). 77 | \end{packed_enum} 78 | 79 | \section{The Kalman filter} 80 | \subsection{The system} 81 | Consider the linear ordinary differential equation (ODE) describing a mass-spring-damper system 82 | \begin{align} 83 | \label{eq:mass spring damper ode} 84 | m\ddot{x}(t) + b\dot{x}(t) + kx(t) &= u(t), 85 | \end{align} 86 | where $m$ is the mass, $b$ is the damping, $k$ is the spring constant, and $u(t)$ is the forcing function. The system \eqref{eq:mass spring damper ode} can be written in state space form 87 | \begin{align} 88 | \mbfdot{x} &= \bbm 0 & 1\\ -\f{k}{m} & -\f{b}{m} \ebm \mbf{x} + \bbm 0\\\f{1}{m} \ebm u\\ 89 | &= \boldsymbol{A}\mbf{x} + \boldsymbol{B}u_{t}, 90 | \end{align} 91 | where 92 | \begin{align} 93 | \label{eq:DT linear kinematic model} 94 | \mbf{x} &= \bbm x & \dot{x} \ebm^{\trans}, 95 | \end{align} 96 | and the time arguments $(t)$ are dropped for brevity. 97 | 98 | The disrete-time kinematic model is given by 99 | \begin{align} 100 | \mbf{x}_{k} &= \mbf{A}\mbf{x}_{k-1} + \mbf{B}u_{k-1}, 101 | \end{align} 102 | where the discrete-time system matrices $\mbf{A}$ and $\mbf{B}$ are computed using some discretization scheme. For the linear example above, the $\mbf{A}$ matrix is given by 103 | \begin{align} 104 | \mbf{A} &= \exp\left( \boldsymbol{A} T_{k} \right),\\ 105 | \mbf{B} &= \int_{0}^{T_{k}}\exp(\boldsymbol{A}\alpha)\dee\alpha \boldsymbol{B}, 106 | \end{align} 107 | where $T_{k}$ is the sampling period \cite{Farrell_Aided_2008}. 108 | 109 | The matrix $\mbf{B}$ can be approximated using forward Euler to get 110 | \begin{align} 111 | \mbf{B} &\approx T_{k}\boldsymbol{B}. 112 | \end{align} 113 | 114 | \subsection{Process model} 115 | The discrete-time process model\sidenote{Also referred to as the kinematic model, motion model, progression model, etc.} is used in the \emph{prediction} step of the Kalman filter. It is given by 116 | \begin{align} 117 | \label{eq:DT linear process model} 118 | \mbf{x}_{k} &= \mbf{A}\mbf{x}_{k-1} + \mbf{B}\mbf{u}_{k-1} + \mbf{L}\mbf{w}_{k-1}, 119 | \end{align} 120 | where $\mbf{x}_{k}\in\rnums^{n_{x}}$ is the state, $\mbf{u}_{k}\in\rnums^{n_{u}}$ is the control input, and $\mbf{w}_{k}\in\rnums^{n_{w}}$ where $\mbf{w}_{k}\sim\mc{N}\left( \mbf{0}, \mbf{Q}_{k} \right)$ is the process noise and $\mbf{Q}_{k}$ is the process noise covariance. 121 | 122 | \subsection{Measurement functions} 123 | The correction step requires a measurement model which is given by 124 | \begin{align} 125 | \mbf{y}_{k} &= \mbf{C}\mbf{x}_{k} + \mbf{M}\mbf{n}_{k}, 126 | \end{align} 127 | where $\mbf{y}_{k}\in\rnums^{n_{y}}$ and $\mbf{n}_{k}\in\rnums^{n_{n}}$ where $\mbf{n}_{k}\sim\mc{N}\left( \mbf{0}, \mbf{R}_{k} \right)$ is the measurement noise and $\mbf{R}_{k}$ is the measurement noise covariance. 128 | 129 | For the example presented, the measurement is a position measurememnt, so the measurement function is given by 130 | \begin{align} 131 | y_{k} &= \bbm 1 & 0 \ebm\mbf{x}_{k} + n_{k}\\ 132 | &= \mbf{C}\mbf{x}_{k} + n_{k}. 133 | \end{align} 134 | 135 | 136 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 137 | % EKF 138 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 139 | \section{The extended Kalman filter} 140 | Without getting into the derivation of the equations, the (extended) Kalman filter equations are given by \cite[eq.~(4.32)]{Barfoot_State_2017} 141 | \begin{subequations} 142 | \label{eq:EKF eqns} 143 | \begin{align} 144 | \label{eq:EKF: prediction} 145 | \mbfcheck{x}_{k} &= \mbf{f}\left( \mbfhat{x}_{k-1}, \mbf{u}_{k-1}, \mbf{0} \right), \\ 146 | \label{eq:EKF: prediction cov} 147 | \mbfcheck{P}_{k} &= \mbf{A}_{k-1}\mbfhat{P}_{k-1}\mbf{A}_{k-1}^{\trans} + \mbf{L}_{k-1}\mbf{Q}_{k}\mbf{L}_{k-1}^{\trans},\\ 148 | \label{eq:EKF: KF gain} 149 | \mbf{K}_{k} &= \mbfcheck{P}_{k}\mbf{H}_{k}\left( \mbf{H}_{k}\mbfcheck{P}_{k}\mbf{H}_{k}^{\trans} + \mbf{M}_{k}\mbf{R}_{k}\mbf{M}_{k}^{\trans} \right)\inv,\\ 150 | \label{eq:EKF: correction} 151 | \mbfhat{x}_{k} &= \mbfcheck{x}_{k} + \mbf{K}_{k}\left( \mbf{y}_{k} - \mbf{g}_{k}\left( \mbfcheck{x}_{k}, \mbf{0} \right) \right),\\ 152 | \label{eq:EKF: correction cov} 153 | \mbfhat{P}_{k} &= \left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)\mbfcheck{P}_{k}\left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)^{\trans} \nonumber\\&\qquad+ \mbf{K}_{k}\mbf{M}_{k}\mbf{R}_{k}\mbf{M}_{k}^{\trans}\mbf{K}_{k}^{\trans}, 154 | \end{align} 155 | \end{subequations} 156 | where 157 | \begin{align} 158 | \mbf{A}_{k-1} &= \left.\pd{\mbf{f}\left( \mbf{x}_{k-1}, \mbf{u}_{k-1}, \mbf{w}_{k-1} \right)}{\mbf{x}_{k-1}}\right|_{\overset{\mbf{x}_{k-1}=\mbfhat{x}_{k-1},}{\mbf{w}_{k-1}=\mbf{0}}},\\ 159 | \mbf{L}_{k-1} &= \left.\pd{\mbf{f}\left( \mbf{x}_{k-1}, \mbf{u}_{k-1}, \mbf{w}_{k-1} \right)}{\mbf{w}_{k-1}}\right|_{\overset{\mbf{x}_{k-1}=\mbfhat{x}_{k-1},}{\mbf{w}_{k-1}=\mbf{0}}},\\ 160 | \mbf{H}_{k-1} &= \left.\pd{\mbf{g}\left( \mbf{x}_{k}, \mbf{n}_{k} \right)}{\mbf{x}_{k}}\right|_{\overset{\mbf{x}_{k}=\mbfcheck{x}_{k},}{\mbf{n}_{k}=\mbf{0}}},\\ 161 | \mbf{M}_{k-1} &= \left.\pd{\mbf{g}\left( \mbf{x}_{k}, \mbf{n}_{k} \right)}{\mbf{n}_{k}}\right|_{\overset{\mbf{x}_{k}=\mbfcheck{x}_{k},}{\mbf{n}_{k}=\mbf{0}}}. 162 | \end{align} 163 | 164 | The covariance equations \eqref{eq:EKF: prediction cov} and \eqref{eq:EKF: correction cov} are computed using first-order covariance propagation on \eqref{eq:EKF: prediction} and \eqref{eq:EKF: correction}, respectively. Let's clarify this point as it will be important when discussing the invariant extended Kalman filter. 165 | 166 | \begin{remark} 167 | \label{remark:splitting gaussian rv into sum} 168 | A Gaussian random variable 169 | \begin{align} 170 | \mbfrv{x}\sim\mc{N}\left( \mbs{\mu}, \mbs{\Sigma} \right) 171 | \end{align} 172 | can be written as 173 | \begin{align} 174 | \mbfrv{x} &= \mbs{\mu} + \delta\mbfrv{x},\\ 175 | \delta\mbfrv{x} &\sim \mc{N}\left( \mbf{0}, \mbs{\Sigma} \right). 176 | \end{align} 177 | \end{remark} 178 | Using Remark~\ref{remark:splitting gaussian rv into sum}, define the (random) variables 179 | \begin{align} 180 | \label{eq:KF: prediction err. def.} 181 | \delta\mbfcheckrv{x}_{k} &\coloneqq \mbfcheckrv{x}_{k} - \mbf{x}_{k},\\ 182 | \label{eq:KF: correction err. def.} 183 | \delta\mbfhatrv{x}_{k} &\coloneqq \mbfhatrv{x}_{k} - \mbf{x}_{k}, 184 | \end{align} 185 | where 186 | \begin{align} 187 | \delta\mbfcheckrv{x}_{k} &\sim\mc{N}\left( \mbf{0}, \mbfcheck{P}_{k} \right),\\ 188 | \delta\mbfhatrv{x}_{k} &\sim\mc{N}\left( \mbf{0}, \mbfhat{P}_{k} \right). 189 | \end{align} 190 | Using Taylor's expansion of \eqref{eq:EKF: prediction}, the error dynamics of the (extended) Kalman filter equations are then given by 191 | \begin{align} 192 | \delta\mbfcheckrv{x}_{k} 193 | &= \mbfcheckrv{x}_{k} - \mbf{x}_{k} \\ 194 | &= \mbf{f}\left( \mbfhatrv{x}_{k-1}, \mbf{u}_{k-1}, \mbf{0} \right) - \mbf{f}\left( \mbf{x}_{k-1}, \mbf{u}_{k-1}, \mbfrv{w}_{k-1} \right)\\ 195 | &\approx \mbf{f}\left( \mbfhat{x}_{k-1}, \mbf{u}_{k-1}, \mbf{0} \right) - \mbf{f}\left( \mbfhat{x}_{k-1},\mbf{u}_{k-1}, \mbf{0} \right) 196 | \nonumber\\&\qquad 197 | - \mbf{A}_{k-1}\underbrace{\left( \mbf{x}_{k-1} - \mbfhatrv{x}_{k-1} \right)}_{-\delta\mbfhatrv{x}_{k-1}} - \mbf{L}_{k-1}\left( \mbfrv{w}_{k-1} - \mbf{0} \right)\\ 198 | &= \mbf{A}_{k-1}\delta\mbfhatrv{x}_{k-1} - \mbf{L}_{k-1}\mbfrv{w}_{k-1}. 199 | \end{align} 200 | The covariance on $\delta\mbfcheck{x}_{k}$ is then given by 201 | \begin{align} 202 | \cov{\delta\mbfcheckrv{x}_{k}} &= \mbf{A}_{k}\mbfhat{P}_{k-1}\mbf{A}_{k-1}^{\trans} + \mbf{L}_{k-1}\mbf{Q}_{k-1}\mbf{L}_{k-1}^{\trans} 203 | \end{align} 204 | which is equivalent to \eqref{eq:EKF: prediction cov}. 205 | 206 | Applying the same concept on the correction equation gives 207 | \begin{align} 208 | \delta\mbfhatrv{x}_{k} 209 | &= \mbfcheckrv{x}_{k} + \mbf{K}_{k}\left( \mbfrv{y}_{k} - \mbf{g}\left( \mbfcheckrv{x}_{k}, \mbf{0} \right) \right) - \mbf{x}_{k}\\ 210 | &= \mbfcheckrv{x}_{k} + \mbf{K}_{k}\left( \mbf{g}\left( \mbf{x}_{k},\mbfrv{n}_{k} \right) - \mbf{g}\left( \mbfcheckrv{x}_{k}, \mbf{0} \right) \right) - \mbf{x}_{k}\\ 211 | &= \mbf{x}_{k} + \delta\mbfcheckrv{x}_{k} + \mbf{K}_{k}\left( \mbf{g}\left( \mbf{x}_{k},\mbfrv{n}_{k} \right) - \mbf{g}\left( \mbfcheckrv{x}_{k}, \mbf{0} \right) \right)\\ 212 | &\approx \delta\mbfcheckrv{x}_{k} +\mbf{K}_{k}\left( \mbf{g}\left( \mbfcheck{x}_{k}, \mbf{0} \right) + \mbf{H}_{k}\left( \mbf{x}_{k} - \mbfcheckrv{x}_{k} \right) \right.\nonumber\\&\qquad \left.+ \mbf{L}_{k}\left( \mbfrv{n}_{k}-\mbf{0} \right) - \mbf{g}\left( \mbfcheck{x}_{k},\mbf{0} \right) \right)\\ 213 | &= \delta\mbfcheckrv{x}_{k} +\mbf{K}_{k}\left( \mbf{H}_{k}\left( \mbf{x}_{k} - \mbfcheckrv{x}_{k} \right) + \mbf{L}_{k}\left( \mbfrv{n}_{k}-\mbf{0} \right) \right)\\ 214 | &= \delta\mbfcheckrv{x}_{k} +\mbf{K}_{k}\left( -\mbf{H}_{k}\delta\mbfcheckrv{x}_{k} + \mbf{L}_{k}\left( \mbfrv{n}_{k}-\mbf{0} \right) \right)\\ 215 | &= \left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)\delta\mbfcheckrv{x}_{k} + \mbf{K}_{k}\mbf{M}_{k}\mbfrv{n}_{k}. 216 | \end{align} 217 | The covariance is then given by 218 | \begin{align} 219 | \cov{\delta\mbfhatrv{x}_{k}} &= \left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)\mbfcheck{P}_{k}\left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)^{\trans} + \mbf{K}_{k}\mbf{M}_{k}\mbf{R}_{k}\mbf{M}_{k}^{\trans}\mbf{K}_{k}^{\trans} 220 | \end{align} 221 | which is equivalent to \eqref{eq:EKF: correction cov}. 222 | 223 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 224 | % InEKF 225 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 226 | \section{The invariant extended Kalman filter} 227 | The invariant filter\cite{Barrau_Invariant_2018} is applicable to states that live in Lie groups. However, since the filter deals with random variables, it's important to know how to represent random variables living in Lie groups. That is, $\mbfrv{X}\in G$, where $G$ is some group. 228 | 229 | Let the $SE(n)$ process model be given by 230 | \begin{align} 231 | \label{eq:InEKF: process model} 232 | \mbfrv{X}_{k} &= \mbf{F}\left( \mbfrv{X}_{k-1}, \mbf{u}_{k-1}, \mbfrv{w}_{k-1} \right)\\ 233 | &= \mbfrv{X}_{k-1}\Exp\left( T_{k-1}\mbf{u}_{k-1} \right)\Exp(\mbfrv{w}_{k-1})\\ 234 | \label{eq:InEKF: SE3 process model} 235 | &= \mbfrv{X}_{k-1}\mbs{\Xi}_{k-1}\Exp(\mbfrv{w}_{k-1}), 236 | \end{align} 237 | \marginnote[-4em]{$T_{k-1} \coloneqq t_{k}-t_{k-1}$ is the sampling period.} 238 | the left-invariant measurement model is given by 239 | \begin{align} 240 | \label{eq:InEKF: LI meas. model} 241 | \mbfrv{y}_{k} &= \mbfrv{X}_{k}\mbf{b} + \mbfrv{n}_{k}, 242 | \end{align} 243 | and the right-invariant measurement model is given by 244 | \begin{align} 245 | \label{eq:InEKF: RI meas. model} 246 | \mbfrv{y}_{k} &= \mbfrv{X}_{k}\inv\mbf{b} + \mbfrv{n}_{k}, 247 | \end{align} 248 | where $\mbf{b}$ is some known constant column matrix. 249 | 250 | \subsection{Random variables on Lie groups} 251 | \label{sec:Random variables on Lie groups} 252 | In the Euclidean case, Remark~\ref{remark:splitting gaussian rv into sum} can be used to describe a random variable. However, how can we do that in a non-Euclidean case? Let's restrict ourselves with Lie groups. 253 | 254 | 255 | A random variable (on a Lie group) can be given by 256 | \begin{align} 257 | \mbfrv{X} &= \mbfbar{X}\delta\mbfrv{X}\\ 258 | &= \mbfbar{X}\exp\left( \mbsrv{\xi}\expand \right)\\ 259 | &= \mbfbar{X} \rplus \mbsrv{\xi}, 260 | \end{align} 261 | where $\rplus$ is the `right perturbation' operator\footnote{There are multiple versions of the $\oplus$ operator such as left, left-invariant, right, and right-invariant.} from 262 | \cite{Sola_Micro_2020} and 263 | \begin{align} 264 | \mbsrv{\xi} &\sim \mc{N}\left( \mbf{0}, \mbs{\Sigma} \right). 265 | \end{align} 266 | This is the Lie-group version of Remark~\ref{remark:splitting gaussian rv into sum}. Note that $\mbfbar{X}\in G$ and $\exp\left( \mbsrv{\xi}\expand \right)\in G$, thus $\mbfrv{X}\in G$ since $G$ is a Lie group which is closed under multiplication. 267 | 268 | \subsection{Left-invariant perturbation} 269 | The left-invariant ``addition'' $\liplus : G \times \rnums^{n} \to G$ is defined by 270 | \begin{align} 271 | \mbf{X}\liplus\mbs{\xi} 272 | &\coloneqq \mbf{X}\exp\left( -\mbs{\xi}\expand \right)\\ 273 | &= \mbf{X}\Exp\left( -\mbs{\xi} \right), 274 | \end{align} 275 | and the left-invariant ``subtraction'' $\liminus : G \times G \to \rnums^{n}$ is defined by 276 | \begin{align} 277 | \mbf{X}_{2}\ominus\mbf{X}_{1} 278 | &\coloneqq \log\left(\mbf{X}_{2}\inv\mbf{X}_{1}\right)\contract\\ 279 | &= \Log\left(\mbf{X}_{2}\inv\mbf{X}_{1}\right). 280 | \end{align} 281 | 282 | \subsection{The invariant extended Kalman filter} 283 | Without deep derivation, let's take the extended Kalman filter equations \eqref{eq:EKF eqns} and expand them to states that live on smooth manifolds. Let's simply replace the Euclidean $+$ and $-$ operators with $\oplus$ and $\ominus$, respectively\footnote{There are different `flavors' as discussed above.}. 284 | 285 | The prediction equations will then be 286 | \begin{align} 287 | \label{eq:InEKF: state prediction} 288 | \mbfcheck{X}_{k} &= \mbf{F}\left( \mbfhat{X}_{k-1}, \mbf{u}_{k-1}, \mbf{0} \right),\\ 289 | \label{eq:InEKF: state prediction cov.} 290 | \mbfcheck{P}_{k} &= \mbf{A}_{k-1}\mbfhat{P}_{k-1}\mbf{A}_{k-1} + \mbf{L}_{k-1}\mbf{Q}_{k-1}\mbf{L}_{k-1}^{\trans}. 291 | \end{align} 292 | But what are $\mbfcheck{P}_{k}$, $\mbf{A}_{k-1}$, and $\mbf{L}_{k-1}$ exactly? Remember, we had to define an error (left invariant, right invariant, etc.). 293 | 294 | Similar to the estimator prediction error \eqref{eq:KF: prediction err. def.} and correction error \eqref{eq:KF: correction err. def.} of the extended Kalman filter, define the estimator prediction and correction left-invariant errors as 295 | \sidenote{Note that $\mbf{X}_{k}$ is not a random variable in the error definition.} 296 | \sidenote{The definition $\delta\mbscheckrv{\xi}\coloneqq \mbfcheckrv{X} \liminus \mbf{X}$ can also be used. However, the KF equations would look slightly different but the results would still hold.} 297 | \begin{align} 298 | \label{eq:InEKF: prediction error} 299 | \delta\mbscheckrv{\xi}_{k} &\coloneqq \mbf{X}_{k} \liminus \mbfcheckrv{X}_{k},\\ 300 | \label{eq:InEKF: correction error} 301 | \delta\mbshatrv{\xi}_{k} &\coloneqq \mbf{X}_{k} \liminus \mbfhatrv{X}_{k}, 302 | \end{align} 303 | respectively. 304 | 305 | Plugging \eqref{eq:InEKF: process model} and \eqref{eq:InEKF: state prediction} into \eqref{eq:InEKF: prediction error} results in 306 | \begin{align} 307 | \delta\mbscheckrv{\xi}_{k} 308 | &= \mbf{X}_{k} \liminus \mbfcheckrv{X}_{k}\\ 309 | &= \mbf{F}\left( \mbfhatrv{X}_{k-1}, \mbf{u}_{k-1}, \mbfrv{w}_{k-1} \right) \liminus \mbf{F}\left( \mbfhat{X}_{k-1}, \mbf{u}_{k-1}, \mbf{0} \right)\\ 310 | &= \Log\left( \left( \mbf{X}_{k-1}\mbs{\Xi}_{k-1}\Exp(\mbfrv{w}_{k-1}) \right)\inv\mbfhatrv{X}_{k-1}\mbs{\Xi}_{k-1} \right)\\ 311 | &= \Log\left( \Exp(-\mbfrv{w}_{k-1})\mbs{\Xi}_{k-1}\inv\mbf{X}_{k-1}\inv\mbfhatrv{X}_{k-1}\mbs{\Xi}_{k-1} \right)\\ 312 | &= \Log\left( \Exp(-\mbfrv{w}_{k-1})\mbs{\Xi}_{k-1}\inv\Exp(\mbshatrv{\xi}_{k-1})\mbs{\Xi}_{k-1} \right)\\ 313 | &= \Log\left( \Exp(-\mbfrv{w}_{k-1})\Exp\left( \Adj{\mbs{\Xi}_{k-1}\inv}\mbshatrv{\xi}_{k-1} \right) \right)\\ 314 | \label{eq:InEKF: prediction err. function of Jacs} 315 | &\approx \underbrace{\Adj{\mbs{\Xi}_{k-1}\inv}}_{\mbf{A}_{k-1}}\mbshatrv{\xi}_{k-1} - \mbfrv{w}_{k-1}, 316 | \end{align} 317 | \marginnote[-11em]{Note the usage of $\Exp(\mbshatrv{\xi}_{k-1}) = \mbf{X}_{k-1}\inv\mbfhatrv{X}_{k-1}$.} 318 | \marginnote[-7em]{Note $\mbf{X}\Exp(\mbs{\xi})\mbf{X}\inv = \Exp(\Adj{\mbf{X}}\mbs{\xi})$.} 319 | where the last equation is an approximation from the BCH formula \cite{Barfoot_State_2017} and $\mbf{L}_{k-1} = -\eye$. 320 | 321 | The covariance on the prediction error is then given by 322 | \begin{align} 323 | \mbfcheck{P}_{k} &\coloneqq \cov{\mbscheckrv{\xi}_{k}} \\ 324 | &\approx \mbf{A}_{k-1}\mbfhat{P}_{k-1}\mbf{A}_{k-1}^{\trans} + \mbf{L}_{k-1}\mbf{Q}_{k-1}\mbf{L}_{k-1}^{\trans}. 325 | \end{align} 326 | 327 | The correction equations can be generalized from \eqref{eq:EKF: correction} by using `$\liplus$' in place of `$+$'. Specifically, 328 | \begin{align} 329 | \mbfhatrv{X}_{k} &= \mbfcheckrv{X}_{k} \liplus \left( \mbf{K}_{k}(\mbf{y}_{k} - \mbf{g}(\mbfcheckrv{X}_{k}, \mbf{0})) \right). 330 | \end{align} 331 | What's the covariance on $\mbfhatrv{X}_{k}$? 332 | \sidenote{Actually, covariance is on $\mbshatrv{\xi}_{k}$.} To answer the question, lets' stick with the left-invariant measurement function \eqref{eq:InEKF: LI meas. model} and plug in the appropriate variables into \eqref{eq:InEKF: correction error}. 333 | \begin{align} 334 | \mbshatrv{\xi}_{k} 335 | &= \mbf{X}_{k}\liminus\mbfhatrv{X}_{k}\\ 336 | &= \mbf{X}_{k} \liminus \left( \mbfcheckrv{X}_{k}\liplus (\mbf{K}_{k}\mbfrv{z}_{k}) \right)\\ 337 | &= \Log\left( \mbf{X}_{k}\inv\mbfcheckrv{X}_{k}\Exp(-\mbf{K}_{k}\mbfrv{z}_{k}) \right)\\ 338 | &= \Log(\Exp(\mbscheckrv{\xi}_{k})\Exp(-\mbf{K}_{k}\mbfrv{z}_{k}))\\ 339 | \label{eq:InEKF: correction error as a function of innovation} 340 | &\approx \mbscheckrv{\xi}_{k} - \mbf{K}_{k}\mbfrv{z}_{k}. 341 | \end{align} 342 | What's the innovation $\mbfrv{z}_{k}$? That's where invariant filtering comes in. Define the left-invariant innovation\sidenote{The left-invariant innovation is used since we assumed we have a left-invariant measurement function.} by 343 | \begin{align} 344 | \label{eq:InEKF: innovation: begin} 345 | \mbfrv{z}_{k} 346 | &\coloneqq \mbfcheckrv{X}_{k}\inv\left( \mbfrv{y}_{k} - \mbf{g}(\mbfcheckrv{X}_{k},\mbf{0}) \right)\\ 347 | &= \mbfcheckrv{X}_{k}\inv\left( \mbf{X}_{k}\mbf{b} + \mbfrv{n}_{k} - \mbfcheck{X}_{k}\mbf{b}\right)\\ 348 | &= \mbfcheckrv{X}_{k}\inv\mbf{X}_{k}\mbf{b} + \mbfcheckrv{X}_{k}\inv\mbfrv{n}_{k} - \mbf{b}\\ 349 | &= \Exp(-\mbscheckrv{\xi}_{k})\mbf{b} - \mbf{b} \mbfcheckrv{X}_{k}\inv\mbfrv{n}_{k}\\ 350 | &\approx \left( \eye - \mbscheckrv{\xi}_{k}\expand \right)\mbf{b} - \mbf{b} + \mbfcheckrv{X}_{k}\inv\mbfrv{n}_{k}\\ 351 | &= -\mbscheckrv{\xi}_{k}\expand\mbf{b} + \mbfcheckrv{X}_{k}\inv\mbfrv{n}_{k}\\ 352 | \label{eq:InEKF: innovation: Jacobians} 353 | &= \underbrace{-\mbf{b}^{\odot}}_{\mbf{H}_{k}}\mbscheckrv{\xi}_{k} + \underbrace{\mbfcheckrv{X}_{k}\inv}_{\mbf{M}_{k}}\mbfrv{n}_{k}, 354 | \end{align} 355 | \marginnote[-1.5cm]{Note that the Jacobian w.r.t. the state $\mbf{H}_{k}$ is state-independent. That is, it does not depend on the state $\mbf{X}_{k}$.} 356 | where 357 | \begin{align} 358 | \mbs{\xi}\expand\mbf{b} \coloneqq \mbf{b}^{\odot}\mbs{\xi}\\ 359 | \end{align} 360 | is defined 361 | \begin{align} 362 | \bbm 363 | \mbf{b}_{1:3}\\ 364 | b_{4} 365 | \ebm^{\odot} 366 | &= 367 | \bbm 368 | -\mbf{b}_{1:3}\crossop & b_{4}\eye \\ \mbf{0} & \mbf{0} 369 | \ebm 370 | \end{align} 371 | for $SE(3)$ and 372 | \begin{align} 373 | \bbm 374 | \mbf{b}_{1:2}\\ 375 | b_{3} 376 | \ebm^{\odot} 377 | &= 378 | \bbm 379 | (1)\crossop\mbf{b}_{1:2} & b_{3}\eye \\ \mbf{0} & \mbf{0} 380 | \ebm\\ 381 | &= 382 | \bbm 383 | \bbm 0 & -1\\1 & 0 \ebm\mbf{b}_{1:2} & b_{3}\eye \\ \mbf{0} & \mbf{0} 384 | \ebm 385 | \end{align} 386 | for $SE(2)$. 387 | 388 | Plugging the above results into \eqref{eq:InEKF: correction error as a function of innovation} gives the approximation 389 | \begin{align} 390 | \mbshatrv{\xi}_{k} &= 391 | \mbscheckrv{\xi}_{k} - \mbf{K}_{k}\mbfrv{z}_{k}\\ 392 | &\approx 393 | \mbscheckrv{\xi}_{k} - \mbf{K}_{k}\left( \mbf{H}_{k}\mbscheckrv{\xi}_{k} + \mbf{M}_{k}\mbfrv{n}_{k} \right)\\ 394 | &= \left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)\mbscheckrv{\xi}_{k} - \mbf{K}_{k}\mbf{M}_{k}\mbfrv{n}_{k}, 395 | \end{align} 396 | which looks very similar to the Kalman filter correction equation with a difference of sign on $\mbfrv{n}_{k}$. The covariance on the correction error is then given by 397 | \begin{align} 398 | \mbfhat{P}_{k} &\coloneqq \cov{\mbshatrv{\xi}_{k}}\\ 399 | &\approx \left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)\mbfcheck{P}_{k}\left( \eye - \mbf{K}_{k}\mbf{H}_{k} \right)^{\trans} + \mbf{K}_{k}\mbf{M}_{k}\mbf{R}_{k}\mbf{M}_{k}^{\trans}\mbf{K}_{k}^{\trans}, 400 | \end{align} 401 | which exactly matches the EKF correction covariance \eqref{eq:EKF: correction cov}. 402 | 403 | 404 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 405 | % L-InEKF example 406 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 407 | \section{Example: Left-invariant extended Kalman filter} 408 | In this section, an example of the left-invariant extended Kalman filter (L-InEKF) is presented. The example is applied on a robot in 3D space. That is, the states $\mbf{X}$ live in the 3D special Euclidean Lie group $SE(3)$. A \cpp implementation of this example is provided in the repository. 409 | 410 | The state is given by 411 | \begin{align} 412 | \mbf{X}_{k} &= 413 | \bbm \mbf{C}_{ab_{k}} &\disp[b_{k}a][a] \\ \mbf{0} & 1 \ebm\\ 414 | &= \Exp(\mbs{\xi}_{k}), 415 | \end{align} 416 | where $\dcm[ab]\in SO(3)$ is the attitude\footnote{Specifically, it's the DCM of frame $\rframe_{a}$ relative to frame $\rframe_{b}$.} and $\disp[b_{k}a][a]$ is the displacement of point $b_{k}$ (robot body) relative to some arbitrary point $a$ resolved in the (world) frame $\rframe_{a}$.\footnote{I realize that it might be confusing to use the same letter to denote a point and a frame.} 417 | Furthermore, the Lie algebra coordinates are given by 418 | \begin{align} 419 | \label{eq:SE2 xi = [xiphi; xir]} 420 | \mbs{\xi}_{k} &= 421 | \bbm 422 | \mbs{\xi}^{\phi}\\ 423 | \mbs{\xi}^{\mathrm{r}} 424 | \ebm, 425 | \end{align} 426 | where $\mbs{\xi}^{\phi}$ are coordinates associated with attitude, and $\mbs{\xi}^{\mathrm{r}}$ are the generalized position\footnote{Note that $\mbs{\xi}^{\mathrm{r}}\neq\disp[b_{k}a][a]$ in general.}. Note that some authors use different ordering for $\mbs{\xi}$. For example, in \cite{Barfoot_State_2017}, the ordering of $\mbs{\xi}^{\phi}$ and $\mbs{\xi}^{\mathrm{r}}$ is flipped. This has an effect on the Adjoint representation and the Jacobians, so care must be taken. 427 | 428 | \subsection{Process model} 429 | The process model is given by \eqref{eq:InEKF: SE3 process model} and the Jacobians are given by \eqref{eq:InEKF: prediction err. function of Jacs}. 430 | Specifically, the Jacobian of the process model w.r.t. $\mbs{\xi}$ is given by 431 | \begin{align} 432 | \mbf{A}_{k-1} &= \Adj{\mbs{\Xi}_{k-1}\inv}, 433 | \end{align} 434 | where 435 | \begin{align} 436 | \Adj{\mbf{X}} &= 437 | \bbm 438 | \dcm[ab] & \mbf{0} \\ 439 | {\disp[ba][a]}\crossop\dcm[ab] & \dcm[ab] 440 | \ebm,\\ 441 | \Adj{\mbf{X}\inv} &= 442 | \bbm 443 | \dcmtrans[ab] & \mbf{0} \\ 444 | -\dcmtrans[ab]{\disp[ba][a]}\crossop & \dcmtrans[ab] 445 | \ebm. 446 | \end{align} 447 | \marginnote[-2cm]{Note that the Adjoint representation depends on the ordering of $\mbs{\xi}$ defined in \eqref{eq:SE2 xi = [xiphi; xir]}.} 448 | 449 | \subsection{Measurement model: GPS} 450 | Let the measurement model be a position sensor given by 451 | \begin{align} 452 | \mbfrv{y}_{k} 453 | &= \disp[b_{k}a][a] + \mbfrv{n}_{k} \\ 454 | &= \bbm \eye & \mbf{0} \ebm 455 | \underbrace{\bbm \dcm[ab_{k}] & \disp[b_{k}a][a] \\\mbf{0} & 1 \ebm}_{\mbf{X}_{k}} 456 | \underbrace{\bbm \mbf{0}\\ 1 \ebm}_{\mbf{b}} + \mbfrv{n}_{k}. 457 | \end{align} 458 | To make things easier, let 459 | \begin{align} 460 | \underbrace{ 461 | \bbm 462 | \mbfrv{y}_{k} \\ 463 | 1 464 | \ebm}_{\mbftilderv{y}_{k}} 465 | &= 466 | \mbf{X}_{k}\mbf{b} + 467 | \underbrace{ 468 | \bbm 469 | \mbfrv{n}_{k} \\ 0 470 | \ebm}_{\tilde{\mbfrv{n}}_{k}}. 471 | \end{align} 472 | 473 | The innovation \eqref{eq:InEKF: innovation: Jacobians} (using $\tilde{\mbfrv{y}}_{k}$) is then given by 474 | \begin{align} 475 | \mbftilderv{z}_{k} 476 | &\approx -\mbf{b}^{\odot}\mbscheckrv{\xi}_{k} + \mbfcheckrv{X}_{k}\inv \mbftilderv{n}_{k}\\ 477 | &= -\bbm \mbf{0} & \eye \\ \mbf{0} & \mbf{0} \ebm \mbscheckrv{\xi}_{k} + \bbm \mbfcheckrv{C}_{ab_{k}}^{\trans} & -\mbfcheckrv{C}_{ab_{k}}^{\trans}\mbfcheckrv{r}^{b_{k}a}_{a} \\ \mbf{0} & 1 \ebm \bbm \mbfrv{n}_{k} \\ 0 \ebm\\ 478 | &= 479 | \bbm 480 | - \bbm \mbf{0} & \eye \ebm\mbscheckrv{\xi}_{k} + \mbfcheckrv{C}_{ab_{k}}^{\trans}\mbfrv{n}_{k}\\ 481 | 0 482 | \ebm\\ 483 | &= \bbm 484 | \mbfrv{z}_{k}\\ 485 | 0 486 | \ebm. 487 | \end{align} 488 | \marginnote[-4.5cm]{ 489 | From \cite{Barfoot_State_2017}, the $(\cdot)^{\odot}$ operator for $SE(3)$ is given by 490 | \begin{align} 491 | \bbm 492 | \mbf{b}_{1:3}\\ 493 | b_{4} 494 | \ebm^{\odot} 495 | &= 496 | \bbm 497 | -\mbf{b}_{1:3}\crossop & b_{4}\eye \\ \mbf{0} & \mbf{0} 498 | \ebm. 499 | \end{align} 500 | } 501 | Ignoring the last row of the equation above gives the (modified) innovation 502 | \begin{align} 503 | \mbfrv{z}_{k} &= \underbrace{-\bbm \mbf{0} & \eye \ebm}_{\mbf{H}_{k}}\mbscheckrv{\xi}_{k} + \underbrace{\mbfcheckrv{C}_{ab_{k}}^{\trans}}_{\mbf{M}_{k}}\mbfrv{n}_{k}. 504 | \end{align} 505 | Thus, 506 | \begin{align} 507 | \mbf{H}_{k} &= - \bbm \mbf{0} & \eye \ebm,\\ 508 | \mbf{M}_{k} &= \mbfcheckrv{C}_{ab_{k}}^{\trans}. 509 | \end{align} 510 | \marginnote[-1.7cm]{Note that the Jacobian of the innovation w.r.t. to $\mbscheck{\xi}_{k}$ is state-independent.} 511 | 512 | \subsection{Results} 513 | The results of using the $SE(2)$ \cpp filter are presented in 514 | Figure~\ref{fig:se2_example_trajectories}-\ref{fig:se2_example_error_plots}. 515 | 516 | \begin{figure}[h] 517 | \centering 518 | \includegraphics[width=\textwidth]{figs/se2_example_trajectories.eps} 519 | \caption{Ground truth and estimated trajectories from the $SE(2)$ example.} 520 | \label{fig:se2_example_trajectories} 521 | \end{figure} 522 | 523 | \begin{figure}[h] 524 | \centering 525 | \includegraphics[width=\textwidth]{figs/se2_example_error_plots.eps} 526 | \caption{Error plots from the $SE(2)$ example.\\ Note that $\mbs{\xi}_{k} = \Log(\mbf{X}_{k})$, where $\mbf{X}_{k}\in SE(2)$ is the planar pose at time $t_{k}$.} 527 | \label{fig:se2_example_error_plots} 528 | \end{figure} 529 | 530 | \clearpage 531 | \begin{appendices} 532 | \input{sections/appendices.tex} 533 | \end{appendices} 534 | 535 | \clearpage 536 | \printbibliography 537 | \end{document} -------------------------------------------------------------------------------- /doc/sections/appendices.tex: -------------------------------------------------------------------------------- 1 | \section{Deriving the invariant extended Kalman filter} 2 | The filtering problem is a MAP problem given by 3 | \begin{align} 4 | \mbfhat{X}_{k} 5 | &= \argmax_{\mbf{X}_{k}\in G} \pdf{\mbf{X}_{k+1}\mid \mbfhat{X}_{k-1}, \mbf{u}_{k-1}, \mbf{y}_{k}}\\ 6 | \label{eq:InEKF derivation:pdf Xhat given Xhatkm1 ukm1 yk} 7 | &= \argmax_{\mbf{X}_{k-1}\in G} \pdf{\mbf{y}_{k}\mid \mbf{X}_{k}}\pdf{\mbf{X}_{k}\mid\mbfhat{X}_{k-1}, \mbf{u}_{k-1}}, 8 | \end{align} 9 | where $\mbfhat{X}_{k-1}$ is the InEKF estimate of $\mbf{X}_{k-1}$. 10 | 11 | As briefly discussed in Section~\ref{sec:Random variables on Lie groups}, random variables on Lie groups can be described by 12 | \begin{align} 13 | \pdf{\mbf{X}; \mbfbar{X}, \mbs{\Sigma}} &= 14 | \mc{N}\left( \mbf{X}\ominus\mbfbar{X}, \mbs{\Sigma} \right)\\ 15 | &= 16 | \label{eq:pdf of X in Lie group} 17 | \eta\exp\left(-\f{1}{2}\norm{ 18 | \mbf{X}\ominus\mbfbar{X} 19 | }_{\mbs{\Sigma}\inv}^{2}\right). 20 | \end{align} 21 | 22 | Let 23 | \begin{align} 24 | \mbfcheck{X}_{k} &\coloneqq (\mbfhat{X}_{k-1}\rplus\mbf{u}_{k-1}) \oplus\mbfrv{w}_{k-1}, 25 | \end{align} 26 | and let the measurement function be given by 27 | \begin{align} 28 | \mbf{y}_{k} &= \mbf{X}_{k}\mbf{b} + \mbfrv{n}_{k}, 29 | \end{align} 30 | where $\mbf{b}$ is a know column matrix. 31 | 32 | Taking the above assumptions into consideration, and plugging \eqref{eq:pdf of X in Lie group} into the MAP objective function \eqref{eq:InEKF derivation:pdf Xhat given Xhatkm1 ukm1 yk} and taking the negative log, results in 33 | \begin{align} 34 | \mbfhat{X}_{k} 35 | &= \argmax_{\mbf{X}_{k}\in G} \pdf{\mbf{y}_{k}\mid \mbf{X}_{k}}\pdf{\mbf{X}_{k}\mid\mbfhat{X}_{k-1}, \mbf{u}_{k-1}}\\ 36 | &= \argmin_{\mbf{X}_{k}\in G} \f{1}{2}\norm{\mbf{y}_{k} - \mbf{X}_{k}\mbf{b}}^{2}_{\mbf{R}\inv} 37 | + \f{1}{2}\norm{ 38 | \mbf{X}_{k}\ominus\mbfcheck{X}_{k} 39 | }^{2}_{\mbfcheck{P}_{k}\inv}, 40 | \end{align} 41 | which is a (nonlinear) least squares problem. 42 | 43 | The optimization problem can be rewritten as 44 | \begin{align} 45 | \label{eq:deriving InEKF: NLS et SigmaInv e} 46 | \mbfhat{X}_{k} &= \argmin_{\mbf{X}_{k}\in G} \f{1}{2}\mbf{e}(\mbf{X}_{k})^{\trans}\mbs{\Sigma}\inv\mbf{e}(\mbf{X}_{k}), 47 | \end{align} 48 | where 49 | \begin{align} 50 | \label{eq:deriving InEKF: error function def} 51 | \mbf{e} &: G \to \rnums^{m},\\ 52 | \mbf{e}(\mbf{X}_{k}) &= 53 | \bbm 54 | \mbf{X}_{k} \ominus \mbfcheck{X}_{k}\\ 55 | \mbf{X}_{k}\mbf{b} - \mbf{y}_{k} 56 | \ebm,\\ 57 | \mbs{\Sigma} &= 58 | \bbm 59 | \mbfcheck{P}_{k} & \\ & \mbf{R}_{k} 60 | \ebm. 61 | \end{align} 62 | \begin{remark} 63 | Note that the error function $\mbf{e}$ is a mapping from the (Lie) group space $G$ to a the Lie algebra vector space $\rnums^{m}$. That's the nice thing about Lie algebra; we can use the standard optimization tools on the Lie algebra Euclidean space. 64 | \end{remark} 65 | 66 | The optimization problem \eqref{eq:deriving InEKF: NLS et SigmaInv e} can be solved using Gauss-Newton algorithm \cite{Nocedal_Numerical_2006, Barfoot_State_2017, Dellaert_Factor_2017}. 67 | 68 | First, get an affine approximation of the error function \eqref{eq:deriving InEKF: error function def} linearized around the operating point $\mbfbar{X}_{k}$. To do so, define the perturbation 69 | \begin{align} 70 | \mbf{d}_{k} &\coloneqq \mbf{X}_{k}\ominus\mbfbar{X}_{k},\\ 71 | \label{eq:deriving InEKF: X_k = Xbar oplus dk} 72 | \mbf{X}_{k} &= \mbfbar{X}_{k} \oplus\mbf{d}_{k}, 73 | \end{align} 74 | \marginnote[-1cm]{The variable name $\mbf{d}_{k}\in\rnums^{n_{x}}$ is chosen because it'll be used as a search direction in the optimization.} 75 | according to the chosen error definition\sidenote{That is, replace `$\ominus$' and `$\oplus$' with the appropriate definition such as `$\liplus$'.}. 76 | 77 | Then, the affine approximation can be written as 78 | \begin{align} 79 | \label{eq:InEKF derivation: error affine approx} 80 | \mbf{e}(\mbf{X}_{k}) &= \mbf{e}(\mbfbar{X}_{k}\oplus\mbf{d}_{k})\\ 81 | &\approx \mbf{e}\left( \mbfbar{X}_{k} \right) + \mbf{J}\mbf{d}_{k}, 82 | \end{align} 83 | where $\mbf{J}\in\rnums^{m\times n}$ is the Jacobian of the error function with respect to the Lie algebra coordinates. 84 | \sidenote{Basically, 85 | \begin{align} 86 | \mbf{J} &= \pd{\mbf{e}(\mbfbar{X}_{k}\oplus \mbs{\xi}_{k})}{\mbs{\xi}_{k}}, 87 | \end{align} 88 | where $\mbs{\xi}_{k}\in \rnums^{n_{k}}$ is the coordinates of the Lie algebra components. 89 | } 90 | 91 | Plugging the error affine approximation \eqref{eq:InEKF derivation: error affine approx} into the objective function gives the quadratic\sidenote{Quadratic in $\mbf{d}_{k}$.} approximation 92 | \begin{align} 93 | \tilde{J}(\mbf{d}_{k}) &\coloneqq 94 | J(\mbfbar{X}_{k}\oplus\mbf{d}_{k}) \\ 95 | &= \f{1}{2}\mbf{e}(\mbfbar{X}_{k}\oplus\mbf{d}_{k})^{\trans}\mbs{\Sigma}\inv\mbf{e}(\mbfbar{X}_{k}\oplus\mbf{d}_{k})\\ 96 | &\approx \f{1}{2}\left( \mbf{e}\left( \mbfbar{X}_{k} \right) + \mbf{J}\mbf{d}_{k} \right)^{\trans}\mbs{\Sigma}\inv\left( \mbf{e}\left( \mbfbar{X}_{k} \right) + \mbf{J}\mbf{d}_{k} \right)\\ 97 | &= \f{1}{2}\mbf{d}_{k}^{\trans}\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{J}\mbf{d}_{k} + \mbf{e}\left( \mbfbar{X}_{k} \right)^{\trans}\mbs{\Sigma}\inv\mbf{J}\mbf{d}_{k} + \f{1}{2}\mbf{e}\left( \mbfbar{X}_{k} \right)^{\trans}\mbs{\Sigma}\inv\mbf{e}\left( \mbfbar{X}_{k} \right). 98 | \end{align} 99 | If $\mbf{J}$ is full column rank, then the quadratic function $\tilde{J}$ is strongly convex and has a unique minimizer. The minimizer is given by solving 100 | \begin{align} 101 | \label{eq:deriving InEKF: optimal search direction} 102 | \mbf{d}_{k}^{\star} &= - \left(\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{J}\right)\inv\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{e}(\mbfbar{X}_{k}), 103 | \end{align} 104 | where 105 | \begin{align} 106 | \label{eq:deriving InEKF: Phat} 107 | \mbfhat{P}_{k} &= \left(\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{J}\right)\inv 108 | \end{align} 109 | at convergence \cite{Barfoot_State_2017}. 110 | 111 | We can try to solve for $\mbf{d}_{k}$ analytically. 112 | Say the error function Jacobian is given by 113 | \begin{align} 114 | \label{eq:deriving InEKF: Jacobian eye C} 115 | \mbf{J} &= \bbm \gamma\eye \\ \mbf{C}_{k}\ebm, 116 | \end{align} 117 | where $\gamma = \pm 1$ depends on the choice of the error definition. 118 | 119 | \subsection{The Kalman gain} 120 | Plugging \eqref{eq:deriving InEKF: Jacobian eye C} into \eqref{eq:deriving InEKF: Phat} gives 121 | \begin{align} 122 | \mbfhat{P}_{k}\inv &= \left(\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{J}\right)\\ 123 | &= 124 | \mbfcheck{P}_{k}\inv + \mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv\mbf{C}_{k}. 125 | \end{align} 126 | Pre-multiplying by $\mbfhat{P}_{k}$ gives 127 | \begin{align} 128 | \eye 129 | &= \mbfhat{P}_{k}\mbfcheck{P}_{k}\inv + \underbrace{\mbfhat{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv}_{\eqqcolon \mbf{K}_{k}}\mbf{C}_{k}\\ 130 | \label{eq:deriving InEKF: eye = PhatPcheckinv + KkCk} 131 | &=\mbfhat{P}_{k}\mbfcheck{P}_{k}\inv + \mbf{K}_{k}\mbf{C}_{k}, 132 | \end{align} 133 | where $\mbf{K}_{k}$ is the \emph{Kalman gain}. 134 | Solving for $\mbfhat{P}_{k}$ results in 135 | \begin{align} 136 | \mbfhat{P}_{k} &= \left( \eye - \mbf{K}_{k}\mbf{C}_{k} \right)\mbfcheck{P}_{k}. 137 | \end{align} 138 | Post-multiply by $\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv$ gives 139 | \begin{align} 140 | \underbrace{\mbfhat{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv}_{\mbf{K}_{k}} &= \left( \eye - \mbf{K}_{k}\mbf{C}_{k} \right)\mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv\\ 141 | \mbf{K}_{k} &= \mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv - \mbf{K}_{k}\mbf{C}_{k}\mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv\\ 142 | \mbf{K}_{k}\left( \eye + \mbf{C}_{k}\mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv \right) &= \mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv 143 | \end{align} 144 | Post-multiply by $\mbf{R}_{k}$ 145 | \begin{align} 146 | \mbf{K}_{k}\left( \mbf{R}_{k} + \mbf{C}_{k}\mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\right) &= \mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}. 147 | \end{align} 148 | Finally, the Kalman gain is given by 149 | \begin{align} 150 | \mbf{K}_{k} &= 151 | \mbfcheck{P}_{k}\mbf{C}_{k}^{\trans} \underbrace{\left( \mbf{R} + \mbf{C}_{k}\mbfcheck{P}_{k}\mbf{C}_{k}^{\trans}\right)\inv}_{\mbf{S}_{k}\inv}. 152 | \end{align} 153 | 154 | \subsection{The search direction} 155 | Plugging the posterior covariance \eqref{eq:deriving InEKF: Phat} into the optimal search direction \eqref{eq:deriving InEKF: optimal search direction} results in 156 | \begin{align} 157 | \mbf{d}_{k}^{\star} &= - \mbfhat{P}_{k}\mbf{J}^{\trans}\mbs{\Sigma}\inv\mbf{e}(\mbfbar{X}_{k})\\ 158 | &= 159 | -\mbfhat{P}_{k} \bbm \gamma\eye & \mbf{C}_{k}^{\trans} \ebm 160 | \bbm \mbfcheck{P}_{k}\inv & \\ & \mbf{R}_{k}\inv \ebm 161 | \bbm \mbfbar{X}_{k}\ominus \mbfcheck{X}_{k} \\\mbfbar{X}_{k}\mbf{b} - \mbf{y}_{k} \ebm 162 | \\ 163 | &= -\mbfhat{P}_{k}\left( \gamma \mbfcheck{P}_{k}\inv\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv(\mbfbar{X}_{k}\mbf{b} - \mbf{y}_{k})\right)\\ 164 | &= -\gamma\mbfhat{P}_{k}\mbfcheck{P}_{k}\inv\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) - \underbrace{\mbfhat{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv}_{\mbf{K}_{k}}(\mbfbar{X}_{k}\mbf{b} - \mbf{y}_{k})\\ 165 | &\overset{\star}{=} 166 | -\gamma\left(\eye - \mbf{K}_{k}\mbf{C}_{k} \right)\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) - \underbrace{\mbfhat{P}_{k}\mbf{C}_{k}^{\trans}\mbf{R}_{k}\inv}_{\mbf{K}_{k}}(\mbfbar{X}_{k}\mbf{b} - \mbf{y}_{k}) \\ 167 | &= -\gamma\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \gamma\mbf{K}_{k}\mbf{C}_{k}\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) - \mbf{K}_{k}\left( \mbfbar{X}_{k}\mbf{b} - \mbf{y}_{k} \right)\\ 168 | &= -\gamma\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{K}_{k}\left(\gamma \mbf{C}_{k} \left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \left( \mbf{y}_{k} - \mbfbar{X}_{k}\mbf{b}\right)\right)\\ 169 | &= -\gamma\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{K}_{k}\left(\gamma \mbf{C}_{k} \left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \left( \mbf{y}_{k} - \mbfbar{X}_{k}\mbf{b}\right)\right)\\ 170 | &= -\gamma\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{K}_{k}\mbf{z}_{k}(\mbfbar{X}_{k}), 171 | \end{align} 172 | \marginnote[-3.5cm]{$\star$ From \eqref{eq:deriving InEKF: eye = PhatPcheckinv + KkCk}, 173 | \begin{align} 174 | \mbfhat{P}_{k}\mbfcheck{P}_{k}\inv &= \eye - \mbf{K}_{k}\mbf{C}_{k}. 175 | \end{align} 176 | } 177 | where 178 | \begin{align} 179 | \label{eq:deriving InEKF: general innovation} 180 | \mbf{z}_{k}(\mbfbar{X}_{k}) &= \gamma\mbf{C}_{k}\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{y}_{k} - \mbfbar{X}_{k}\mbf{b} 181 | \end{align} 182 | is the innovation. 183 | 184 | Finally, using \eqref{eq:deriving InEKF: X_k = Xbar oplus dk}, the posterior estimate is 185 | \begin{align} 186 | \mbfhat{X}_{k} 187 | &= \mbfbar{X}\oplus\mbf{d}_{k}^{\star}\\ 188 | \label{eq:deriving InEKF: general update equation} 189 | &= \mbfbar{X}\oplus\left(-\gamma\left( \mbfbar{X}_{k}\ominus\mbfcheck{X}_{k} \right) + \mbf{K}_{k}\mbf{z}_{k}(\mbfbar{X}_{k})\right). 190 | \end{align} 191 | Note that if the linearization point is defined as 192 | \begin{align} 193 | \mbfbar{X}_{k} &\coloneqq \mbfcheck{X}_{k}, 194 | \end{align} 195 | then 196 | the innovation \eqref{eq:deriving InEKF: general innovation} simplifies to 197 | \begin{align} 198 | \mbf{z}_{k}(\mbfcheck{X}_{k}) &= \mbf{y}_{k} - \mbfbar{X}_{k}\mbf{b}, 199 | \end{align} 200 | and the update equation \eqref{eq:deriving InEKF: general update equation} becomes 201 | \begin{align} 202 | \mbfbar{X}_{k} &= \mbfcheck{X}_{k}\oplus\left( \mbf{K}_{k}\mbf{z}_{k}(\mbfcheck{X}_{k}) \right)\\ 203 | &= \mbfcheck{X}_{k}\oplus\left( \mbf{K}_{k}(\mbf{y}_{k} - \mbfcheck{X}_{k}\mbf{b}) \right). 204 | \end{align} 205 | This is the invariant extended Kalman filter correction. -------------------------------------------------------------------------------- /doc/sections/titlepage.tex: -------------------------------------------------------------------------------- 1 | \makeatletter 2 | \begin{titlepage} 3 | \begin{fullwidth} 4 | \begin{center} 5 | 6 | % \vspace*{cm} 7 | 8 | % % \includegraphics[width=0.3\textwidth]{figs/McGill_Wordmark.png} 9 | % \vspace{0.5cm} 10 | 11 | % \Huge 12 | % \textsc{McGill University} 13 | % \vspace{0.5cm} 14 | 15 | % \Huge 16 | % \textsc{DECAR Group} 17 | 18 | % \vspace{0.5cm} 19 | 20 | \LARGE 21 | \textsc{Notes} 22 | 23 | \vspace{5cm} 24 | 25 | \Huge 26 | % \thline 27 | \vspace{0.6em} 28 | \textbf{\@title} 29 | % \thline 30 | 31 | \vfill 32 | 33 | \Large 34 | {\@author}\\[10pt] 35 | % 26062940\\[10pt] 36 | % \textit{Supervisor:}\\ 37 | % {Prof.} {James Richard Forbes}\\[10pt] 38 | 39 | 40 | % \large 41 | % Department of Mechanical Engineering, McGill University\\ 42 | % 817 Sherbrooke Street West, Montreal, QC, H3A 0C3\\[10pt] 43 | 44 | \today 45 | 46 | \end{center} 47 | \end{fullwidth} 48 | \end{titlepage} -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Add Kalman fitler example 2 | message(STATUS "Building KalmanFilter") 3 | add_subdirectory( KalmanFilter) 4 | # Add InEKF 5 | message(STATUS "Building InEKF") 6 | add_subdirectory( InEKF) -------------------------------------------------------------------------------- /examples/InEKF/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required( VERSION 3.10) 2 | project(example_SE2) 3 | 4 | # Append path with Config.cmake dirs/files 5 | 6 | # Find the RandomVariable directory 7 | find_package(RandomVariable) 8 | # Find the Eigen library 9 | find_package(Eigen3 REQUIRED) 10 | # Find the manif library 11 | find_package(manif REQUIRED) 12 | # Find the YAML-cpp library 13 | find_package(yaml-cpp REQUIRED) 14 | find_package(InEKF) 15 | 16 | add_executable(example_SE2 example_SE2.cpp) 17 | 18 | target_link_libraries(example_SE2 19 | PUBLIC 20 | InEKF::SE2 21 | ) 22 | 23 | 24 | if(CMAKE_BUILD_TYPE STREQUAL "Debug") 25 | # Add debug flag 26 | target_compile_definitions(example_SE2 PRIVATE DEBUG) 27 | elseif(CMAKE_BUILD_TYPE STREQUAL "Release") 28 | endif() 29 | 30 | # Set required C++ standard flag 31 | set_property(TARGET example_SE2 PROPERTY CXX_STANDARD 17) 32 | -------------------------------------------------------------------------------- /examples/InEKF/config.yml: -------------------------------------------------------------------------------- 1 | # Keep files absolute 2 | filename_prior : "/home/aa/Documents/Data/Data_generator/SE2/meas_prior.txt" 3 | filename_gyro : "/home/aa/Documents/Data/Data_generator/SE2/meas_gyro.txt" 4 | # Velocity measurements 5 | filename_vel : "/home/aa/Documents/Data/Data_generator/SE2/meas_vel.txt" 6 | # GPS measurements 7 | filename_gps : "/home/aa/Documents/Data/Data_generator/SE2/meas_gps.txt" 8 | # Output file 9 | filename_out : "/home/aa/Documents/Data/Data_generator/SE2/est_xhat.txt" -------------------------------------------------------------------------------- /examples/InEKF/example_SE2.cpp: -------------------------------------------------------------------------------- 1 | #include "inekf_se2.h" 2 | #include 3 | 4 | int main(int argc, const char *argv[]) { 5 | // Configurations 6 | YAML::Node config; 7 | std::string filename_config; 8 | filename_config = argv[1]; 9 | std::cout << "Filename passed: " << filename_config << std::endl; 10 | config = YAML::LoadFile(filename_config); 11 | 12 | // Get output file name 13 | const std::string filename_kf = config["filename_kf"].as(); 14 | 15 | // Run L-InEKF 16 | std::vector X_hat = GetSe2InekfEstimates(filename_config); 17 | 18 | std::cout << "Exporting L-InEKF estimates to " 19 | << config["filename_kf"].as() << std::endl; 20 | RV::IO::write(X_hat, filename_kf, "X"); 21 | } 22 | -------------------------------------------------------------------------------- /examples/InEKF/readme.md: -------------------------------------------------------------------------------- 1 | # In this directory 2 | * An implementation of a left-invariant extended Kalman filter (L-InEKF) on *SE(2)*. 3 | * The simple kinematic model is of a robot driving in plane, with control inputs being 4 | 1. angular velocity, and 5 | 2. linear velocity. 6 | * Position (GPS) measurements are provided as exteroceptive measurements (possibly at different rates than the control inputs). 7 | 8 | # Using the executable 9 | A `configy.yml` (including its path) is passed to the executable as an argument. This configuration file consists of filenames of the sensor data. For example, 10 | 1. `filename_prior` contains a prior on the first *SE(2)$ state. 11 | 2. `filename_gyro` contains rate gyro sensor measurements. 12 | 3. `filename_vel` contains linear velocity (resolved in the body frame) sensor measurements. 13 | 4. `filename_gps` contains the position (GPS) sensor measurements. 14 | 5. `filename_out` specifies the location of the output of the executable. 15 | Note that all the measurements are assumed to be noisy. 16 | 17 | # Generating sensor data 18 | The sensor measurements can be generated using executables from the `RandomVariable` package that is included in `extern/` directory of this repo. 19 | The files can be generated using the `RandomVariable/MATLAB_txt_IO/3D_sim/generate_data_SE2.m` MATLAB script. This script uses a `.mat` file generated from *SE(3)* data generator; this data-generator is not publicly available. -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/gt_states.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aalbaali/cpp_filter/8a1b6745769b2d5d346e3f08f81a080028eb6dfd/examples/InEKF/sensor_data/gt_states.mat -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/meas_gps.txt: -------------------------------------------------------------------------------- 1 | mean_size : 2, 1 2 | dof : 2 3 | num_meas : 22 4 | ================================================================================================================ 5 | Time r_bt_t_11 r_bt_t_12 cov_11 cov_12 cov_21 cov_22 6 | ================================================================================================================ 7 | 0.190000, 0.186553, 0.010742, 0.000100, 0.000000, 0.000000, 0.000100, 8 | 0.390000, 0.388880, -0.004979, 0.000100, 0.000000, 0.000000, 0.000100, 9 | 0.590000, 0.589515, -0.007332, 0.000100, 0.000000, 0.000000, 0.000100, 10 | 0.790000, 0.789315, 0.009238, 0.000100, 0.000000, 0.000000, 0.000100, 11 | 0.990000, 0.976783, 0.011308, 0.000100, 0.000000, 0.000000, 0.000100, 12 | 1.190000, 1.006917, 0.213694, 0.000100, 0.000000, 0.000000, 0.000100, 13 | 1.390000, 1.003258, 0.431585, 0.000100, 0.000000, 0.000000, 0.000100, 14 | 1.590000, 0.858553, 0.484471, 0.000100, 0.000000, 0.000000, 0.000100, 15 | 1.790000, 0.635791, 0.489442, 0.000100, 0.000000, 0.000000, 0.000100, 16 | 1.990000, 0.457936, 0.491572, 0.000100, 0.000000, 0.000000, 0.000100, 17 | 2.190000, 0.245606, 0.499981, 0.000100, 0.000000, 0.000000, 0.000100, 18 | 2.390000, 0.057248, 0.519487, 0.000100, 0.000000, 0.000000, 0.000100, 19 | 2.590000, 0.001557, 0.668409, 0.000100, 0.000000, 0.000000, 0.000100, 20 | 2.790000, -0.001215, 0.848385, 0.000100, 0.000000, 0.000000, 0.000100, 21 | 2.990000, 0.096214, 0.992504, 0.000100, 0.000000, 0.000000, 0.000100, 22 | 3.190000, 0.297827, 1.008273, 0.000100, 0.000000, 0.000000, 0.000100, 23 | 3.390000, 0.492171, 0.952310, 0.000100, 0.000000, 0.000000, 0.000100, 24 | 3.590000, 0.486420, 0.745450, 0.000100, 0.000000, 0.000000, 0.000100, 25 | 3.790000, 0.494636, 0.558788, 0.000100, 0.000000, 0.000000, 0.000100, 26 | 3.990000, 0.494215, 0.364449, 0.000100, 0.000000, 0.000000, 0.000100, 27 | 4.190000, 0.492488, 0.184095, 0.000100, 0.000000, 0.000000, 0.000100, 28 | 4.390000, 0.515546, -0.007304, 0.000100, 0.000000, 0.000000, 0.000100, 29 | -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/meas_gyro.txt: -------------------------------------------------------------------------------- 1 | mean_size : 1, 1 2 | dof : 1 3 | num_meas : 457 4 | ================================================ 5 | Time omega_bi_b_11 cov_11 6 | ================================================ 7 | 0.000000, 0.001309, 0.000025, 8 | 0.010000, 0.002944, 0.000025, 9 | 0.020000, 0.000216, 0.000025, 10 | 0.030000, 0.000204, 0.000025, 11 | 0.040000, 0.004614, 0.000025, 12 | 0.050000, 0.004698, 0.000025, 13 | 0.060000, -0.006270, 0.000025, 14 | 0.070000, -0.006307, 0.000025, 15 | 0.080000, 0.006847, 0.000025, 16 | 0.090000, -0.001335, 0.000025, 17 | 0.100000, 0.000890, 0.000025, 18 | 0.110000, 0.001202, 0.000025, 19 | 0.120000, -0.001163, 0.000025, 20 | 0.130000, 0.000708, 0.000025, 21 | 0.140000, -0.001908, 0.000025, 22 | 0.150000, 0.005442, 0.000025, 23 | 0.160000, 0.002902, 0.000025, 24 | 0.170000, -0.003739, 0.000025, 25 | 0.180000, 0.003369, 0.000025, 26 | 0.190000, -0.006507, 0.000025, 27 | 0.200000, -0.005778, 0.000025, 28 | 0.210000, -0.005957, 0.000025, 29 | 0.220000, -0.000978, 0.000025, 30 | 0.230000, 0.002548, 0.000025, 31 | 0.240000, 0.004679, 0.000025, 32 | 0.250000, -0.002819, 0.000025, 33 | 0.260000, -0.003974, 0.000025, 34 | 0.270000, 0.000924, 0.000025, 35 | 0.280000, 0.003253, 0.000025, 36 | 0.290000, -0.005641, 0.000025, 37 | 0.300000, -0.002016, 0.000025, 38 | 0.310000, 0.000459, 0.000025, 39 | 0.320000, -0.002171, 0.000025, 40 | 0.330000, -0.001130, 0.000025, 41 | 0.340000, -0.002561, 0.000025, 42 | 0.350000, 0.008235, 0.000025, 43 | 0.360000, -0.009041, 0.000025, 44 | 0.370000, 0.000564, 0.000025, 45 | 0.380000, 0.008949, 0.000025, 46 | 0.390000, -0.001699, 0.000025, 47 | 0.400000, 0.001079, 0.000025, 48 | 0.410000, -0.010053, 0.000025, 49 | 0.420000, -0.008066, 0.000025, 50 | 0.430000, -0.007318, 0.000025, 51 | 0.440000, 0.007886, 0.000025, 52 | 0.450000, 0.003915, 0.000025, 53 | 0.460000, -0.002563, 0.000025, 54 | 0.470000, 0.001354, 0.000025, 55 | 0.480000, 0.009567, 0.000025, 56 | 0.490000, -0.004275, 0.000025, 57 | 0.500000, 0.002440, 0.000025, 58 | 0.510000, 0.004119, 0.000025, 59 | 0.520000, -0.006076, 0.000025, 60 | 0.530000, -0.006076, 0.000025, 61 | 0.540000, 0.001778, 0.000025, 62 | 0.550000, 0.003088, 0.000025, 63 | 0.560000, -0.007098, 0.000025, 64 | 0.570000, 0.005423, 0.000025, 65 | 0.580000, -0.002071, 0.000025, 66 | 0.590000, -0.009138, 0.000025, 67 | 0.600000, 0.001181, 0.000025, 68 | 0.610000, 0.006520, 0.000025, 69 | 0.620000, -0.001273, 0.000025, 70 | 0.630000, 0.007080, 0.000025, 71 | 0.640000, 0.003805, 0.000025, 72 | 0.650000, -0.006734, 0.000025, 73 | 0.660000, -0.004181, 0.000025, 74 | 0.670000, -0.000175, 0.000025, 75 | 0.680000, 0.006026, 0.000025, 76 | 0.690000, 0.002604, 0.000025, 77 | 0.700000, -0.006936, 0.000025, 78 | 0.710000, 0.006950, 0.000025, 79 | 0.720000, 0.001015, 0.000025, 80 | 0.730000, 0.004385, 0.000025, 81 | 0.740000, -0.002877, 0.000025, 82 | 0.750000, -0.005011, 0.000025, 83 | 0.760000, -0.004819, 0.000025, 84 | 0.770000, 0.005487, 0.000025, 85 | 0.780000, -0.010093, 0.000025, 86 | 0.790000, 0.000169, 0.000025, 87 | 0.800000, -0.002852, 0.000025, 88 | 0.810000, -0.002194, 0.000025, 89 | 0.820000, -0.002818, 0.000025, 90 | 0.830000, -0.001341, 0.000025, 91 | 0.840000, 0.001265, 0.000025, 92 | 0.850000, -0.003534, 0.000025, 93 | 0.860000, -0.004648, 0.000025, 94 | 0.870000, -0.007393, 0.000025, 95 | 0.880000, -0.005323, 0.000025, 96 | 0.890000, 0.006761, 0.000025, 97 | 0.900000, -0.002200, 0.000025, 98 | 0.910000, -0.007879, 0.000025, 99 | 0.920000, 0.987233, 0.000025, 100 | 0.930000, 6.937916, 0.000025, 101 | 0.940000, 11.666914, 0.000025, 102 | 0.950000, 14.610232, 0.000025, 103 | 0.960000, 16.240427, 0.000025, 104 | 0.970000, 17.013468, 0.000025, 105 | 0.980000, 17.244678, 0.000025, 106 | 0.990000, 17.114986, 0.000025, 107 | 1.000000, 16.522430, 0.000025, 108 | 1.010000, 15.188584, 0.000025, 109 | 1.020000, 12.665708, 0.000025, 110 | 1.030000, 8.499165, 0.000025, 111 | 1.040000, 2.370946, 0.000025, 112 | 1.050000, -0.005761, 0.000025, 113 | 1.060000, -0.009815, 0.000025, 114 | 1.070000, 0.014311, 0.000025, 115 | 1.080000, 0.001960, 0.000025, 116 | 1.090000, 0.010292, 0.000025, 117 | 1.100000, 0.002105, 0.000025, 118 | 1.110000, -0.007271, 0.000025, 119 | 1.120000, 0.005029, 0.000025, 120 | 1.130000, -0.002051, 0.000025, 121 | 1.140000, 0.006131, 0.000025, 122 | 1.150000, 0.005201, 0.000025, 123 | 1.160000, 0.003388, 0.000025, 124 | 1.170000, 0.001812, 0.000025, 125 | 1.180000, -0.000969, 0.000025, 126 | 1.190000, 0.009232, 0.000025, 127 | 1.200000, -0.000612, 0.000025, 128 | 1.210000, -0.006059, 0.000025, 129 | 1.220000, -0.006268, 0.000025, 130 | 1.230000, 0.000776, 0.000025, 131 | 1.240000, -0.004048, 0.000025, 132 | 1.250000, -0.008005, 0.000025, 133 | 1.260000, -0.001636, 0.000025, 134 | 1.270000, 0.003254, 0.000025, 135 | 1.280000, 0.004873, 0.000025, 136 | 1.290000, -0.000346, 0.000025, 137 | 1.300000, -0.003598, 0.000025, 138 | 1.310000, -0.002433, 0.000025, 139 | 1.320000, -0.007361, 0.000025, 140 | 1.330000, 0.001151, 0.000025, 141 | 1.340000, -0.003805, 0.000025, 142 | 1.350000, -0.003134, 0.000025, 143 | 1.360000, -0.002325, 0.000025, 144 | 1.370000, -0.001317, 0.000025, 145 | 1.380000, 0.001909, 0.000025, 146 | 1.390000, 0.198304, 0.000025, 147 | 1.400000, 5.177392, 0.000025, 148 | 1.410000, 10.514116, 0.000025, 149 | 1.420000, 13.928693, 0.000025, 150 | 1.430000, 15.878222, 0.000025, 151 | 1.440000, 16.848199, 0.000025, 152 | 1.450000, 17.209666, 0.000025, 153 | 1.460000, 17.191207, 0.000025, 154 | 1.470000, 16.760755, 0.000025, 155 | 1.480000, 15.675477, 0.000025, 156 | 1.490000, 13.537596, 0.000025, 157 | 1.500000, 9.885730, 0.000025, 158 | 1.510000, 4.217548, 0.000025, 159 | 1.520000, 0.044477, 0.000025, 160 | 1.530000, 0.005421, 0.000025, 161 | 1.540000, 0.003586, 0.000025, 162 | 1.550000, -0.001836, 0.000025, 163 | 1.560000, 0.007278, 0.000025, 164 | 1.570000, 0.003441, 0.000025, 165 | 1.580000, 0.005859, 0.000025, 166 | 1.590000, -0.003690, 0.000025, 167 | 1.600000, 0.006109, 0.000025, 168 | 1.610000, -0.000914, 0.000025, 169 | 1.620000, -0.003527, 0.000025, 170 | 1.630000, -0.005000, 0.000025, 171 | 1.640000, -0.003562, 0.000025, 172 | 1.650000, -0.007989, 0.000025, 173 | 1.660000, -0.002988, 0.000025, 174 | 1.670000, -0.004747, 0.000025, 175 | 1.680000, 0.004537, 0.000025, 176 | 1.690000, 0.007671, 0.000025, 177 | 1.700000, -0.002595, 0.000025, 178 | 1.710000, -0.000637, 0.000025, 179 | 1.720000, -0.004615, 0.000025, 180 | 1.730000, 0.005238, 0.000025, 181 | 1.740000, -0.006779, 0.000025, 182 | 1.750000, 0.006914, 0.000025, 183 | 1.760000, 0.003283, 0.000025, 184 | 1.770000, -0.009492, 0.000025, 185 | 1.780000, -0.002695, 0.000025, 186 | 1.790000, -0.001987, 0.000025, 187 | 1.800000, 0.002677, 0.000025, 188 | 1.810000, 0.006248, 0.000025, 189 | 1.820000, -0.000290, 0.000025, 190 | 1.830000, -0.002812, 0.000025, 191 | 1.840000, -0.012917, 0.000025, 192 | 1.850000, -0.003009, 0.000025, 193 | 1.860000, 0.005630, 0.000025, 194 | 1.870000, -0.003638, 0.000025, 195 | 1.880000, 0.002991, 0.000025, 196 | 1.890000, 0.001951, 0.000025, 197 | 1.900000, -0.007634, 0.000025, 198 | 1.910000, -0.001144, 0.000025, 199 | 1.920000, 0.004266, 0.000025, 200 | 1.930000, 0.002461, 0.000025, 201 | 1.940000, 0.003243, 0.000025, 202 | 1.950000, 0.000304, 0.000025, 203 | 1.960000, -0.004614, 0.000025, 204 | 1.970000, 0.002414, 0.000025, 205 | 1.980000, 0.000978, 0.000025, 206 | 1.990000, -0.001361, 0.000025, 207 | 2.000000, -0.007649, 0.000025, 208 | 2.010000, 0.000171, 0.000025, 209 | 2.020000, -0.004224, 0.000025, 210 | 2.030000, 0.008145, 0.000025, 211 | 2.040000, 0.002338, 0.000025, 212 | 2.050000, 0.009092, 0.000025, 213 | 2.060000, 0.000583, 0.000025, 214 | 2.070000, -0.001372, 0.000025, 215 | 2.080000, -0.002881, 0.000025, 216 | 2.090000, -0.002828, 0.000025, 217 | 2.100000, -0.001929, 0.000025, 218 | 2.110000, 0.007044, 0.000025, 219 | 2.120000, -0.005716, 0.000025, 220 | 2.130000, 0.003656, 0.000025, 221 | 2.140000, -0.000494, 0.000025, 222 | 2.150000, 0.007515, 0.000025, 223 | 2.160000, -0.006769, 0.000025, 224 | 2.170000, 0.001602, 0.000025, 225 | 2.180000, -0.005077, 0.000025, 226 | 2.190000, -0.002601, 0.000025, 227 | 2.200000, -0.000109, 0.000025, 228 | 2.210000, -0.002430, 0.000025, 229 | 2.220000, 0.001875, 0.000025, 230 | 2.230000, 0.000131, 0.000025, 231 | 2.240000, 0.001672, 0.000025, 232 | 2.250000, -0.007222, 0.000025, 233 | 2.260000, -0.001713, 0.000025, 234 | 2.270000, -0.002564, 0.000025, 235 | 2.280000, -0.000119, 0.000025, 236 | 2.290000, -0.000276, 0.000025, 237 | 2.300000, 0.004105, 0.000025, 238 | 2.310000, 0.000656, 0.000025, 239 | 2.320000, 0.006117, 0.000025, 240 | 2.330000, 0.005835, 0.000025, 241 | 2.340000, -0.007026, 0.000025, 242 | 2.350000, -0.005756, 0.000025, 243 | 2.360000, -0.004031, 0.000025, 244 | 2.370000, -3.252151, 0.000025, 245 | 2.380000, -9.214351, 0.000025, 246 | 2.390000, -13.114340, 0.000025, 247 | 2.400000, -15.440047, 0.000025, 248 | 2.410000, -16.654548, 0.000025, 249 | 2.420000, -17.162582, 0.000025, 250 | 2.430000, -17.229002, 0.000025, 251 | 2.440000, -16.937567, 0.000025, 252 | 2.450000, -16.069272, 0.000025, 253 | 2.460000, -14.285571, 0.000025, 254 | 2.470000, -11.110962, 0.000025, 255 | 2.480000, -6.090874, 0.000025, 256 | 2.490000, -0.520955, 0.000025, 257 | 2.500000, 0.000069, 0.000025, 258 | 2.510000, -0.009912, 0.000025, 259 | 2.520000, 0.015511, 0.000025, 260 | 2.530000, -0.001049, 0.000025, 261 | 2.540000, 0.011135, 0.000025, 262 | 2.550000, 0.003316, 0.000025, 263 | 2.560000, -0.000471, 0.000025, 264 | 2.570000, -0.007348, 0.000025, 265 | 2.580000, -0.002603, 0.000025, 266 | 2.590000, 0.001251, 0.000025, 267 | 2.600000, -0.010090, 0.000025, 268 | 2.610000, -0.002793, 0.000025, 269 | 2.620000, 0.002972, 0.000025, 270 | 2.630000, 0.003973, 0.000025, 271 | 2.640000, -0.000564, 0.000025, 272 | 2.650000, -0.006030, 0.000025, 273 | 2.660000, -0.006625, 0.000025, 274 | 2.670000, 0.000305, 0.000025, 275 | 2.680000, 0.000821, 0.000025, 276 | 2.690000, 0.008987, 0.000025, 277 | 2.700000, 0.002320, 0.000025, 278 | 2.710000, 0.000240, 0.000025, 279 | 2.720000, 0.000585, 0.000025, 280 | 2.730000, -0.001214, 0.000025, 281 | 2.740000, -0.003092, 0.000025, 282 | 2.750000, 0.004752, 0.000025, 283 | 2.760000, -0.000850, 0.000025, 284 | 2.770000, 0.001332, 0.000025, 285 | 2.780000, 0.001720, 0.000025, 286 | 2.790000, -0.004332, 0.000025, 287 | 2.800000, -0.005041, 0.000025, 288 | 2.810000, -0.002682, 0.000025, 289 | 2.820000, -0.010821, 0.000025, 290 | 2.830000, 0.003009, 0.000025, 291 | 2.840000, -1.609400, 0.000025, 292 | 2.850000, -7.728873, 0.000025, 293 | 2.860000, -12.190426, 0.000025, 294 | 2.870000, -14.904953, 0.000025, 295 | 2.880000, -16.401702, 0.000025, 296 | 2.890000, -17.070426, 0.000025, 297 | 2.900000, -17.253008, 0.000025, 298 | 2.910000, -17.076138, 0.000025, 299 | 2.920000, -16.392823, 0.000025, 300 | 2.930000, -14.913085, 0.000025, 301 | 2.940000, -12.186848, 0.000025, 302 | 2.950000, -7.749129, 0.000025, 303 | 2.960000, -1.616697, 0.000025, 304 | 2.970000, -0.002039, 0.000025, 305 | 2.980000, 0.007355, 0.000025, 306 | 2.990000, 0.006981, 0.000025, 307 | 3.000000, -0.000590, 0.000025, 308 | 3.010000, 0.000982, 0.000025, 309 | 3.020000, 0.005988, 0.000025, 310 | 3.030000, 0.006673, 0.000025, 311 | 3.040000, 0.000565, 0.000025, 312 | 3.050000, 0.011713, 0.000025, 313 | 3.060000, 0.004223, 0.000025, 314 | 3.070000, 0.006566, 0.000025, 315 | 3.080000, -0.002257, 0.000025, 316 | 3.090000, -0.007112, 0.000025, 317 | 3.100000, 0.005452, 0.000025, 318 | 3.110000, 0.003115, 0.000025, 319 | 3.120000, 0.003631, 0.000025, 320 | 3.130000, -0.004060, 0.000025, 321 | 3.140000, -0.003005, 0.000025, 322 | 3.150000, 0.005561, 0.000025, 323 | 3.160000, -0.002000, 0.000025, 324 | 3.170000, 0.007775, 0.000025, 325 | 3.180000, -0.003870, 0.000025, 326 | 3.190000, 0.009787, 0.000025, 327 | 3.200000, -0.008620, 0.000025, 328 | 3.210000, -0.012137, 0.000025, 329 | 3.220000, 0.001100, 0.000025, 330 | 3.230000, 0.009168, 0.000025, 331 | 3.240000, 0.005190, 0.000025, 332 | 3.250000, 0.005631, 0.000025, 333 | 3.260000, -0.001058, 0.000025, 334 | 3.270000, 0.001027, 0.000025, 335 | 3.280000, 0.001509, 0.000025, 336 | 3.290000, -0.006124, 0.000025, 337 | 3.300000, 0.008156, 0.000025, 338 | 3.310000, -0.517495, 0.000025, 339 | 3.320000, -6.071189, 0.000025, 340 | 3.330000, -11.114302, 0.000025, 341 | 3.340000, -14.270903, 0.000025, 342 | 3.350000, -16.063751, 0.000025, 343 | 3.360000, -16.931977, 0.000025, 344 | 3.370000, -17.239107, 0.000025, 345 | 3.380000, -17.167147, 0.000025, 346 | 3.390000, -16.653330, 0.000025, 347 | 3.400000, -15.438300, 0.000025, 348 | 3.410000, -13.119680, 0.000025, 349 | 3.420000, -9.221127, 0.000025, 350 | 3.430000, -3.250480, 0.000025, 351 | 3.440000, -0.006210, 0.000025, 352 | 3.450000, 0.008021, 0.000025, 353 | 3.460000, 0.003873, 0.000025, 354 | 3.470000, 0.001342, 0.000025, 355 | 3.480000, 0.000625, 0.000025, 356 | 3.490000, 0.002090, 0.000025, 357 | 3.500000, -0.002036, 0.000025, 358 | 3.510000, 0.002682, 0.000025, 359 | 3.520000, 0.002769, 0.000025, 360 | 3.530000, 0.004631, 0.000025, 361 | 3.540000, -0.005308, 0.000025, 362 | 3.550000, 0.001241, 0.000025, 363 | 3.560000, -0.000606, 0.000025, 364 | 3.570000, 0.009657, 0.000025, 365 | 3.580000, -0.008769, 0.000025, 366 | 3.590000, 0.003335, 0.000025, 367 | 3.600000, -0.002821, 0.000025, 368 | 3.610000, -0.005213, 0.000025, 369 | 3.620000, 0.006609, 0.000025, 370 | 3.630000, 0.004801, 0.000025, 371 | 3.640000, 0.004461, 0.000025, 372 | 3.650000, 0.011066, 0.000025, 373 | 3.660000, 0.003450, 0.000025, 374 | 3.670000, -0.005984, 0.000025, 375 | 3.680000, -0.000059, 0.000025, 376 | 3.690000, -0.005064, 0.000025, 377 | 3.700000, 0.001708, 0.000025, 378 | 3.710000, -0.000404, 0.000025, 379 | 3.720000, 0.002823, 0.000025, 380 | 3.730000, 0.003985, 0.000025, 381 | 3.740000, 0.008157, 0.000025, 382 | 3.750000, -0.002702, 0.000025, 383 | 3.760000, 0.003406, 0.000025, 384 | 3.770000, -0.005552, 0.000025, 385 | 3.780000, 0.005312, 0.000025, 386 | 3.790000, 0.004906, 0.000025, 387 | 3.800000, -0.003730, 0.000025, 388 | 3.810000, 0.000066, 0.000025, 389 | 3.820000, 0.009520, 0.000025, 390 | 3.830000, -0.007148, 0.000025, 391 | 3.840000, 0.001632, 0.000025, 392 | 3.850000, -0.004918, 0.000025, 393 | 3.860000, -0.004281, 0.000025, 394 | 3.870000, 0.004351, 0.000025, 395 | 3.880000, 0.002721, 0.000025, 396 | 3.890000, 0.005229, 0.000025, 397 | 3.900000, 0.007035, 0.000025, 398 | 3.910000, -0.006362, 0.000025, 399 | 3.920000, -0.003905, 0.000025, 400 | 3.930000, -0.004774, 0.000025, 401 | 3.940000, 0.000871, 0.000025, 402 | 3.950000, 0.007560, 0.000025, 403 | 3.960000, 0.000656, 0.000025, 404 | 3.970000, -0.001737, 0.000025, 405 | 3.980000, 0.000748, 0.000025, 406 | 3.990000, -0.006698, 0.000025, 407 | 4.000000, -0.003928, 0.000025, 408 | 4.010000, 0.000363, 0.000025, 409 | 4.020000, -0.005442, 0.000025, 410 | 4.030000, -0.006117, 0.000025, 411 | 4.040000, 0.011103, 0.000025, 412 | 4.050000, 0.003243, 0.000025, 413 | 4.060000, -0.004649, 0.000025, 414 | 4.070000, -0.004611, 0.000025, 415 | 4.080000, -0.001908, 0.000025, 416 | 4.090000, -0.004170, 0.000025, 417 | 4.100000, -0.005832, 0.000025, 418 | 4.110000, 0.008310, 0.000025, 419 | 4.120000, 0.003291, 0.000025, 420 | 4.130000, 0.000791, 0.000025, 421 | 4.140000, 0.001209, 0.000025, 422 | 4.150000, -0.004832, 0.000025, 423 | 4.160000, -0.003271, 0.000025, 424 | 4.170000, 0.000813, 0.000025, 425 | 4.180000, -0.004420, 0.000025, 426 | 4.190000, 0.005380, 0.000025, 427 | 4.200000, -0.007077, 0.000025, 428 | 4.210000, 0.003369, 0.000025, 429 | 4.220000, -0.002871, 0.000025, 430 | 4.230000, 0.006359, 0.000025, 431 | 4.240000, 0.002453, 0.000025, 432 | 4.250000, -0.003765, 0.000025, 433 | 4.260000, -0.000496, 0.000025, 434 | 4.270000, 0.000601, 0.000025, 435 | 4.280000, 0.004207, 0.000025, 436 | 4.290000, -0.003483, 0.000025, 437 | 4.300000, 0.003125, 0.000025, 438 | 4.310000, -0.001675, 0.000025, 439 | 4.320000, 0.003749, 0.000025, 440 | 4.330000, 0.004837, 0.000025, 441 | 4.340000, 0.009623, 0.000025, 442 | 4.350000, -0.004357, 0.000025, 443 | 4.360000, -0.002972, 0.000025, 444 | 4.370000, 0.009857, 0.000025, 445 | 4.380000, -0.003673, 0.000025, 446 | 4.390000, 0.000051, 0.000025, 447 | 4.400000, -0.001020, 0.000025, 448 | 4.410000, 0.005575, 0.000025, 449 | 4.420000, 0.002465, 0.000025, 450 | 4.430000, -0.012465, 0.000025, 451 | 4.440000, 0.006514, 0.000025, 452 | 4.450000, -0.007150, 0.000025, 453 | 4.460000, 0.000725, 0.000025, 454 | 4.470000, 0.001718, 0.000025, 455 | 4.480000, 0.002529, 0.000025, 456 | 4.490000, 0.007332, 0.000025, 457 | 4.500000, -0.008597, 0.000025, 458 | 4.510000, 0.003050, 0.000025, 459 | 4.520000, 0.003488, 0.000025, 460 | 4.530000, 0.008678, 0.000025, 461 | 4.540000, -0.002601, 0.000025, 462 | 4.550000, -0.004250, 0.000025, 463 | 4.560000, -0.003157, 0.000025, 464 | -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/meas_prior.txt: -------------------------------------------------------------------------------- 1 | mean_size : 3, 3 2 | dof : 3 3 | num_meas : 1 4 | ================================================================================================================================================================================================================ 5 | Time X_11 X_21 X_31 X_12 X_22 X_32 X_13 X_23 X_33 cov_11 cov_12 cov_13 cov_21 cov_22 cov_23 cov_31 cov_32 cov_33 6 | ================================================================================================================================================================================================================ 7 | 0.000000, 0.999993, 0.003750, 0.000000, -0.003750, 0.999993, 0.000000, 0.003445, 0.002557, 1.000000, 0.000010, 0.000000, 0.000000, 0.000000, 0.000010, 0.000000, 0.000000, 0.000000, 0.000010, 8 | -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/meas_vel.txt: -------------------------------------------------------------------------------- 1 | mean_size : 2, 1 2 | dof : 2 3 | num_meas : 457 4 | ================================================================================================================ 5 | Time v_btt_b_11 v_btt_b_12 cov_11 cov_12 cov_21 cov_22 6 | ================================================================================================================ 7 | 0.000000, 0.979232, -0.006906, 0.000100, 0.000000, 0.000000, 0.000100, 8 | 0.010000, 1.008645, 0.003545, 0.000100, 0.000000, 0.000000, 0.000100, 9 | 0.020000, 0.992792, 0.005436, 0.000100, 0.000000, 0.000000, 0.000100, 10 | 0.030000, 1.001875, -0.005665, 0.000100, 0.000000, 0.000000, 0.000100, 11 | 0.040000, 1.003338, -0.004347, 0.000100, 0.000000, 0.000000, 0.000100, 12 | 0.050000, 1.003534, 0.008773, 0.000100, 0.000000, 0.000000, 0.000100, 13 | 0.060000, 0.998964, -0.007926, 0.000100, 0.000000, 0.000000, 0.000100, 14 | 0.070000, 1.020635, -0.006571, 0.000100, 0.000000, 0.000000, 0.000100, 15 | 0.080000, 1.005744, 0.001772, 0.000100, 0.000000, 0.000000, 0.000100, 16 | 0.090000, 1.014709, -0.004501, 0.000100, 0.000000, 0.000000, 0.000100, 17 | 0.100000, 1.000571, 0.013717, 0.000100, 0.000000, 0.000000, 0.000100, 18 | 0.110000, 0.996703, -0.009373, 0.000100, 0.000000, 0.000000, 0.000100, 19 | 0.120000, 1.000878, 0.002736, 0.000100, 0.000000, 0.000000, 0.000100, 20 | 0.130000, 1.006227, 0.016169, 0.000100, 0.000000, 0.000000, 0.000100, 21 | 0.140000, 0.996621, -0.012235, 0.000100, 0.000000, 0.000000, 0.000100, 22 | 0.150000, 0.999357, 0.008324, 0.000100, 0.000000, 0.000000, 0.000100, 23 | 0.160000, 0.986605, -0.003989, 0.000100, 0.000000, 0.000000, 0.000100, 24 | 0.170000, 0.981582, 0.012700, 0.000100, 0.000000, 0.000000, 0.000100, 25 | 0.180000, 1.003724, 0.014926, 0.000100, 0.000000, 0.000000, 0.000100, 26 | 0.190000, 1.011251, -0.000379, 0.000100, 0.000000, 0.000000, 0.000100, 27 | 0.200000, 0.981394, 0.000727, 0.000100, 0.000000, 0.000000, 0.000100, 28 | 0.210000, 0.996680, 0.004046, 0.000100, 0.000000, 0.000000, 0.000100, 29 | 0.220000, 1.016153, 0.036179, 0.000100, 0.000000, 0.000000, 0.000100, 30 | 0.230000, 1.022904, 0.013977, 0.000100, 0.000000, 0.000000, 0.000100, 31 | 0.240000, 0.992558, -0.012515, 0.000100, 0.000000, 0.000000, 0.000100, 32 | 0.250000, 0.990480, 0.005305, 0.000100, 0.000000, 0.000000, 0.000100, 33 | 0.260000, 0.993544, 0.014318, 0.000100, 0.000000, 0.000000, 0.000100, 34 | 0.270000, 0.993507, 0.006760, 0.000100, 0.000000, 0.000000, 0.000100, 35 | 0.280000, 0.994120, 0.004285, 0.000100, 0.000000, 0.000000, 0.000100, 36 | 0.290000, 1.004507, -0.004432, 0.000100, 0.000000, 0.000000, 0.000100, 37 | 0.300000, 0.988196, -0.008388, 0.000100, 0.000000, 0.000000, 0.000100, 38 | 0.310000, 1.011802, 0.003376, 0.000100, 0.000000, 0.000000, 0.000100, 39 | 0.320000, 0.991572, 0.009684, 0.000100, 0.000000, 0.000000, 0.000100, 40 | 0.330000, 0.990351, -0.006459, 0.000100, 0.000000, 0.000000, 0.000100, 41 | 0.340000, 1.005067, -0.016314, 0.000100, 0.000000, 0.000000, 0.000100, 42 | 0.350000, 0.989204, -0.001293, 0.000100, 0.000000, 0.000000, 0.000100, 43 | 0.360000, 1.004798, -0.010843, 0.000100, 0.000000, 0.000000, 0.000100, 44 | 0.370000, 0.982273, 0.002314, 0.000100, 0.000000, 0.000000, 0.000100, 45 | 0.380000, 0.998253, -0.006884, 0.000100, 0.000000, 0.000000, 0.000100, 46 | 0.390000, 1.003953, 0.001658, 0.000100, 0.000000, 0.000000, 0.000100, 47 | 0.400000, 1.009135, -0.002604, 0.000100, 0.000000, 0.000000, 0.000100, 48 | 0.410000, 0.979954, 0.016820, 0.000100, 0.000000, 0.000000, 0.000100, 49 | 0.420000, 0.983873, -0.000180, 0.000100, 0.000000, 0.000000, 0.000100, 50 | 0.430000, 0.997213, 0.004727, 0.000100, 0.000000, 0.000000, 0.000100, 51 | 0.440000, 0.995297, -0.005247, 0.000100, 0.000000, 0.000000, 0.000100, 52 | 0.450000, 1.002748, 0.018359, 0.000100, 0.000000, 0.000000, 0.000100, 53 | 0.460000, 0.984814, -0.009049, 0.000100, 0.000000, 0.000000, 0.000100, 54 | 0.470000, 0.999319, 0.013031, 0.000100, 0.000000, 0.000000, 0.000100, 55 | 0.480000, 1.009537, -0.011096, 0.000100, 0.000000, 0.000000, 0.000100, 56 | 0.490000, 1.005725, 0.006028, 0.000100, 0.000000, 0.000000, 0.000100, 57 | 0.500000, 0.994004, -0.015604, 0.000100, 0.000000, 0.000000, 0.000100, 58 | 0.510000, 1.016572, 0.005738, 0.000100, 0.000000, 0.000000, 0.000100, 59 | 0.520000, 1.000428, -0.005951, 0.000100, 0.000000, 0.000000, 0.000100, 60 | 0.530000, 0.993295, 0.003173, 0.000100, 0.000000, 0.000000, 0.000100, 61 | 0.540000, 0.988330, 0.007364, 0.000100, 0.000000, 0.000000, 0.000100, 62 | 0.550000, 0.995216, 0.011612, 0.000100, 0.000000, 0.000000, 0.000100, 63 | 0.560000, 1.003020, -0.016710, 0.000100, 0.000000, 0.000000, 0.000100, 64 | 0.570000, 1.016437, -0.011204, 0.000100, 0.000000, 0.000000, 0.000100, 65 | 0.580000, 0.990185, -0.002351, 0.000100, 0.000000, 0.000000, 0.000100, 66 | 0.590000, 0.989496, 0.003046, 0.000100, 0.000000, 0.000000, 0.000100, 67 | 0.600000, 0.985801, 0.008125, 0.000100, 0.000000, 0.000000, 0.000100, 68 | 0.610000, 1.005954, -0.007042, 0.000100, 0.000000, 0.000000, 0.000100, 69 | 0.620000, 1.008875, 0.015169, 0.000100, 0.000000, 0.000000, 0.000100, 70 | 0.630000, 0.991348, 0.000263, 0.000100, 0.000000, 0.000000, 0.000100, 71 | 0.640000, 0.985218, 0.009509, 0.000100, 0.000000, 0.000000, 0.000100, 72 | 0.650000, 1.018430, 0.019200, 0.000100, 0.000000, 0.000000, 0.000100, 73 | 0.660000, 1.005663, -0.004781, 0.000100, 0.000000, 0.000000, 0.000100, 74 | 0.670000, 0.972859, -0.002543, 0.000100, 0.000000, 0.000000, 0.000100, 75 | 0.680000, 1.007511, -0.003645, 0.000100, 0.000000, 0.000000, 0.000100, 76 | 0.690000, 0.991848, 0.012493, 0.000100, 0.000000, 0.000000, 0.000100, 77 | 0.700000, 0.991184, -0.013253, 0.000100, 0.000000, 0.000000, 0.000100, 78 | 0.710000, 0.994234, 0.012441, 0.000100, 0.000000, 0.000000, 0.000100, 79 | 0.720000, 1.009514, -0.009349, 0.000100, 0.000000, 0.000000, 0.000100, 80 | 0.730000, 0.984730, -0.014868, 0.000100, 0.000000, 0.000000, 0.000100, 81 | 0.740000, 1.019657, 0.001277, 0.000100, 0.000000, 0.000000, 0.000100, 82 | 0.750000, 0.988257, 0.008664, 0.000100, 0.000000, 0.000000, 0.000100, 83 | 0.760000, 1.005206, -0.011892, 0.000100, 0.000000, 0.000000, 0.000100, 84 | 0.770000, 1.000489, -0.005112, 0.000100, 0.000000, 0.000000, 0.000100, 85 | 0.780000, 1.004789, 0.028147, 0.000100, 0.000000, 0.000000, 0.000100, 86 | 0.790000, 1.008334, 0.008871, 0.000100, 0.000000, 0.000000, 0.000100, 87 | 0.800000, 1.000613, -0.005892, 0.000100, 0.000000, 0.000000, 0.000100, 88 | 0.810000, 0.990726, -0.018498, 0.000100, 0.000000, 0.000000, 0.000100, 89 | 0.820000, 0.998408, -0.018368, 0.000100, 0.000000, 0.000000, 0.000100, 90 | 0.830000, 1.004433, 0.008386, 0.000100, 0.000000, 0.000000, 0.000100, 91 | 0.840000, 0.993012, -0.003520, 0.000100, 0.000000, 0.000000, 0.000100, 92 | 0.850000, 1.000834, 0.009567, 0.000100, 0.000000, 0.000000, 0.000100, 93 | 0.860000, 0.990211, 0.002390, 0.000100, 0.000000, 0.000000, 0.000100, 94 | 0.870000, 0.996038, -0.023175, 0.000100, 0.000000, 0.000000, 0.000100, 95 | 0.880000, 0.981598, -0.018327, 0.000100, 0.000000, 0.000000, 0.000100, 96 | 0.890000, 1.000982, 0.011210, 0.000100, 0.000000, 0.000000, 0.000100, 97 | 0.900000, 0.989882, -0.010030, 0.000100, 0.000000, 0.000000, 0.000100, 98 | 0.910000, 1.004593, 0.008996, 0.000100, 0.000000, 0.000000, 0.000100, 99 | 0.920000, 0.996857, 0.002301, 0.000100, 0.000000, 0.000000, 0.000100, 100 | 0.930000, 1.000537, -0.009995, 0.000100, 0.000000, 0.000000, 0.000100, 101 | 0.940000, 0.997894, -0.000547, 0.000100, 0.000000, 0.000000, 0.000100, 102 | 0.950000, 1.011316, 0.005003, 0.000100, 0.000000, 0.000000, 0.000100, 103 | 0.960000, 0.996389, -0.010402, 0.000100, 0.000000, 0.000000, 0.000100, 104 | 0.970000, 1.003086, -0.014682, 0.000100, 0.000000, 0.000000, 0.000100, 105 | 0.980000, 0.980480, 0.007555, 0.000100, 0.000000, 0.000000, 0.000100, 106 | 0.990000, 0.989636, 0.005298, 0.000100, 0.000000, 0.000000, 0.000100, 107 | 1.000000, 1.003592, 0.009460, 0.000100, 0.000000, 0.000000, 0.000100, 108 | 1.010000, 0.994519, 0.006018, 0.000100, 0.000000, 0.000000, 0.000100, 109 | 1.020000, 1.001684, 0.022250, 0.000100, 0.000000, 0.000000, 0.000100, 110 | 1.030000, 1.008729, -0.004962, 0.000100, 0.000000, 0.000000, 0.000100, 111 | 1.040000, 0.994018, 0.002281, 0.000100, 0.000000, 0.000000, 0.000100, 112 | 1.050000, 1.007582, -0.002035, 0.000100, 0.000000, 0.000000, 0.000100, 113 | 1.060000, 0.992340, 0.016539, 0.000100, 0.000000, 0.000000, 0.000100, 114 | 1.070000, 0.999792, -0.018061, 0.000100, 0.000000, 0.000000, 0.000100, 115 | 1.080000, 0.978172, 0.003475, 0.000100, 0.000000, 0.000000, 0.000100, 116 | 1.090000, 0.996312, 0.010798, 0.000100, 0.000000, 0.000000, 0.000100, 117 | 1.100000, 1.001721, 0.002484, 0.000100, 0.000000, 0.000000, 0.000100, 118 | 1.110000, 1.009673, -0.006833, 0.000100, 0.000000, 0.000000, 0.000100, 119 | 1.120000, 1.000793, 0.003761, 0.000100, 0.000000, 0.000000, 0.000100, 120 | 1.130000, 1.007640, 0.018938, 0.000100, 0.000000, 0.000000, 0.000100, 121 | 1.140000, 1.004555, -0.006557, 0.000100, 0.000000, 0.000000, 0.000100, 122 | 1.150000, 0.997452, -0.007383, 0.000100, 0.000000, 0.000000, 0.000100, 123 | 1.160000, 1.007999, 0.000628, 0.000100, 0.000000, 0.000000, 0.000100, 124 | 1.170000, 0.982408, 0.005295, 0.000100, 0.000000, 0.000000, 0.000100, 125 | 1.180000, 1.004117, 0.014173, 0.000100, 0.000000, 0.000000, 0.000100, 126 | 1.190000, 0.999692, -0.001243, 0.000100, 0.000000, 0.000000, 0.000100, 127 | 1.200000, 1.004930, 0.009785, 0.000100, 0.000000, 0.000000, 0.000100, 128 | 1.210000, 0.994660, -0.016725, 0.000100, 0.000000, 0.000000, 0.000100, 129 | 1.220000, 1.008609, -0.015594, 0.000100, 0.000000, 0.000000, 0.000100, 130 | 1.230000, 1.001375, 0.006315, 0.000100, 0.000000, 0.000000, 0.000100, 131 | 1.240000, 1.001242, 0.010713, 0.000100, 0.000000, 0.000000, 0.000100, 132 | 1.250000, 0.997615, -0.003631, 0.000100, 0.000000, 0.000000, 0.000100, 133 | 1.260000, 0.997705, -0.007536, 0.000100, 0.000000, 0.000000, 0.000100, 134 | 1.270000, 1.010789, -0.000139, 0.000100, 0.000000, 0.000000, 0.000100, 135 | 1.280000, 1.004159, 0.000358, 0.000100, 0.000000, 0.000000, 0.000100, 136 | 1.290000, 1.019685, 0.007787, 0.000100, 0.000000, 0.000000, 0.000100, 137 | 1.300000, 0.999893, 0.018057, 0.000100, 0.000000, 0.000000, 0.000100, 138 | 1.310000, 0.999946, 0.003772, 0.000100, 0.000000, 0.000000, 0.000100, 139 | 1.320000, 0.997241, 0.001839, 0.000100, 0.000000, 0.000000, 0.000100, 140 | 1.330000, 0.990446, 0.013800, 0.000100, 0.000000, 0.000000, 0.000100, 141 | 1.340000, 1.009228, 0.008212, 0.000100, 0.000000, 0.000000, 0.000100, 142 | 1.350000, 1.006852, 0.006331, 0.000100, 0.000000, 0.000000, 0.000100, 143 | 1.360000, 1.005413, -0.006417, 0.000100, 0.000000, 0.000000, 0.000100, 144 | 1.370000, 0.991371, -0.022421, 0.000100, 0.000000, 0.000000, 0.000100, 145 | 1.380000, 1.005981, -0.001753, 0.000100, 0.000000, 0.000000, 0.000100, 146 | 1.390000, 0.995359, 0.005787, 0.000100, 0.000000, 0.000000, 0.000100, 147 | 1.400000, 1.033321, -0.017004, 0.000100, 0.000000, 0.000000, 0.000100, 148 | 1.410000, 0.982180, -0.006631, 0.000100, 0.000000, 0.000000, 0.000100, 149 | 1.420000, 0.996291, -0.014053, 0.000100, 0.000000, 0.000000, 0.000100, 150 | 1.430000, 0.978337, -0.015224, 0.000100, 0.000000, 0.000000, 0.000100, 151 | 1.440000, 1.010117, -0.011928, 0.000100, 0.000000, 0.000000, 0.000100, 152 | 1.450000, 0.992135, -0.013259, 0.000100, 0.000000, 0.000000, 0.000100, 153 | 1.460000, 1.012699, -0.010721, 0.000100, 0.000000, 0.000000, 0.000100, 154 | 1.470000, 0.994626, -0.023113, 0.000100, 0.000000, 0.000000, 0.000100, 155 | 1.480000, 1.002394, -0.004952, 0.000100, 0.000000, 0.000000, 0.000100, 156 | 1.490000, 0.997850, 0.028563, 0.000100, 0.000000, 0.000000, 0.000100, 157 | 1.500000, 0.989771, 0.016563, 0.000100, 0.000000, 0.000000, 0.000100, 158 | 1.510000, 0.999271, 0.016753, 0.000100, 0.000000, 0.000000, 0.000100, 159 | 1.520000, 0.992518, 0.023986, 0.000100, 0.000000, 0.000000, 0.000100, 160 | 1.530000, 0.984722, 0.011173, 0.000100, 0.000000, 0.000000, 0.000100, 161 | 1.540000, 1.007316, -0.004839, 0.000100, 0.000000, 0.000000, 0.000100, 162 | 1.550000, 1.005093, 0.020919, 0.000100, 0.000000, 0.000000, 0.000100, 163 | 1.560000, 1.000190, 0.011504, 0.000100, 0.000000, 0.000000, 0.000100, 164 | 1.570000, 1.008073, -0.012674, 0.000100, 0.000000, 0.000000, 0.000100, 165 | 1.580000, 0.989147, 0.005161, 0.000100, 0.000000, 0.000000, 0.000100, 166 | 1.590000, 0.986792, -0.005819, 0.000100, 0.000000, 0.000000, 0.000100, 167 | 1.600000, 0.999954, -0.011476, 0.000100, 0.000000, 0.000000, 0.000100, 168 | 1.610000, 1.011322, 0.002633, 0.000100, 0.000000, 0.000000, 0.000100, 169 | 1.620000, 0.994399, 0.000438, 0.000100, 0.000000, 0.000000, 0.000100, 170 | 1.630000, 0.994755, 0.000835, 0.000100, 0.000000, 0.000000, 0.000100, 171 | 1.640000, 1.017259, -0.021677, 0.000100, 0.000000, 0.000000, 0.000100, 172 | 1.650000, 0.982068, -0.001688, 0.000100, 0.000000, 0.000000, 0.000100, 173 | 1.660000, 1.006279, -0.001414, 0.000100, 0.000000, 0.000000, 0.000100, 174 | 1.670000, 1.005408, -0.001786, 0.000100, 0.000000, 0.000000, 0.000100, 175 | 1.680000, 0.989058, -0.001777, 0.000100, 0.000000, 0.000000, 0.000100, 176 | 1.690000, 1.009472, 0.007489, 0.000100, 0.000000, 0.000000, 0.000100, 177 | 1.700000, 1.002231, -0.008385, 0.000100, 0.000000, 0.000000, 0.000100, 178 | 1.710000, 1.003276, -0.001850, 0.000100, 0.000000, 0.000000, 0.000100, 179 | 1.720000, 0.992885, -0.006153, 0.000100, 0.000000, 0.000000, 0.000100, 180 | 1.730000, 1.021834, 0.012950, 0.000100, 0.000000, 0.000000, 0.000100, 181 | 1.740000, 1.006188, 0.012966, 0.000100, 0.000000, 0.000000, 0.000100, 182 | 1.750000, 1.002440, -0.012057, 0.000100, 0.000000, 0.000000, 0.000100, 183 | 1.760000, 0.991551, -0.021693, 0.000100, 0.000000, 0.000000, 0.000100, 184 | 1.770000, 0.996673, 0.006955, 0.000100, 0.000000, 0.000000, 0.000100, 185 | 1.780000, 0.993705, -0.005258, 0.000100, 0.000000, 0.000000, 0.000100, 186 | 1.790000, 1.003233, -0.005707, 0.000100, 0.000000, 0.000000, 0.000100, 187 | 1.800000, 0.986960, -0.007178, 0.000100, 0.000000, 0.000000, 0.000100, 188 | 1.810000, 1.004738, -0.010253, 0.000100, 0.000000, 0.000000, 0.000100, 189 | 1.820000, 1.004402, 0.003122, 0.000100, 0.000000, 0.000000, 0.000100, 190 | 1.830000, 1.010439, 0.000939, 0.000100, 0.000000, 0.000000, 0.000100, 191 | 1.840000, 1.009919, -0.005386, 0.000100, 0.000000, 0.000000, 0.000100, 192 | 1.850000, 1.009939, 0.003857, 0.000100, 0.000000, 0.000000, 0.000100, 193 | 1.860000, 0.983697, 0.011144, 0.000100, 0.000000, 0.000000, 0.000100, 194 | 1.870000, 0.998628, -0.010417, 0.000100, 0.000000, 0.000000, 0.000100, 195 | 1.880000, 1.006123, 0.004590, 0.000100, 0.000000, 0.000000, 0.000100, 196 | 1.890000, 1.003305, -0.004848, 0.000100, 0.000000, 0.000000, 0.000100, 197 | 1.900000, 1.006608, -0.013376, 0.000100, 0.000000, 0.000000, 0.000100, 198 | 1.910000, 0.993707, 0.017587, 0.000100, 0.000000, 0.000000, 0.000100, 199 | 1.920000, 0.996316, 0.012654, 0.000100, 0.000000, 0.000000, 0.000100, 200 | 1.930000, 1.003034, -0.005519, 0.000100, 0.000000, 0.000000, 0.000100, 201 | 1.940000, 0.996851, -0.000269, 0.000100, 0.000000, 0.000000, 0.000100, 202 | 1.950000, 0.994940, 0.014757, 0.000100, 0.000000, 0.000000, 0.000100, 203 | 1.960000, 0.991052, -0.010725, 0.000100, 0.000000, 0.000000, 0.000100, 204 | 1.970000, 1.004513, 0.004324, 0.000100, 0.000000, 0.000000, 0.000100, 205 | 1.980000, 1.012956, -0.026282, 0.000100, 0.000000, 0.000000, 0.000100, 206 | 1.990000, 0.998230, 0.003613, 0.000100, 0.000000, 0.000000, 0.000100, 207 | 2.000000, 0.990741, -0.005753, 0.000100, 0.000000, 0.000000, 0.000100, 208 | 2.010000, 1.007053, 0.006266, 0.000100, 0.000000, 0.000000, 0.000100, 209 | 2.020000, 1.002833, -0.014824, 0.000100, 0.000000, 0.000000, 0.000100, 210 | 2.030000, 1.006346, 0.002386, 0.000100, 0.000000, 0.000000, 0.000100, 211 | 2.040000, 1.016794, 0.007006, 0.000100, 0.000000, 0.000000, 0.000100, 212 | 2.050000, 0.986916, 0.006371, 0.000100, 0.000000, 0.000000, 0.000100, 213 | 2.060000, 0.999082, -0.000564, 0.000100, 0.000000, 0.000000, 0.000100, 214 | 2.070000, 1.000411, -0.002560, 0.000100, 0.000000, 0.000000, 0.000100, 215 | 2.080000, 0.995801, -0.007995, 0.000100, 0.000000, 0.000000, 0.000100, 216 | 2.090000, 0.982693, 0.007385, 0.000100, 0.000000, 0.000000, 0.000100, 217 | 2.100000, 0.980467, 0.017532, 0.000100, 0.000000, 0.000000, 0.000100, 218 | 2.110000, 0.993647, -0.002195, 0.000100, 0.000000, 0.000000, 0.000100, 219 | 2.120000, 0.995632, 0.003570, 0.000100, 0.000000, 0.000000, 0.000100, 220 | 2.130000, 0.999802, -0.013995, 0.000100, 0.000000, 0.000000, 0.000100, 221 | 2.140000, 0.976760, -0.002393, 0.000100, 0.000000, 0.000000, 0.000100, 222 | 2.150000, 0.998565, 0.014682, 0.000100, 0.000000, 0.000000, 0.000100, 223 | 2.160000, 0.995732, 0.002736, 0.000100, 0.000000, 0.000000, 0.000100, 224 | 2.170000, 1.006171, -0.014190, 0.000100, 0.000000, 0.000000, 0.000100, 225 | 2.180000, 1.007358, -0.001389, 0.000100, 0.000000, 0.000000, 0.000100, 226 | 2.190000, 0.987906, -0.001234, 0.000100, 0.000000, 0.000000, 0.000100, 227 | 2.200000, 0.995079, 0.022386, 0.000100, 0.000000, 0.000000, 0.000100, 228 | 2.210000, 0.997028, 0.006775, 0.000100, 0.000000, 0.000000, 0.000100, 229 | 2.220000, 1.011692, 0.013875, 0.000100, 0.000000, 0.000000, 0.000100, 230 | 2.230000, 0.985149, -0.006689, 0.000100, 0.000000, 0.000000, 0.000100, 231 | 2.240000, 0.983052, -0.000764, 0.000100, 0.000000, 0.000000, 0.000100, 232 | 2.250000, 1.008565, 0.002984, 0.000100, 0.000000, 0.000000, 0.000100, 233 | 2.260000, 0.989182, 0.018276, 0.000100, 0.000000, 0.000000, 0.000100, 234 | 2.270000, 0.993907, 0.014294, 0.000100, 0.000000, 0.000000, 0.000100, 235 | 2.280000, 1.000076, 0.009909, 0.000100, 0.000000, 0.000000, 0.000100, 236 | 2.290000, 1.007889, 0.033042, 0.000100, 0.000000, 0.000000, 0.000100, 237 | 2.300000, 1.001373, -0.006693, 0.000100, 0.000000, 0.000000, 0.000100, 238 | 2.310000, 1.012739, -0.003180, 0.000100, 0.000000, 0.000000, 0.000100, 239 | 2.320000, 0.995956, -0.002600, 0.000100, 0.000000, 0.000000, 0.000100, 240 | 2.330000, 0.997842, 0.003759, 0.000100, 0.000000, 0.000000, 0.000100, 241 | 2.340000, 1.008648, -0.011088, 0.000100, 0.000000, 0.000000, 0.000100, 242 | 2.350000, 1.007648, -0.009169, 0.000100, 0.000000, 0.000000, 0.000100, 243 | 2.360000, 0.991193, -0.031759, 0.000100, 0.000000, 0.000000, 0.000100, 244 | 2.370000, 1.001974, 0.009010, 0.000100, 0.000000, 0.000000, 0.000100, 245 | 2.380000, 1.001161, 0.000893, 0.000100, 0.000000, 0.000000, 0.000100, 246 | 2.390000, 0.999462, 0.007090, 0.000100, 0.000000, 0.000000, 0.000100, 247 | 2.400000, 1.006109, 0.003251, 0.000100, 0.000000, 0.000000, 0.000100, 248 | 2.410000, 0.998314, -0.014642, 0.000100, 0.000000, 0.000000, 0.000100, 249 | 2.420000, 1.005816, -0.013583, 0.000100, 0.000000, 0.000000, 0.000100, 250 | 2.430000, 0.994337, -0.010463, 0.000100, 0.000000, 0.000000, 0.000100, 251 | 2.440000, 1.004940, -0.012936, 0.000100, 0.000000, 0.000000, 0.000100, 252 | 2.450000, 0.996401, 0.004599, 0.000100, 0.000000, 0.000000, 0.000100, 253 | 2.460000, 1.008247, -0.006716, 0.000100, 0.000000, 0.000000, 0.000100, 254 | 2.470000, 1.000316, -0.006744, 0.000100, 0.000000, 0.000000, 0.000100, 255 | 2.480000, 0.987807, 0.007985, 0.000100, 0.000000, 0.000000, 0.000100, 256 | 2.490000, 1.004254, 0.008956, 0.000100, 0.000000, 0.000000, 0.000100, 257 | 2.500000, 0.993487, 0.010561, 0.000100, 0.000000, 0.000000, 0.000100, 258 | 2.510000, 0.988271, 0.003307, 0.000100, 0.000000, 0.000000, 0.000100, 259 | 2.520000, 1.003858, -0.017174, 0.000100, 0.000000, 0.000000, 0.000100, 260 | 2.530000, 1.007806, 0.016156, 0.000100, 0.000000, 0.000000, 0.000100, 261 | 2.540000, 1.012396, -0.000420, 0.000100, 0.000000, 0.000000, 0.000100, 262 | 2.550000, 1.002791, -0.003377, 0.000100, 0.000000, 0.000000, 0.000100, 263 | 2.560000, 0.990942, -0.006449, 0.000100, 0.000000, 0.000000, 0.000100, 264 | 2.570000, 1.003654, -0.002175, 0.000100, 0.000000, 0.000000, 0.000100, 265 | 2.580000, 1.021531, 0.004331, 0.000100, 0.000000, 0.000000, 0.000100, 266 | 2.590000, 1.004055, -0.002333, 0.000100, 0.000000, 0.000000, 0.000100, 267 | 2.600000, 0.999253, -0.009288, 0.000100, 0.000000, 0.000000, 0.000100, 268 | 2.610000, 1.012726, -0.010319, 0.000100, 0.000000, 0.000000, 0.000100, 269 | 2.620000, 0.996140, -0.006550, 0.000100, 0.000000, 0.000000, 0.000100, 270 | 2.630000, 1.015945, 0.008865, 0.000100, 0.000000, 0.000000, 0.000100, 271 | 2.640000, 0.997462, -0.000326, 0.000100, 0.000000, 0.000000, 0.000100, 272 | 2.650000, 1.004033, -0.005029, 0.000100, 0.000000, 0.000000, 0.000100, 273 | 2.660000, 0.992268, 0.008514, 0.000100, 0.000000, 0.000000, 0.000100, 274 | 2.670000, 1.004793, 0.004534, 0.000100, 0.000000, 0.000000, 0.000100, 275 | 2.680000, 1.012561, -0.010914, 0.000100, 0.000000, 0.000000, 0.000100, 276 | 2.690000, 1.005901, 0.011596, 0.000100, 0.000000, 0.000000, 0.000100, 277 | 2.700000, 1.014622, 0.001618, 0.000100, 0.000000, 0.000000, 0.000100, 278 | 2.710000, 0.999068, 0.007413, 0.000100, 0.000000, 0.000000, 0.000100, 279 | 2.720000, 0.992340, -0.022983, 0.000100, 0.000000, 0.000000, 0.000100, 280 | 2.730000, 1.004238, -0.004100, 0.000100, 0.000000, 0.000000, 0.000100, 281 | 2.740000, 0.997172, -0.001773, 0.000100, 0.000000, 0.000000, 0.000100, 282 | 2.750000, 1.001730, 0.004861, 0.000100, 0.000000, 0.000000, 0.000100, 283 | 2.760000, 0.989716, -0.003257, 0.000100, 0.000000, 0.000000, 0.000100, 284 | 2.770000, 0.991022, 0.000844, 0.000100, 0.000000, 0.000000, 0.000100, 285 | 2.780000, 0.989302, -0.011181, 0.000100, 0.000000, 0.000000, 0.000100, 286 | 2.790000, 0.995452, 0.000117, 0.000100, 0.000000, 0.000000, 0.000100, 287 | 2.800000, 0.990484, -0.001706, 0.000100, 0.000000, 0.000000, 0.000100, 288 | 2.810000, 0.995238, 0.007411, 0.000100, 0.000000, 0.000000, 0.000100, 289 | 2.820000, 1.006618, 0.012439, 0.000100, 0.000000, 0.000000, 0.000100, 290 | 2.830000, 0.983905, -0.014246, 0.000100, 0.000000, 0.000000, 0.000100, 291 | 2.840000, 1.019635, -0.006324, 0.000100, 0.000000, 0.000000, 0.000100, 292 | 2.850000, 1.001247, 0.006276, 0.000100, 0.000000, 0.000000, 0.000100, 293 | 2.860000, 0.999119, -0.014892, 0.000100, 0.000000, 0.000000, 0.000100, 294 | 2.870000, 0.997869, -0.004918, 0.000100, 0.000000, 0.000000, 0.000100, 295 | 2.880000, 1.017605, 0.016390, 0.000100, 0.000000, 0.000000, 0.000100, 296 | 2.890000, 1.002268, -0.006058, 0.000100, 0.000000, 0.000000, 0.000100, 297 | 2.900000, 1.000863, 0.002399, 0.000100, 0.000000, 0.000000, 0.000100, 298 | 2.910000, 0.989026, 0.009316, 0.000100, 0.000000, 0.000000, 0.000100, 299 | 2.920000, 0.997162, -0.014195, 0.000100, 0.000000, 0.000000, 0.000100, 300 | 2.930000, 0.988109, 0.011176, 0.000100, 0.000000, 0.000000, 0.000100, 301 | 2.940000, 0.997767, 0.014476, 0.000100, 0.000000, 0.000000, 0.000100, 302 | 2.950000, 0.995351, 0.000846, 0.000100, 0.000000, 0.000000, 0.000100, 303 | 2.960000, 0.979418, 0.002701, 0.000100, 0.000000, 0.000000, 0.000100, 304 | 2.970000, 0.982821, -0.010546, 0.000100, 0.000000, 0.000000, 0.000100, 305 | 2.980000, 1.003594, 0.016493, 0.000100, 0.000000, 0.000000, 0.000100, 306 | 2.990000, 0.998420, -0.000019, 0.000100, 0.000000, 0.000000, 0.000100, 307 | 3.000000, 1.011992, 0.001631, 0.000100, 0.000000, 0.000000, 0.000100, 308 | 3.010000, 0.998997, -0.001502, 0.000100, 0.000000, 0.000000, 0.000100, 309 | 3.020000, 0.992898, 0.004560, 0.000100, 0.000000, 0.000000, 0.000100, 310 | 3.030000, 0.997484, 0.001176, 0.000100, 0.000000, 0.000000, 0.000100, 311 | 3.040000, 1.015562, -0.008721, 0.000100, 0.000000, 0.000000, 0.000100, 312 | 3.050000, 0.994449, 0.004377, 0.000100, 0.000000, 0.000000, 0.000100, 313 | 3.060000, 1.004181, 0.002509, 0.000100, 0.000000, 0.000000, 0.000100, 314 | 3.070000, 1.017823, -0.009412, 0.000100, 0.000000, 0.000000, 0.000100, 315 | 3.080000, 0.997637, -0.005959, 0.000100, 0.000000, 0.000000, 0.000100, 316 | 3.090000, 0.991805, -0.003831, 0.000100, 0.000000, 0.000000, 0.000100, 317 | 3.100000, 1.018913, -0.007601, 0.000100, 0.000000, 0.000000, 0.000100, 318 | 3.110000, 1.004285, -0.012699, 0.000100, 0.000000, 0.000000, 0.000100, 319 | 3.120000, 0.991320, 0.008957, 0.000100, 0.000000, 0.000000, 0.000100, 320 | 3.130000, 0.994447, -0.003204, 0.000100, 0.000000, 0.000000, 0.000100, 321 | 3.140000, 0.994144, 0.011221, 0.000100, 0.000000, 0.000000, 0.000100, 322 | 3.150000, 0.982267, 0.002224, 0.000100, 0.000000, 0.000000, 0.000100, 323 | 3.160000, 0.993294, 0.004887, 0.000100, 0.000000, 0.000000, 0.000100, 324 | 3.170000, 0.983718, 0.017977, 0.000100, 0.000000, 0.000000, 0.000100, 325 | 3.180000, 0.995907, 0.020394, 0.000100, 0.000000, 0.000000, 0.000100, 326 | 3.190000, 0.993278, -0.009494, 0.000100, 0.000000, 0.000000, 0.000100, 327 | 3.200000, 1.003012, -0.010025, 0.000100, 0.000000, 0.000000, 0.000100, 328 | 3.210000, 1.003025, 0.004421, 0.000100, 0.000000, 0.000000, 0.000100, 329 | 3.220000, 1.004236, 0.000994, 0.000100, 0.000000, 0.000000, 0.000100, 330 | 3.230000, 0.999561, -0.007563, 0.000100, 0.000000, 0.000000, 0.000100, 331 | 3.240000, 0.993378, 0.016192, 0.000100, 0.000000, 0.000000, 0.000100, 332 | 3.250000, 0.978903, -0.009832, 0.000100, 0.000000, 0.000000, 0.000100, 333 | 3.260000, 1.003088, 0.004476, 0.000100, 0.000000, 0.000000, 0.000100, 334 | 3.270000, 0.994972, 0.002843, 0.000100, 0.000000, 0.000000, 0.000100, 335 | 3.280000, 0.997200, 0.009267, 0.000100, 0.000000, 0.000000, 0.000100, 336 | 3.290000, 1.004031, -0.005097, 0.000100, 0.000000, 0.000000, 0.000100, 337 | 3.300000, 0.977358, 0.004136, 0.000100, 0.000000, 0.000000, 0.000100, 338 | 3.310000, 1.020572, 0.004975, 0.000100, 0.000000, 0.000000, 0.000100, 339 | 3.320000, 0.997174, 0.016406, 0.000100, 0.000000, 0.000000, 0.000100, 340 | 3.330000, 1.001602, 0.017244, 0.000100, 0.000000, 0.000000, 0.000100, 341 | 3.340000, 1.001753, 0.006783, 0.000100, 0.000000, 0.000000, 0.000100, 342 | 3.350000, 1.014619, 0.007758, 0.000100, 0.000000, 0.000000, 0.000100, 343 | 3.360000, 1.023204, -0.012258, 0.000100, 0.000000, 0.000000, 0.000100, 344 | 3.370000, 0.986986, 0.012118, 0.000100, 0.000000, 0.000000, 0.000100, 345 | 3.380000, 0.995043, 0.000535, 0.000100, 0.000000, 0.000000, 0.000100, 346 | 3.390000, 1.008951, 0.002900, 0.000100, 0.000000, 0.000000, 0.000100, 347 | 3.400000, 0.997513, 0.004362, 0.000100, 0.000000, 0.000000, 0.000100, 348 | 3.410000, 0.988265, -0.000479, 0.000100, 0.000000, 0.000000, 0.000100, 349 | 3.420000, 1.012088, 0.003421, 0.000100, 0.000000, 0.000000, 0.000100, 350 | 3.430000, 1.027293, 0.012112, 0.000100, 0.000000, 0.000000, 0.000100, 351 | 3.440000, 1.017476, 0.002225, 0.000100, 0.000000, 0.000000, 0.000100, 352 | 3.450000, 0.987713, -0.006704, 0.000100, 0.000000, 0.000000, 0.000100, 353 | 3.460000, 1.015172, -0.009996, 0.000100, 0.000000, 0.000000, 0.000100, 354 | 3.470000, 1.008928, 0.006145, 0.000100, 0.000000, 0.000000, 0.000100, 355 | 3.480000, 0.990103, 0.009733, 0.000100, 0.000000, 0.000000, 0.000100, 356 | 3.490000, 0.987911, 0.001808, 0.000100, 0.000000, 0.000000, 0.000100, 357 | 3.500000, 0.991162, 0.002761, 0.000100, 0.000000, 0.000000, 0.000100, 358 | 3.510000, 0.998809, -0.014768, 0.000100, 0.000000, 0.000000, 0.000100, 359 | 3.520000, 1.010667, 0.010931, 0.000100, 0.000000, 0.000000, 0.000100, 360 | 3.530000, 1.004264, -0.003282, 0.000100, 0.000000, 0.000000, 0.000100, 361 | 3.540000, 0.981085, -0.008694, 0.000100, 0.000000, 0.000000, 0.000100, 362 | 3.550000, 1.003003, 0.001285, 0.000100, 0.000000, 0.000000, 0.000100, 363 | 3.560000, 0.999570, 0.001484, 0.000100, 0.000000, 0.000000, 0.000100, 364 | 3.570000, 1.011650, 0.009601, 0.000100, 0.000000, 0.000000, 0.000100, 365 | 3.580000, 0.983686, 0.000031, 0.000100, 0.000000, 0.000000, 0.000100, 366 | 3.590000, 0.979762, 0.020054, 0.000100, 0.000000, 0.000000, 0.000100, 367 | 3.600000, 1.015783, 0.012601, 0.000100, 0.000000, 0.000000, 0.000100, 368 | 3.610000, 1.002983, 0.007289, 0.000100, 0.000000, 0.000000, 0.000100, 369 | 3.620000, 0.996438, 0.010076, 0.000100, 0.000000, 0.000000, 0.000100, 370 | 3.630000, 0.996268, -0.015702, 0.000100, 0.000000, 0.000000, 0.000100, 371 | 3.640000, 1.009066, 0.016181, 0.000100, 0.000000, 0.000000, 0.000100, 372 | 3.650000, 1.005720, -0.026805, 0.000100, 0.000000, 0.000000, 0.000100, 373 | 3.660000, 1.009466, 0.022937, 0.000100, 0.000000, 0.000000, 0.000100, 374 | 3.670000, 0.979340, 0.015670, 0.000100, 0.000000, 0.000000, 0.000100, 375 | 3.680000, 1.010869, -0.009708, 0.000100, 0.000000, 0.000000, 0.000100, 376 | 3.690000, 0.993822, -0.023225, 0.000100, 0.000000, 0.000000, 0.000100, 377 | 3.700000, 1.024696, -0.011551, 0.000100, 0.000000, 0.000000, 0.000100, 378 | 3.710000, 1.000250, -0.003717, 0.000100, 0.000000, 0.000000, 0.000100, 379 | 3.720000, 1.002906, 0.000712, 0.000100, 0.000000, 0.000000, 0.000100, 380 | 3.730000, 0.985946, -0.000078, 0.000100, 0.000000, 0.000000, 0.000100, 381 | 3.740000, 1.000007, -0.003785, 0.000100, 0.000000, 0.000000, 0.000100, 382 | 3.750000, 1.002949, -0.007431, 0.000100, 0.000000, 0.000000, 0.000100, 383 | 3.760000, 0.987969, -0.003411, 0.000100, 0.000000, 0.000000, 0.000100, 384 | 3.770000, 0.993427, -0.013002, 0.000100, 0.000000, 0.000000, 0.000100, 385 | 3.780000, 1.007141, -0.003285, 0.000100, 0.000000, 0.000000, 0.000100, 386 | 3.790000, 1.004923, -0.003496, 0.000100, 0.000000, 0.000000, 0.000100, 387 | 3.800000, 1.004203, 0.006285, 0.000100, 0.000000, 0.000000, 0.000100, 388 | 3.810000, 1.000559, -0.012281, 0.000100, 0.000000, 0.000000, 0.000100, 389 | 3.820000, 0.996979, -0.010195, 0.000100, 0.000000, 0.000000, 0.000100, 390 | 3.830000, 1.004865, -0.004146, 0.000100, 0.000000, 0.000000, 0.000100, 391 | 3.840000, 0.992763, 0.005874, 0.000100, 0.000000, 0.000000, 0.000100, 392 | 3.850000, 1.009231, -0.011651, 0.000100, 0.000000, 0.000000, 0.000100, 393 | 3.860000, 0.999789, 0.005302, 0.000100, 0.000000, 0.000000, 0.000100, 394 | 3.870000, 0.999228, 0.018556, 0.000100, 0.000000, 0.000000, 0.000100, 395 | 3.880000, 0.997317, 0.006020, 0.000100, 0.000000, 0.000000, 0.000100, 396 | 3.890000, 0.987892, -0.006978, 0.000100, 0.000000, 0.000000, 0.000100, 397 | 3.900000, 1.004394, 0.006561, 0.000100, 0.000000, 0.000000, 0.000100, 398 | 3.910000, 1.002943, 0.021718, 0.000100, 0.000000, 0.000000, 0.000100, 399 | 3.920000, 0.994942, -0.005654, 0.000100, 0.000000, 0.000000, 0.000100, 400 | 3.930000, 1.010336, -0.008209, 0.000100, 0.000000, 0.000000, 0.000100, 401 | 3.940000, 1.022165, -0.005583, 0.000100, 0.000000, 0.000000, 0.000100, 402 | 3.950000, 0.996943, 0.011288, 0.000100, 0.000000, 0.000000, 0.000100, 403 | 3.960000, 0.992160, 0.009371, 0.000100, 0.000000, 0.000000, 0.000100, 404 | 3.970000, 1.007921, -0.010745, 0.000100, 0.000000, 0.000000, 0.000100, 405 | 3.980000, 1.008340, 0.003418, 0.000100, 0.000000, 0.000000, 0.000100, 406 | 3.990000, 0.994441, -0.006307, 0.000100, 0.000000, 0.000000, 0.000100, 407 | 4.000000, 1.000190, 0.002434, 0.000100, 0.000000, 0.000000, 0.000100, 408 | 4.010000, 0.991735, 0.003159, 0.000100, 0.000000, 0.000000, 0.000100, 409 | 4.020000, 0.998768, -0.008395, 0.000100, 0.000000, 0.000000, 0.000100, 410 | 4.030000, 0.989823, -0.005634, 0.000100, 0.000000, 0.000000, 0.000100, 411 | 4.040000, 0.991951, 0.010725, 0.000100, 0.000000, 0.000000, 0.000100, 412 | 4.050000, 0.988634, 0.001332, 0.000100, 0.000000, 0.000000, 0.000100, 413 | 4.060000, 1.005206, -0.009689, 0.000100, 0.000000, 0.000000, 0.000100, 414 | 4.070000, 1.003687, -0.004831, 0.000100, 0.000000, 0.000000, 0.000100, 415 | 4.080000, 0.991170, -0.014607, 0.000100, 0.000000, 0.000000, 0.000100, 416 | 4.090000, 0.990504, 0.017155, 0.000100, 0.000000, 0.000000, 0.000100, 417 | 4.100000, 0.984290, -0.006531, 0.000100, 0.000000, 0.000000, 0.000100, 418 | 4.110000, 0.988451, 0.007613, 0.000100, 0.000000, 0.000000, 0.000100, 419 | 4.120000, 0.990819, -0.004437, 0.000100, 0.000000, 0.000000, 0.000100, 420 | 4.130000, 1.007227, 0.009312, 0.000100, 0.000000, 0.000000, 0.000100, 421 | 4.140000, 0.989072, -0.001918, 0.000100, 0.000000, 0.000000, 0.000100, 422 | 4.150000, 1.003783, -0.000108, 0.000100, 0.000000, 0.000000, 0.000100, 423 | 4.160000, 1.018688, 0.012950, 0.000100, 0.000000, 0.000000, 0.000100, 424 | 4.170000, 1.013331, -0.020540, 0.000100, 0.000000, 0.000000, 0.000100, 425 | 4.180000, 0.996483, -0.002396, 0.000100, 0.000000, 0.000000, 0.000100, 426 | 4.190000, 0.989133, -0.007904, 0.000100, 0.000000, 0.000000, 0.000100, 427 | 4.200000, 1.007255, -0.013971, 0.000100, 0.000000, 0.000000, 0.000100, 428 | 4.210000, 1.011704, 0.016633, 0.000100, 0.000000, 0.000000, 0.000100, 429 | 4.220000, 1.000132, 0.017979, 0.000100, 0.000000, 0.000000, 0.000100, 430 | 4.230000, 1.016070, 0.000788, 0.000100, 0.000000, 0.000000, 0.000100, 431 | 4.240000, 0.995999, 0.008521, 0.000100, 0.000000, 0.000000, 0.000100, 432 | 4.250000, 0.993356, -0.001763, 0.000100, 0.000000, 0.000000, 0.000100, 433 | 4.260000, 1.012006, -0.010106, 0.000100, 0.000000, 0.000000, 0.000100, 434 | 4.270000, 0.993026, 0.008778, 0.000100, 0.000000, 0.000000, 0.000100, 435 | 4.280000, 0.981621, 0.009307, 0.000100, 0.000000, 0.000000, 0.000100, 436 | 4.290000, 0.999047, -0.001479, 0.000100, 0.000000, 0.000000, 0.000100, 437 | 4.300000, 0.998461, -0.003877, 0.000100, 0.000000, 0.000000, 0.000100, 438 | 4.310000, 0.999443, -0.004626, 0.000100, 0.000000, 0.000000, 0.000100, 439 | 4.320000, 0.992112, 0.010096, 0.000100, 0.000000, 0.000000, 0.000100, 440 | 4.330000, 0.999967, 0.004570, 0.000100, 0.000000, 0.000000, 0.000100, 441 | 4.340000, 1.005650, -0.001730, 0.000100, 0.000000, 0.000000, 0.000100, 442 | 4.350000, 0.996411, 0.004310, 0.000100, 0.000000, 0.000000, 0.000100, 443 | 4.360000, 0.996968, 0.005186, 0.000100, 0.000000, 0.000000, 0.000100, 444 | 4.370000, 0.989108, 0.000290, 0.000100, 0.000000, 0.000000, 0.000100, 445 | 4.380000, 1.001193, 0.013794, 0.000100, 0.000000, 0.000000, 0.000100, 446 | 4.390000, 1.017481, -0.002265, 0.000100, 0.000000, 0.000000, 0.000100, 447 | 4.400000, 0.993280, -0.010454, 0.000100, 0.000000, 0.000000, 0.000100, 448 | 4.410000, 0.994248, 0.000026, 0.000100, 0.000000, 0.000000, 0.000100, 449 | 4.420000, 0.987536, -0.004771, 0.000100, 0.000000, 0.000000, 0.000100, 450 | 4.430000, 1.005512, 0.014249, 0.000100, 0.000000, 0.000000, 0.000100, 451 | 4.440000, 1.009819, -0.006349, 0.000100, 0.000000, 0.000000, 0.000100, 452 | 4.450000, 0.980009, -0.003983, 0.000100, 0.000000, 0.000000, 0.000100, 453 | 4.460000, 1.003493, 0.000501, 0.000100, 0.000000, 0.000000, 0.000100, 454 | 4.470000, 0.993948, -0.005631, 0.000100, 0.000000, 0.000000, 0.000100, 455 | 4.480000, 1.004431, 0.013849, 0.000100, 0.000000, 0.000000, 0.000100, 456 | 4.490000, 1.006267, -0.021905, 0.000100, 0.000000, 0.000000, 0.000100, 457 | 4.500000, 0.991588, -0.010779, 0.000100, 0.000000, 0.000000, 0.000100, 458 | 4.510000, 0.990923, 0.013858, 0.000100, 0.000000, 0.000000, 0.000100, 459 | 4.520000, 1.003629, 0.000239, 0.000100, 0.000000, 0.000000, 0.000100, 460 | 4.530000, 0.998806, -0.019000, 0.000100, 0.000000, 0.000000, 0.000100, 461 | 4.540000, 0.997054, -0.006099, 0.000100, 0.000000, 0.000000, 0.000100, 462 | 4.550000, 0.998365, -0.007962, 0.000100, 0.000000, 0.000000, 0.000100, 463 | 4.560000, 1.006620, -0.003268, 0.000100, 0.000000, 0.000000, 0.000100, 464 | -------------------------------------------------------------------------------- /examples/InEKF/sensor_data/readme.md: -------------------------------------------------------------------------------- 1 | The sensor files here are an example of sensor data. -------------------------------------------------------------------------------- /examples/KalmanFilter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required( VERSION 3.10) 2 | project(example_kf) 3 | 4 | # Find the RandomVariable directory 5 | list(APPEND CMAKE_PREFIX_PATH "../../extern/RandomVariable/cmake") 6 | find_package(RandomVariable) 7 | # Find the Eigen library 8 | find_package(Eigen3 REQUIRED) 9 | # # Find the manif library 10 | # find_package(manif REQUIRED) 11 | add_executable(example_kf kalman_filter.cpp) 12 | 13 | target_link_libraries(example_kf 14 | RandomVariable::RandomVariable 15 | ) 16 | 17 | target_include_directories(example_kf SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) 18 | 19 | if(CMAKE_BUILD_TYPE STREQUAL "Debug") 20 | message("CMAKE_BUILD_TYPE: Debug mode\n") 21 | # Add debug flag 22 | target_compile_definitions(example_kf PUBLIC DEBUG) 23 | elseif(CMAKE_BUILD_TYPE STREQUAL "Release") 24 | message("CMAKE_BUILD_TYPE: Release mode") 25 | endif() 26 | 27 | # Set required C++ standard flag 28 | set_property(TARGET example_kf PROPERTY CXX_STANDARD 17) 29 | -------------------------------------------------------------------------------- /examples/KalmanFilter/kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include // For nice outputs (setw) 4 | #include // To definte the file_name for exporting data 5 | #include 6 | #include 7 | 8 | #include // To use the exponential function 9 | #include "Eigen/Dense" 10 | 11 | // Include parser 12 | #include "RV.h" 13 | #include "RVIO.h" 14 | 15 | 16 | Eigen::VectorXd process_model( Eigen::VectorXd x_km1, Eigen::VectorXd u_km1, Eigen::MatrixXd A, Eigen::MatrixXd B){ 17 | return A * x_km1 + B * u_km1; 18 | } 19 | 20 | // Dimension of control input measurements 21 | const size_t size_u = 1; 22 | // Dimension of the exeteroceptive measurement 23 | const size_t size_y = 1; 24 | // Dimension of each pose 25 | const size_t size_x = 2; 26 | 27 | // Eigen matrix types 28 | typedef Eigen::Matrix< double, size_x, 1> VectorPose; 29 | typedef Eigen::Matrix< double, size_x, size_x> CovPose; 30 | 31 | typedef Eigen::Matrix< double, size_x, size_x> MatrixA; 32 | typedef Eigen::Matrix< double, size_x, size_x> MatrixQ; 33 | typedef Eigen::Matrix< double, size_x, size_u> MatrixB; 34 | 35 | 36 | // Acceleration measurement class 37 | typedef RandomVariable< size_u> MeasControlInput; 38 | typedef RandomVariable< size_y> MeasGPS; 39 | typedef RandomVariable< size_x> PoseEstimate; 40 | 41 | // Returns A, B, Q discrete-time matrices 42 | template 43 | std::tuple getDiscreteABQ(MatrixA A_ct, MatrixB B_ct, T_L L_ct, T_Qct Q_ct, double dt){ 44 | // Construct the Xi matrix 45 | Eigen::Matrix Xi; 46 | Xi.setZero(); 47 | Xi.block< size_x, size_x>(0, 0) = A_ct; 48 | Xi.block< size_x, size_x>(size_x, size_x) = -A_ct.transpose(); 49 | Xi.block< size_x, size_x>(2 * size_x, 2 * size_x) = A_ct; 50 | Xi.block< size_x, size_u>(2 * size_x, 3 * size_x) = B_ct; 51 | Xi.block< size_x, size_x>(0, size_x) = L_ct * Q_ct * L_ct.transpose(); 52 | 53 | // Eigen::Matrix 54 | auto 55 | Gamma = (dt * Xi).exp(); 56 | // Extract the blocks 57 | MatrixA A_dt = Gamma.block (0, 0); 58 | MatrixB B_dt = Gamma.block (2 * size_x, 3 * size_x); 59 | auto Gamma_12 = Gamma.block(0, size_x); 60 | MatrixQ Q_dt = Gamma_12 * A_dt.transpose(); 61 | return std::make_tuple(A_dt, B_dt, Q_dt); 62 | } 63 | 64 | 65 | 66 | // Control input file name 67 | const std::string file_name_u = "/home/aa/Documents/Data/Data_generator/linear_system/msd_acc.txt"; 68 | 69 | const std::string file_name_gps = "/home/aa/Documents/Data/Data_generator/linear_system/msd_pos.txt"; 70 | 71 | 72 | int main(){ 73 | // GPS sensor 74 | Eigen::Matrix sys_C; 75 | sys_C << 1, 0; 76 | Eigen::Matrix sys_M; 77 | sys_M.setIdentity(); 78 | 79 | // Define CT matrices 80 | // ********************************************* 81 | // Cotinuous-time (CT) system parameters 82 | // Continuous-time system matrix 83 | MatrixA sys_A_ct; 84 | // Continuous-time control matrix 85 | MatrixB sys_B_ct; 86 | Eigen::Vector2d sys_L_ct; 87 | // System is a mass-spring-damper system with unit mass, spring-constant, and damping. 88 | sys_A_ct << 0, 1, -1, -1; 89 | sys_B_ct << 0, 1; 90 | sys_L_ct = sys_B_ct; 91 | 92 | // ********************************************* 93 | // Initial conditions 94 | VectorPose x_0( 0., 0.); 95 | // Covariance on initial condition 96 | CovPose cov_P_0; 97 | cov_P_0 << 1e-3, 0, 0, 1e-3; 98 | 99 | 100 | // ************************************************ 101 | // Import control input 102 | std::vector< MeasControlInput> meas_control_input = RV::IO::import( file_name_u); 103 | #ifdef DEBUG 104 | for( auto i : meas_control_input){ 105 | std::cout << i.time() << "\t" << i.mean() << "\t" << i.cov().transpose() << std::endl; 106 | } 107 | #endif 108 | // Lambda function that extractes the sample time (dt) 109 | auto dt_func = [&meas_control_input](int k){ 110 | return meas_control_input[k].time() - meas_control_input[k-1].time(); 111 | }; 112 | #ifdef DEBUG 113 | // Try it out 114 | std::cout << "dt_{5}: " << dt_func(5) << std::endl; 115 | #endif 116 | 117 | // ************************************************ 118 | // Import GPS measurements 119 | std::vector< MeasGPS> meas_gps = RV::IO::import( file_name_gps); 120 | 121 | // ********************************************* 122 | // Number of poses 123 | // TODO: change it to match the number of control input measurements 124 | const unsigned int K = meas_control_input.size() + 1; 125 | // const unsigned int K = 10; 126 | 127 | // ********************************************* 128 | // Estaimted states 129 | std::vector< PoseEstimate> estiamted_states( K); 130 | // Set time steps for each state 131 | for( int i = 0; i < K - 1; i++){ 132 | estiamted_states[i].setTime( 133 | meas_control_input[i].time() 134 | ); 135 | } 136 | // Set the last estimate time 137 | estiamted_states[K - 1].setTime( estiamted_states[K-2].time() + dt_func(K-2)); 138 | 139 | // Set the time from the first time element of the control input measurement 140 | estiamted_states[0].setMean( x_0); 141 | estiamted_states[0].setCov(cov_P_0); 142 | 143 | 144 | // ********************************************* 145 | // Run the Kalman filter 146 | // Estimate and covariance at the previous step 147 | VectorPose x_km1; 148 | CovPose P_km1; 149 | // Estimate and covariance at the current step 150 | VectorPose x_k; 151 | CovPose P_k; 152 | // Control input and covariance at previous time step 153 | Eigen::Matrix u_km1; 154 | Eigen::Matrix Cov_u_km1; 155 | // DT process model matrices 156 | MatrixA A_km1; 157 | MatrixB B_km1; 158 | MatrixA Q_km1; // Discrete-time process noise covariance 159 | // Index that keeps track of the gps measurements 160 | size_t idx_gps = 0; 161 | for( int i = 1; i < K; i++){ 162 | // Get the pose estimate and covariance at the previous step 163 | x_km1 = estiamted_states[i - 1].mean(); 164 | P_km1 = estiamted_states[i - 1].cov(); 165 | u_km1 = meas_control_input[i - 1].mean(); 166 | Cov_u_km1 = meas_control_input[i - 1].cov(); 167 | 168 | // Compute DT process model matrices 169 | double dt; 170 | if( i < K - 1){ 171 | dt = dt_func(i); 172 | }// Else, use the same dt from the previous iteration. 173 | std::tie( A_km1, B_km1, Q_km1) = getDiscreteABQ( sys_A_ct, sys_B_ct, sys_L_ct, Cov_u_km1, dt); 174 | 175 | // Prediction step 176 | x_k = process_model( x_km1, u_km1, A_km1, B_km1); 177 | P_k = A_km1 * P_km1 * A_km1. transpose() + Q_km1; 178 | // Ensure symmetry of the covariance 179 | P_k = 0.5 * (P_k + P_k.transpose()); 180 | 181 | // Check for correction 182 | if( idx_gps < meas_gps.size() && (meas_gps[idx_gps].time() <= estiamted_states[i].time())){ 183 | // Implement a correction 184 | auto R_k = meas_gps[idx_gps].cov(); 185 | auto y_k = meas_gps[idx_gps].mean(); 186 | // Compute S_k 187 | Eigen::Matrix< double, size_y, size_y> S_k = sys_C * P_k * sys_C.transpose() + sys_M * R_k * sys_M.transpose(); 188 | // Ensure symmetry 189 | S_k = 0.5 * (S_k + S_k.transpose()); 190 | // Compute Kalman gain 191 | // Eigen::Matrix K_k = 192 | // (S_k.ldlt().solve(sys_C * P_k)).transpose(); 193 | Eigen::Matrix K_k = 194 | (S_k.colPivHouseholderQr().solve(sys_C * P_k)).transpose(); 195 | // Update state estimate (xhat) 196 | x_k += K_k * (y_k - sys_C * x_k); 197 | // Update covariance 198 | P_k = ((Eigen::Matrix::Identity()) - K_k * sys_C) * P_k; 199 | // Ensure symmetry 200 | P_k = 0.5 * (P_k + P_k.transpose()); 201 | 202 | idx_gps++; 203 | } 204 | // For now, store estimates 205 | estiamted_states[i].setMean( x_k); 206 | estiamted_states[i].setCov( P_k); 207 | } 208 | 209 | // Display estimates 210 | std::cout << "\n\n===============================\nEstimates\n" << std::endl; 211 | std::cout << "Time\t\tMeas\t\tVar" << std::endl; 212 | for( auto meas : estiamted_states){ 213 | // RV::IO::print( meas); 214 | std::cout << meas.mean().transpose() << std::endl; 215 | // std::cout << std::endl; 216 | } 217 | 218 | 219 | 220 | // ****************************************** 221 | // Exporting data 222 | std::string file_name_out = "/home/aa/Documents/Data/Data_generator/linear_system/msd_kf_estimates.txt"; 223 | std::cout << "\nExporting estimates to '" << file_name_out << "'" << std::endl; 224 | 225 | RV::IO::write( estiamted_states, file_name_out, "x"); 226 | } -------------------------------------------------------------------------------- /examples/KalmanFilter/playground.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include // For nice outputs 4 | #include 5 | #include "Eigen/Dense" 6 | 7 | // Dimension of control input measurements 8 | const size_t size_u = 1; 9 | // Dimension of the exeteroceptive measurement 10 | const size_t size_y = 1; 11 | // Dimension of each pose 12 | const size_t size_x = 2; 13 | 14 | typedef Eigen::Matrix< double, size_x, 1> VectorPose; 15 | typedef Eigen::Matrix< double, size_x, size_x> CovPose; 16 | 17 | typedef Eigen::Matrix< double, size_x, size_x> MatrixA; 18 | typedef Eigen::Matrix< double, size_x, size_x> MatrixQ; 19 | typedef Eigen::Matrix< double, size_x, size_u> MatrixB; 20 | 21 | #include 22 | // Returns A, B, Q discrete-time matrices 23 | template 24 | std::tuple getDiscreteABQ(MatrixA A_ct, MatrixB B_ct, T_L L_ct, T_Qct Q_ct, double dt){ 25 | // Construct the Xi matrix 26 | Eigen::Matrix Xi; 27 | Xi.setZero(); 28 | Xi.block< size_x, size_x>(0, 0) = A_ct; 29 | Xi.block< size_x, size_x>(size_x, size_x) = -A_ct.transpose(); 30 | Xi.block< size_x, size_x>(2 * size_x, 2 * size_x) = A_ct; 31 | Xi.block< size_x, size_u>(2 * size_x, 3 * size_x) = B_ct; 32 | Xi.block< size_x, size_x>(0, size_x) = L_ct * Q_ct * L_ct.transpose(); 33 | 34 | // Eigen::Matrix 35 | auto 36 | Gamma = (dt * Xi).exp(); 37 | // Extract the blocks 38 | MatrixA A_dt = Gamma.block (0, 0); 39 | MatrixB B_dt = Gamma.block (2 * size_x, 3 * size_x); 40 | auto Gamma_12 = Gamma.block(0, size_x); 41 | MatrixQ Q_dt = Gamma_12 * A_dt.transpose(); 42 | return std::make_tuple(A_dt, B_dt, Q_dt); 43 | } 44 | 45 | int main(){ 46 | // The parameters defined in this scope are only needed to compute the DT matrices. Therefore, they'll be deleted once they leave the scope. 47 | // ********************************************* 48 | // Cotinuous-time (CT) system parameters 49 | // Continuous-time system matrix 50 | Eigen::Matrix2d sys_A_ct; 51 | // Continuous-time control matrix 52 | Eigen::Vector2d sys_B_ct; 53 | // System is a mass-spring-damper system with unit mass, spring-constant, and damping. 54 | sys_A_ct << 0, 1, -1, -1; 55 | sys_B_ct << 0, 1; 56 | auto sys_L_ct = sys_B_ct; 57 | 58 | Eigen::Matrix Q_ct_km1; 59 | Q_ct_km1.setIdentity(); 60 | 61 | // Declare the discrete-time (DT) system matrices. These are to be computed using a zero-order hold using continous-time (CT) matrices 62 | Eigen::Matrix2d sys_A; 63 | Eigen::Vector2d sys_B; 64 | Eigen::Matrix2d sys_Q; 65 | // std::tie( sys_A, sys_B, sys_Q) = getDiscreteABQ( sys_A_ct, sys_B_ct, sys_L_ct, Q_ct_km1); 66 | 67 | std::tie( sys_A, sys_B, sys_Q) = getDiscreteABQ( sys_A_ct, sys_B_ct, sys_L_ct, Q_ct_km1, 0.1); 68 | std::cout << "sys_A:\n" << sys_A << std::endl; 69 | std::cout << "sys_B:\n" << sys_B << std::endl; 70 | } --------------------------------------------------------------------------------