# IKFoM
**Repository Path**: marslab/IKFoM
## Basic Information
- **Project Name**: IKFoM
- **Description**: No description available
- **Primary Language**: C++
- **License**: GPL-2.0
- **Default Branch**: hdj-test
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 1
- **Forks**: 0
- **Created**: 2022-01-07
- **Last Updated**: 2025-07-16
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
## 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 \times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2 "\mathbb{R}^m\times SO(3)\times\cdots\times SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2")
and any of its sub-manifolds, and it is extendable to other types of manifold when necessary.
**Developers**
[Dongjiao He](https://github.com/Joanna-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](http://eigen.tuxfamily.org/index.php?title=Main_Page).
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
```
2. 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
```
3. 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))
);
```
4. Implement the vector field  "\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)")
and its differentiation }{\partial\delta\mathbf{x}} "\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}")
, }{\partial\mathbf{w}} "\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}")
:
```
Eigen::Matrix f(state &s, const input &i) {}
Eigen::Matrix df_dx(state &s, const input &i) {} //notice S2 has length of 3 and dimension of 2
Eigen::Matrix df_dw(state &s, const input &i) {}
```
5. Implement the output equation  "\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)")
and its differentiation \boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}} "\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}")
, \boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}} "\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}")
:
```
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) {}
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or covariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance
```
esekfom::esekf kf;
```
7. 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);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated(z, R); // measurement noise covariance: R
```
*Remarks:*
- 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.
5. Implement the output equation and its differentiation , :
```
measurement h_share(state &s, esekfom::share_datastruct &share_data)
{
if(share_data.converge) {} // this value is true means iteration is converged
share_data.valid = false; // the iteration stops before convergence when this value is false
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
}
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or convariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance:
```
esekfom::esekf kf;
```
7. Deliver the defined models, maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **limit** into the **esekf** object:
```
kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, limit);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_share();
```
## 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
```
2. 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
```
3. 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))
);
```
4. Implement the vector field and its differentiation , :
```
Eigen::Matrix f(state &s, const input &i) {}
Eigen::Matrix df_dx(state &s, const input &i) {} //notice S2 has length of 3 and dimension of 2
Eigen::Matrix df_dw(state &s, const input &i) {}
```
5. Implement the output equation and its differentiation , :
```
Eigen::Matrix 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) {}
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or cavariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance:
```
esekfom::esekf kf;
```
7. Deliver the defined models, maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **limit** into the **esekf** object:
```
kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, limit);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_dyn(z, R); // measurement noise covariance: R
```
*Remarks:*
- 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.
5. Implement the output equation and its differentiation , :
```
Eigen::Matrix h_dyn_share(state &s, esekfom::dyn_share_datastruct &dyn_share_data)
{
if(dyn_share_data.converge) {} // this value is true means iteration is converged
dyn_share_data.valid = false; // the iteration stops before convergence when this value is false
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
}
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or cavariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance:
```
esekfom::esekf kf;
```
7. Deliver the defined models, maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **limit** into the **esekf** object:
```
kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, limit);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_dyn_share();
```
## 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
```
2. 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
```
3. 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))
);
```
4. Implement the vector field and its differentiation , :
```
Eigen::Matrix f(state &s, const input &i) {}
Eigen::Matrix df_dx(state &s, const input &i) {} //notice S2 has length of 3 and dimension of 2
Eigen::Matrix df_dw(state &s, const input &i) {}
```
5. Implement the differentiation of the output equation , :
```
Eigen::Matrix dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false
Eigen::Matrix dh_dv(state &s, bool &valid) {}
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or covariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance:
```
esekfom::esekf kf;
```
7. Deliver the defined models, maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **limit** into the **esekf** object:
```
kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, limit);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. 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
```
then an iterated update is executed:
```
kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R
```
*Remarks:*
- 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.
5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
1) initial state or covariance:
```
state init_state;
esekfom::esekf::cov init_P;
esekfom::esekf kf(init_state,init_P);
```
2) default state and covariance:
```
esekfom::esekf kf;
```
6. Deliver the defined models, maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **limit** into the **esekf** object:
```
kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, limit);
```
7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
8. 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 &dyn_runtime_share_data)
{
if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged
dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false
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
}
```
then an iterated update is executed:
```
kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share);
```
## 5. 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](https://github.com/hku-mars/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
```
## 6.Acknowledgments
Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.