# IKFoM **Repository Path**: Sytx_1/IKFoM ## Basic Information - **Project Name**: IKFoM - **Description**: No description available - **Primary Language**: Unknown - **License**: GPL-2.0 - **Default Branch**: main - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2025-04-28 - **Last Updated**: 2025-04-28 ## 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 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](https://github.com/Joanna-HE) **Our related video**: https://youtu.be/sz_ZlDkl6fA ## 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 that is defined as , and its differentiation , , where w=0 could be left out: ``` Eigen::Matrix f(state &s, const input &i) { Eigen::Matrix res = Eigen::Matrix::Zero(); res(0) = s.vel[0]; res(1) = s.vel[1]; res(2) = s.vel[2]; return res; } Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); return cov; } Eigen::Matrix df_dw(state &s, const input &i) { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); return cov; } ``` Those functions would be called during the ekf state predict 5. 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 dh_dx(state &s, bool &valid) {} Eigen::Matrix dh_dv(state &s, bool &valid) {} ``` Those functions would be called during the ekf state update 6. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) default state and covariance: ``` esekfom::esekf kf; ``` where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**. 7. 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); ``` 8. 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 ``` 9. 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. 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 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 6. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) default state and covariance: ``` esekfom::esekf kf; ``` 7. 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); ``` 8. 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 ``` 9. 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::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 ``` 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 that is defined as , and its differentiation , , where w=0 could be left out: ``` Eigen::Matrix f(state &s, const input &i) { Eigen::Matrix res = Eigen::Matrix::Zero(); res(0) = s.vel[0]; res(1) = s.vel[1]; res(2) = s.vel[2]; return res; } Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); return cov; } Eigen::Matrix df_dw(state &s, const input &i) { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); return cov; } ``` Those functions would be called during ekf state predict 5. Implement the output equation and its differentiation , : ``` Eigen::Matrix 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 h_; h_(0) = s.pos[0]; return h_; } Eigen::Matrix dh_dx(state &s, bool &valid) {} Eigen::Matrix dh_dv(state &s, bool &valid) {} ``` Those functions would be called during ekf state update 6. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) default state and covariance: ``` esekfom::esekf kf; ``` where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof** 7. 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); ``` 8. 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 ``` 9. 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. 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 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 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 ``` 6. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) default state and covariance: ``` esekfom::esekf kf; ``` 7. 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); ``` 8. 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 ``` 9. 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::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 ``` 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 that is defined as , and its differentiation , , where w=0 could be left out: ``` Eigen::Matrix f(state &s, const input &i) { Eigen::Matrix res = Eigen::Matrix::Zero(); res(0) = s.vel[0]; res(1) = s.vel[1]; res(2) = s.vel[2]; return res; } Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); return cov; } Eigen::Matrix df_dw(state &s, const input &i) { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); return cov; } ``` Those functions would be called during ekf state predict 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) {} ``` Those functions would be called during ekf state update 6. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) ``` esekfom::esekf kf; ``` Where **process_noise_dof** is the dimension of process noise, of type of std int 7. 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); ``` 8. 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 ``` 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 { 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. 5. 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::cov init_P; esekfom::esekf kf(init_state,init_P); ``` (2) default state and covariance: ``` esekfom::esekf kf; ``` 6. 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); ``` 7. 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 ``` 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 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::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](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 FAST_LIO cd .. catkin_make source devel/setup.bash ``` Livox Avia rosbag: Can be downloaded from [here](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/hdj65822_connect_hku_hk/Eu1wAc34q1ZDvHCwe2ajAK4BsH82-c3yTldI9Blp6_rivg?e=41sEEw) 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](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM). 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](https://github.com/TixiaoShan/LIO-SAM), 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.