A computationally efficient and convenient toolkit of iterated Kalman filter.

Overview

IKFoM

IKFoM (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the menifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold and any of its sub-manifolds, and it is extendable to other types of manifold when necessary.

Developers

Dongjiao He

Our related video: https://youtu.be/sz_ZlDkl6fA

Our related paper: https://arxiv.org/pdf/2102.03804.pdf

1. Prerequisites

1.1. Eigen && Boost

Eigen >= 3.3.4, Follow Eigen Installation.

Boost >= 1.65.

2. Usage

Clone the repository:

    git clone https://github.com/hku-mars/IKFoM.git
  1. include the necessary head file:
#include
  1. Select and instantiate the primitive manifolds:
    typedef MTK::SO3 SO3; // scalar type of variable: double
    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
    typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
  1. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds:
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
((vect3, pos)) // ((primitive manifold type, name of variable))
((vect3, vel))
((SO3, rot))
((vect3, bg))
((vect3, ba))
((S2, grav))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I)) 
);
  1. Implement the vector field and its differentiation , :
Eigen::Matrix f(state &s, input &i)}
Eigen::Matrix df_dx(state &s, input &i)} //notice S2 has length of 3 and dimension of 2
Eigen::Matrix df_dw(state &s, input &i)}
  1. Implement the output equation and its differentiation , :
measurement h(state &s, bool &valid)} //the iteration stops before convergence when valid is false
Eigen::Matrix dh_dx(state &s, bool &valid)} 
Eigen::Matrix dh_dv(state &s, bool &valid)}
  1. Instantiate an esekf object kf and initialize it with initial state and covariance.
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
  1. Deliver the defined models, maximum iteration numbers Maximum_iter, and the std array for testing convergence limit into the esekf object:
kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, limit);
  1. In the running time, once an input in is received with time interval dt, a propagation is executed:
kf.predict(dt, Q, input); // process noise covariance: Q
  1. Once a measurement z is received, an iterated update is executed:
kf.update_iterated(z, R); // measurement noise covariance: R

Remarks:

  • We only show the usage when the measurement is of constant dimension and type. If the measurement of your system is changing, there are iterated update functions for the case where measurement is an Eigen vector of changing dimension, and the case where measurement is a changing manifold. The usage of those two conditions would be added later, whose principles are mostly the same as the above case.

3. Run the sample

Clone the repository:

    git clone https://github.com/hku-mars/IKFoM.git

In the Samples file folder, there is the scource code that applys the IKFoM on the original source code from FAST LIO. Please follow the README.md shown in that repository excepting the step 2. Build, which is modified as:

cd ~/catkin_ws/src
cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable
cd ..
catkin_make
source devel/setup.bash

4.Acknowledgments

Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.

Issues
  • 2-sphere manifold in example 6

    2-sphere manifold in example 6

    Hi, author, Thanks for your hard-core paper and sharing to community very much! I have three questions and want to ask for your help. Could you recommend some reference materials about 2-sphere manifold? Thanks for your help and time! image

    opened by narutojxl 6
  • 常量维度测量值更新函数定义问题

    常量维度测量值更新函数定义问题

    作者您好,想请教一个问题: Readme里面看到当测量值是常量维度时的第5步,需要实现一个输出等式"Implement the output equation h(x,v)",并求相关的偏导 假如我现在需要在原有的激光雷达的测量更新基础上,再利用rtk提供的位置信息,对fast-lio的状态向量中位置P进行测量更新,我定义如下的函数是否可行?期待您的回复 image

    opened by shuqiu0202 5
  • How to simplify the solution of equation Eq.(38) in the paper?

    How to simplify the solution of equation Eq.(38) in the paper?

    Hi author, Thank you for your work. I can't get the form in your article in the process of simplifying the solution of \delta j in Eq.(38). I showed my calculation process and hope to get your help. I hope you can understand my problem. 微信图片_20210818220557

    opened by zpw6106 4
  • A question about the S2 manifold

    A question about the S2 manifold

    Thank you so much for your wonderful work. I would like to ask how to determine the orthogonal basis B in the tangent space of S2 manifold. The relevant code seems to be

    				if (vec[0] + length > tolerance<scalar>())
    				{
    
    					res << -vec[1], -vec[2],
    						length - vec[1] * vec[1] / (length + vec[0]), -vec[2] * vec[1] / (length + vec[0]),
    						-vec[2] * vec[1] / (length + vec[0]), length - vec[2] * vec[2] / (length + vec[0]);
    					res /= length;
    				}
    				else
    				{
    					res = Eigen::Matrix<scalar, 3, 2>::Zero();
    					res(1, 1) = -1;
    					res(2, 0) = 1;
    				}
    

    Can you list some relevant materials for me to learn about this and the M and N matrix used in S2 manifold?

    opened by daijicheng 2
  • Delete unnecessary files for using the IKFoM toolkit

    Delete unnecessary files for using the IKFoM toolkit

    (:warning: It's the same commit as the other one, but without doing the Fast-LIO modification if not wanted to be done.)

    Deletes all the irrelevant files and only keeps the toolkit for its use as a submodule.

    opened by Huguet57 1
  • Fast-LIO2's Kalman Filter's modification

    Fast-LIO2's Kalman Filter's modification

    Copied from the Fast-LIO2 source code except for non-informative comments. For anyone who wants to properly use Fast-LIO2's IKFoM modified Kalman Filter function.

    opened by Huguet57 2
  • How to compute equation (38)?

    How to compute equation (38)?

    I am confusing how to compute equation (38) in "Kalman Filters on Differentiable Manifolds" even though I figure out the method in "The iterated kalman filter update as a gauss-newton method". They have different loss functions. image

    opened by LeisureLei 6
Owner
HKU-Mars-Lab
Mechatronics and Robotic Systems (MaRS) Laboratory
HKU-Mars-Lab
The CImg Library is a small and open-source C++ toolkit for image processing

http://cimg.eu The CImg Library is a small and open-source C++ toolkit for image processing, designed with these properties in mind: CImg defines clas

David Tschumperlé 1.1k Jun 27, 2022
Video, Image and GIF upscale/enlarge(Super-Resolution) and Video frame interpolation. Achieved with Waifu2x, SRMD, RealSR, Anime4K, RIFE, CAIN, DAIN and ACNet.

Video, Image and GIF upscale/enlarge(Super-Resolution) and Video frame interpolation. Achieved with Waifu2x, SRMD, RealSR, Anime4K, RIFE, CAIN, DAIN and ACNet.

Aaron Feng 7k Jun 29, 2022
C++ image processing and machine learning library with using of SIMD: SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2, AVX, AVX2, AVX-512, VMX(Altivec) and VSX(Power7), NEON for ARM.

Introduction The Simd Library is a free open source image processing and machine learning library, designed for C and C++ programmers. It provides man

Ihar Yermalayeu 1.6k Jun 24, 2022
CGIF, A fast and lightweight GIF encoder that can create GIF animations and images

CGIF, a GIF encoder written in C A fast and lightweight GIF encoder that can create GIF animations and images. Summary of the main features: user-defi

Daniel Löbl 63 Jun 29, 2022
libspng is a C library for reading and writing PNG format files with a focus on security and ease of use.

libspng (simple png) is a C library for reading and writing Portable Network Graphics (PNG) format files with a focus on security and ease of use.

Randy 508 Jun 22, 2022
A crude untested example showing how to retrieve and display images from multiple cameras with OpenCV and wxWidgets.

About wxOpenCVCameras is a crude untested example of how to retrieve and display images from multiple cameras, using OpenCV to grab images from a came

PB 5 Jun 8, 2022
Generate Height map with Generator (OpenGL and imgui) and Construct Splat Map with generated height map using Algorithm

Generate Height map with Generator (OpenGL and imgui) and Construct Splat Map with generated height map using Algorithm(DPS, BFS, Gradient Descent ... etc) . At Renderer, with height map and blend map which are generated in front of this stage, render high quality terrain with OpenGL

Snowapril 35 Mar 22, 2022
PNG encoder and decoder in C and C++.

PNG encoder and decoder in C and C++, without dependencies

Lode Vandevenne 1.6k Jun 25, 2022
An image and texture viewer for tga, png, apng, exr, dds, gif, hdr, jpg, tif, ico, webp, and bmp files

An image and texture viewer for tga, png, apng, exr, dds, gif, hdr, jpg, tif, ico, webp, and bmp files. Uses Dear ImGui, OpenGL, and Tacent. Useful for game devs as it displays information like the presence of an alpha channel and querying specific pixels for their colour.

Tristan Grimmer 132 Jun 22, 2022
A bunch of functions and helpers classes for D3D12, and DXGI libraries

TypedD3D A bunch of functions and helpers classes for D3D12, and DXGI libraries Namespaces Helpers A bunch of helper functions that interface more wit

Renzy Alarcon 6 Jun 17, 2022
Reading, writing, and processing images in a wide variety of file formats, using a format-agnostic API, aimed at VFX applications.

README for OpenImageIO Introduction The primary target audience for OIIO is VFX studios and developers of tools such as renderers, compositors, viewer

OpenImageIO 1.5k Jun 29, 2022
Tiny ISO-compliant C++ EXIF and XMP parsing library for JPEG.

TinyEXIF: Tiny ISO-compliant C++ EXIF and XMP parsing library for JPEG Introduction TinyEXIF is a tiny, lightweight C++ library for parsing the metada

cDc 76 May 27, 2022
Video++, a C++14 high performance video and image processing library.

Video++ Video++ is a video and image processing library taking advantage of the C++14 standard to ease the writing of fast video and image processing

Matthieu Garrigues 681 Jun 14, 2022
The minimal opencv for Android, iOS and ARM Linux

opencv-mobile ✔️ This project provides the minimal build of opencv library for the Android, iOS and ARM Linux platforms. ✔️ We provide prebuild binary

null 818 Jun 30, 2022
Monster Mash: New Sketch-Based Modeling and Animation Tool

Monster Mash is a new sketch-based modeling and animation tool that allows you to quickly sketch a character, inflate it into 3D, and promptly animate it. You can perform all interactions in the sketching plane. No 3D manipulation is required.

Google 1.1k Jun 27, 2022
ZT is a zig-contained library that automatically compiles+links ImGui, OpenGL, and GLFW into typed packages.

ZT is a zig-contained library that automatically compiles+links ImGui, OpenGL, and GLFW into typed packages. By zig contained I mean that ZT is intend

null 80 Jun 18, 2022
ppl.cv is a high-performance image processing library of openPPL supporting x86 and cuda platforms.

ppl.cv is a high-performance image processing library of openPPL supporting x86 and cuda platforms.

null 318 Jun 10, 2022