• Stars
    star
    420
  • Rank 103,194 (Top 3 %)
  • Language
    C++
  • License
    GNU General Publi...
  • Created almost 4 years ago
  • Updated over 1 year ago

Reviews

There are no reviews yet. Be the first to send feedback to the community and the maintainers!

Repository Details

A computationally efficient and convenient toolkit of iterated Kalman filter.

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 manifold 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

1. Prerequisites

1.1. Eigen && Boost

Eigen >= 3.3.4, Follow Eigen Installation.

Boost >= 1.65.

2. Usage when the measurement is of constant dimension and type.

Clone the repository:

    git clone https://github.com/hku-mars/IKFoM.git
  1. include the necessary head file:
#include<esekfom/esekfom.hpp>
  1. Select and instantiate the primitive manifolds:
    typedef MTK::SO3<double> SO3; // scalar type of variable: double
    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
    typedef MTK::S2<double, 98, 10, 1> 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 that is defined as , and its differentiation , , where w=0 could be left out:
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
	res(0) = s.vel[0];
	res(1) = s.vel[1];
	res(2) = s.vel[2];
	return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
	return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
	return cov;
}

Those functions would be called during the ekf state predict

  1. Implement the output equation and its differentiation , :
measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false
{
	if (condition){ valid = false; 
	} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
	measurement h_;
	h_.position = s.pos;
	return h_;
}
Eigen::Matrix<double, measurement_dof, state_dof> dh_dx(state &s, bool &valid) {} 
Eigen::Matrix<double, measurement_dof, measurement_noise_dof> dh_dv(state &s, bool &valid) {}

Those functions would be called during the ekf state update

  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance.

(1) initial state and covariance:

state init_state;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);

(2) default state and covariance:

esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;

where process_noise_dof is the dimension of process noise, with the type of std int, and so for measurement_noise_dof.

  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
  1. In the running time, once an input in or a measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
  1. Once a measurement z is received, an iterated update is executed:
kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix

Remarks(1):

  • We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
  1. Implement the output equation and its differentiation , :
measurement h_share(state &s, esekfom::share_datastruct<state, measurement, measurement_noise_dof> &share_data) 
{
	if(share_data.converge) {} // this value is true means iteration is converged 
	if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
	share_data.h_x = H_x; // H_x is the result matrix of the first differentiation 
	share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
	share_data.R = R; // R is the measurement noise covariance
	share_data.z = z; // z is the obtained measurement 

	measurement h_;
	h_.position = s.pos;
	return h_;
}

This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function

  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance.

(1) initial state and covariance:

state init_state;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);

(2) default state and covariance:

esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi);
  1. In the running time, once an input in or a measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q
  1. Once a measurement z is received, an iterated update is executed:
kf.update_iterated_share();

Remarks(2):

  • The value of the state x and the covariance P are able to be changed by functions change_x() and change_P():
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov set_P;
kf.change_P(set_P);

3. Usage when the measurement is an Eigen vector of changing dimension.

Clone the repository:

    git clone https://github.com/hku-mars/IKFoM.git
  1. include the necessary head file:
#include<esekfom/esekfom.hpp>
  1. Select and instantiate the primitive manifolds:
    typedef MTK::SO3<double> SO3; // scalar type of variable: double
    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
    typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
  1. Build system state and input 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 that is defined as , and its differentiation , , where w=0 could be left out:
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
	res(0) = s.vel[0];
	res(1) = s.vel[1];
	res(2) = s.vel[2];
	return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
	return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
	return cov;
}

Those functions would be called during ekf state predict

  1. Implement the output equation and its differentiation , :
Eigen::Matrix<double, Eigen::Dynamic, 1> h(state &s, bool &valid) //the iteration stops before convergence when valid is false {
	if (condition){ valid = false; 
	} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
	Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
	h_(0) = s.pos[0];
	return h_;
}
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s, bool &valid) {} 
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s, bool &valid) {}

Those functions would be called during ekf state update

  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance.

(1) initial state and covariance:

state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);

(2) default state and covariance:

esekfom::esekf<state, process_noise_dof, input> kf;

where process_noise_dof is the dimension of process noise, with the type of std int, and so for measurement_noise_dof

  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
  1. In the running time, once an input in or an measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
  1. Once a measurement z is received, an iterated update is executed:
kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix

Remarks(1):

  • We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
  1. Implement the output equation and its differentiation , :
Eigen::Matrix<double, Eigen::Dynamic, 1> h_dyn_share(state &s, esekfom::dyn_share_datastruct<double> &dyn_share_data) 
{
	if(dyn_share_data.converge) {} // this value is true means iteration is converged 
	if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
	dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
	dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation 
	dyn_share_data.R = R; // R is the measurement noise covariance
	dyn_share_data.z = z; // z is the obtained measurement 

	Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
	h_(0) = s.pos[0];
	return h_;
}
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance. (1) initial state and covariance:
state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);

(2) default state and covariance:

esekfom::esekf<state, process_noise_dof, input> kf;
  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi);
  1. In the running time, once an input in or a measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
  1. Once a measurement z is received, an iterated update is executed:
kf.update_iterated_dyn_share();

Remarks(2):

  • The value of the state x and the covariance P are able to be changed by functions change_x() and change_P():
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
kf.change_P(set_P);

4. Usage when the measurement is a changing manifold during the run time.

Clone the repository:

    git clone https://github.com/hku-mars/IKFoM.git
  1. include the necessary head file:
#include<esekfom/esekfom.hpp>
  1. Select and instantiate the primitive manifolds:
    typedef MTK::SO3<double> SO3; // scalar type of variable: double
    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
    typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
  1. Build system state and input 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 that is defined as , and its differentiation , , where w=0 could be left out:
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
	res(0) = s.vel[0];
	res(1) = s.vel[1];
	res(2) = s.vel[2];
	return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
	return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
	return cov;
}

Those functions would be called during ekf state predict

  1. Implement the differentiation of the output equation , :
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s, bool &valid) {}

Those functions would be called during ekf state update

  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance.

(1) initial state and covariance:

state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);

(2)

esekfom::esekf<state, process_noise_dof, input> kf;

Where process_noise_dof is the dimension of process noise, of type of std int

  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi);
  1. In the running time, once an input in or a measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q
  1. Once a measurement z is received, build system measurement as compound manifolds following step 3 and implement the output equation :
measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false
{
	if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied
	measurement h_;
	h_.pos = s.pos;
	return h_;
}

then an iterated update is executed:

kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix

Remarks(1):

  • We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
  1. Instantiate an esekf object kf and initialize it with initial or default state and covariance.

(1) initial state and covariance:

state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);

(2) default state and covariance:

esekfom::esekf<state, process_noise_dof, input> kf;
  1. Deliver the defined models, std int maximum iteration numbers Maximum_iter, and the std array for testing convergence epsi into the esekf object:
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi);
  1. In the running time, once an input in or a measurement z is received dt after the last propagation or update, a propagation is executed:
kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix
  1. Once a measurement z is received, build system measurement as compound manifolds following step 3 and implement the output equation and its differentiation , :
measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct<double> &dyn_runtime_share_data) 
{
	if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged 
	if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied
	dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation 
	dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
	dyn_runtime_share_data.R = R; // R is the measurement noise covariance
	
	measurement h_;
	h_.pos = s.pos;
	return h_;
}

This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function

then an iterated update is executed:

kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share);

Remarks(2):

  • The value of the state x and the covariance P are able to be changed by functions change_x() and change_P():
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
kf.change_P(set_P);

5. Run samples

Clone the repository:

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

Sample 1. In the Sample_1 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 FAST_LIO
cd ..
catkin_make
source devel/setup.bash

Livox Avia rosbag: Can be downloaded from here

For the indoor bag, run:

roslaunch fast_lio mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag

For the outdoor bag, run:

roslaunch fast_lio mapping_avia_outdoor.launch
rosbag play YOUR_DOWNLOADED.bag

Sample 2. In the Sample_2 file folder, there is the scource code that applys the IKFoM on the original source code from LINS. Please follow the README.md shown in that repository excepting the step Compile, which is modified as:

cd ~/catkin_ws/src
cp -r ~/IKFoM/Samples/LINS---LiDAR-inertial-SLAM LINS---LiDAR-inertial-SLAM
cd ..
catkin_make -j1
source devel/setup.bash

LIO-SAM Dataset: Can be found here, in which, Campus_dataset(large) and Campus_dataset(small) are tested in our paper.

Run

roslaunch lins run_port_exp.launch
rosbag play YOUR_DOWNLOADED.bag --clock

6.Acknowledgments

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

More Repositories

1

FAST_LIO

A computationally efficient and robust LiDAR-inertial odometry (LIO) package
C++
2,549
star
2

r3live

A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package
C++
1,958
star
3

loam_livox

A robust LiDAR Odometry and Mapping (LOAM) package for Livox-LiDAR
C++
1,435
star
4

FAST-LIVO

A Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry (LIVO).
C++
1,086
star
5

livox_camera_calib

This repository is used for automatic calibration between high resolution LiDAR and camera in targetless scenes.
C++
863
star
6

LiDAR_IMU_Init

[IROS2022] Robust Real-time LiDAR-inertial Initialization Method.
C++
834
star
7

Point-LIO

C++
745
star
8

r2live

R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping package
C++
721
star
9

BALM

An efficient and consistent bundle adjustment for lidar mapping
C++
700
star
10

ikd-Tree

This repository provides implementation of an incremental k-d tree for robotic applications.
C++
607
star
11

ImMesh

ImMesh: An Immediate LiDAR Localization and Meshing Framework
C++
590
star
12

STD

A 3D point cloud descriptor for place recognition
C++
548
star
13

VoxelMap

[RA-L 2022] An efficient and probabilistic adaptive voxel mapping method for LiDAR odometry
C++
479
star
14

mlcc

Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras
C++
479
star
15

FAST-LIVO2

FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
471
star
16

HBA

[RAL 2023] A globally consistent LiDAR map optimization module
C++
437
star
17

M-detector

C++
362
star
18

LTAOM

C++
325
star
19

ROG-Map

C++
294
star
20

MARSIM

MARSIM: A light-weight point-realistic simulator for LiDAR-based UAVs
C++
283
star
21

D-Map

D-Map provides an efficient occupancy mapping approach for high-resolution LiDAR sensors.
C++
280
star
22

decentralized_loam

207
star
23

joint-lidar-camera-calib

Joint intrinsic and extrinsic LiDAR-camera calibration.
C++
194
star
24

SLAM-HKU-MaRS-LAB

In this repository, we present our research works of HKU-MaRS lab that related to SLAM
191
star
25

Voxel-SLAM

C++
185
star
26

Swarm-LIO2

Swarm-LIO2: Decentralized, Efficient LiDAR-inertial Odometry for UAV Swarms
158
star
27

dyn_small_obs_avoidance

C++
154
star
28

IPC

Integrated Planning and Control for Quadrotor Navigation in Presence of Sudden Crossing Objects and Disturbances
C++
147
star
29

btc_descriptor

137
star
30

PULSAR

C++
102
star
31

lidar_car_platfrom

48
star
32

iBTC

39
star
33

crossgap_il_rl

Python
38
star
34

multi_lidar_calib

28
star
35

Livox_handheld

25
star
36

mapping_eval

2
star