This is the code repository of SFUISE, the first online continuous-time UWB-inertial state estimation system in ROS. Quaternion-based cubic B-splines are exploited to represent states continuously over time with efficient solutions to time derivatives and spatial differentiations in closed form. The functional core of the system is a novel sliding-window spline fitting scheme that is equipped with a customized implementation of LM method.

The system supports UWB-inertial fusion for both ToA and TDoA principles of ultra-wideband ranging with minimized external dependencies. See below for usage of SFUISE using public and our own recorded data sets. Detailed information about the system can be found in our paper on arXiv or IEEE, and demonstrated on Youtube.

The work has been publlished on IEEE Robotics and Automation Letters (RA-L).

Thank you for citing our paper if you use any of this code:

@ARTICLE{RAL23_Li,
  author={Li, Kailai and Cao, Ziyu and Hanebeck, Uwe D.},
  journal={IEEE Robotics and Automation Letters}, 
  title={Continuous-Time Ultra-Wideband-Inertial Fusion}, 
  year={2023},
  volume={8},
  number={7},
  pages={4338-4345},
  doi={10.1109/LRA.2023.3281932}
}

System dependencies (tested on Ubuntu 20.04)

• ROS (tested with Noetic)
• Eigen (tested with Eigen 3.3.7)
• SuiteSparse

Compile with catkin_tools:

cd ~/catkin_ws/src
git clone https://github.com/KIT-ISAS/SFUISE
cd ..
catkin build sfuise

Run following commands in terminal

• Example for running sfuise on UTIL (TDoA-inertial):

# Change anchor_path in config_test_util.yaml
roslaunch sfuise sfuise_test_util.launch
rosbag play const1-trial1-tdoa2.bag

• Example for running sfuise on ISAS-Walk (ToA-inertial, own data set):

roslaunch sfuise sfuise_test_isas-walk1.launch
rosbag play ISAS-Walk1.bag

Kailai Li (Email: kailai.li@liu.se)

Ziyu Cao (Email: ziyu.cao@kit.edu)

We hereby recommend reading lie-spline-experiments for reference. The IMU integration was derived from VINS-Fusion.

The source code is released under GPLv3 license.

We are constantly working on improving our code. For any technical issues, please contact Kailai Li (kailai.li@liu.se).