Skip to content

IMU + X(GNSS, 6DoF Odom) Loosely-Coupled Fusion Localization based on ESKF, IEKF, UKF(UKF/SPKF, JUKF, SVD-UKF) and MAP

License

Notifications You must be signed in to change notification settings

cggos/imu_x_fusion

Repository files navigation

IMU + X Fusion

IMU + X Loosely-Coupled Fusion Localization


Overview

Features

  • Dataset

    • KAIST dataset: ROS publish
  • Sensors

    • IMU
      • Numerical Integration (TODO: RK4)
      • Pre-Integration
    • Wheel Odometer
    • Manometer
    • GPS
    • 6DoF Odom (Stereo Visual Odometry)
    • LiDAR
    • Multi-modal data
  • State Estimation

    • EKF
      • ESKF
      • IEKF
    • UKF
      • JUKF
      • SVD-UKF
    • Particle Filter
    • MAP
      • User-defined G-N
      • Ceres-Solver
      • G2O
      • GTSAM
  • Local / Global Angular Error (Rotation Perturbation)

    • Local Angular Error (OK)
    • Global Angular Error (TODO: why Poor)
    enum ANGULAR_ERROR { LOCAL_ANGULAR_ERROR, GLOBAL_ANGULAR_ERROR };
  • Debug

    • Check Measurement Jacobian Matrix

Requirements

tested on Ubuntu 16.04 and Ubuntu 18.04

  • Ubuntu 16.04 & ROS Kinetic

    • OpenCV 3
    • ROS package: nmea_navsat_driver
    • GeographicLib 1.50.1 (built from souce code, cmake 3.18.0 tested)
  • Ubuntu 18.04 & ROS Melodic

    • OpenCV 3
    • ROS package: nmea_navsat_driver
    • GeographicLib 1.49
      sudo apt install libgeographic-dev

Build

mkdir -p ws_msf/src
cd ws_msf/src
git clone xxx
cd ..
catkin_make -j4 # error happened when using the default cmake 3.5.1 on Ubuntu 16.04, upgrade it
# or
catkin build -j4

Run on Host

IMU + GNSS

test data: utbm_robocar_dataset_20180719_noimage.bag

  • [sensor_msgs/Imu] /imu/data: 100 hz
  • [nmea_msgs/Sentence] /nmea_sentence: 15 hz
  • [sensor_msgs/NavSatFix] /fix: 5 hz
  • [nav_msgs/Path] /nav_path: 63 hz
roslaunch imu_x_fusion imu_gnss_fusion.launch
rosbag play -s 25 utbm_robocar_dataset_20180719_noimage.bag

ROS graph and path on rviz:

plot the result path (fusion_gps.csv & fusion_state.csv) on Google Map using the scripts folium_csv.py:

IMU + 6DoF Odom

VO: ORB-SLAM2 (Stereo) + EuRoC V1_01_easy.bag

roslaunch imu_x_fusion imu_vo_fusion.launch [est:=ekf, ukf or map]
run ORB-SLAM2 (Stereo) and play back bag file
# https://github.com/cggos/orbslam2_cg
# pose cov:
# sigma_pv: 0.001
# sigma_rp: 0.5
# sigma_yaw: 0.5
roslaunch orbslam2_ros run_stereo_euroc.launch

rosbag play V1_01_easy.bag

results(Green path: estimated pose; Red path: pose of VO):

Use the recorded bag file directly

Download orbslam2_v101easy.bag

rosbag play orbslam2_v101easy.bag

VO: ORB-SLAM2 (Stereo) + MYNTEYE-S1030 Camera

# TODO: Test
roslaunch imu_x_fusion imu_vo_fusion_mynteye.launch

roslaunch mynt_eye_ros_wrapper mynteye.launch

Run with KAIST Dataset

rosrun imu_x_fusion kaist_pub /dev_sdb/datasets/KAIST/urban39-pankyo

Run with Docker

# pull from DockerHub
sudo docker pull cggos/ubuntu-ros-slam:bionic-melodic

# run, e.g.: imu_vo_fusion
./scripts/run_docker.sh

# modify the script for running others

Code Format

code format based on Google style

./batch_format.sh

Docs

  • Requirements

    sudo apt install graphviz
    sudo apt install doxygen
  • Generate

    # output in html and latex dir
    doxygen Doxygen.config

Community