gnss_imu_odom_ESKF

gnss imu odometry sensor fusion localization by ESKF(output NED pose)

• gnss imu sensor fusion localization by ESKF
• gnss imu odometry sensor fusion localization by ESKF

Environment

OS : Ubuntu MATE with Raspberry pi4(8GB)
ROS : noetic

Input Output(IO)

Input

• nmea messages /nmea_sentence(nmea_msgs/Sentence)
• gps messages /fix(sensor_msgs/NavSatFix)
• imu messages /imu/data(sensor_msgs/Imu)
• odom messages /odom(geometry_msgs/Pose)

Output

• estimatid_pose messages /estimatid_pose(geometry_msgs/Pose)
• estimatid_path messages /estimatid_path(nav_msgs/Path)

Class Diagram

classDiagram
    class IMU_Data {
        double timestamp
        Eigen::Vector3d acc
        Eigen::Vector3d gyro
    }
    class GPS_Data {
        double timestamp
        Eigen::Vector3d lla
        Eigen::Vector3d ned
    }
    class State {
        double timestamp
        Eigen::Vector3d position
        Eigen::Vector3d velocity
        Eigen::Quaterniond quaternion
        Eigen::Vector3d acc_bias
        Eigen::Vector3d gyro_bias
        Eigen::Vector3d gravity
        Eigen::Matrix<double, 18, 18> PEst
        Eigen::Matrix<double, 18, 1> error
    }
    class map_projection_reference {
        uint64_t timestamp
        double lat_rad
        double lon_rad
        double sin_lat
        double cos_lat
        bool init_done
    }
    class GEOGRAPHY {
        -map_projection_reference* ref
        +GEOGRAPHY()
        +~GEOGRAPHY()
        +int map_projection_init(map_projection_reference*, double, double)
        +int map_projection_project(map_projection_reference*, double, double, float*, float*)
        +double constrain(double, double, double)
        +bool lla2enu(double*, const double*, const double*)
        +bool lla2xyz(double*, const double*)
        +bool xyz2enu(double*, const double*, const double*)
        +void rot3d(double R[3][3], const double, const double)
        +void matrixMultiply(double C[3][3], const double A[3][3], const double B[3][3])
        +void matrixMultiply(double c[3], const double A[3][3], const double b[3])
        +void rot(double R[3][3], const double, const int)
    }
    class ESKF {
        -State x
        +ESKF()
        +~ESKF()
        +void Init(const GPS_Data&, State&)
        +void Predict(const IMU_Data&, State&)
        +void Correct(const GPS_Data&, State&)
        +void State_update(State&)
        +void Error_State_Reset(State&)
        +Eigen::Quaterniond kronecker_product(const Eigen::Quaterniond&, const Eigen::Quaterniond&)
        +Eigen::Matrix3d skewsym_matrix(const Eigen::Vector3d&)
        +Eigen::Quaterniond euler_to_quatertion(Eigen::Vector3d)
        +Eigen::Matrix<double, 18, 18> calcurate_Jacobian_Fx(Eigen::Vector3d, Eigen::Vector3d, Eigen::Matrix3d, const double)
        +Eigen::Matrix<double, 18, 12> calcurate_Jacobian_Fi()
        +Eigen::Matrix<double, 12, 12> calcurate_Jacobian_Qi(const double)
        +Eigen::Matrix<double, 3, 18> calcurate_Jacobian_H(State&)
        +Eigen::Quaterniond getQuaFromAA(Eigen::Vector3d)
        +Eigen::Matrix<double, 3, 3> getRotFromAA(Eigen::Vector3d)
    }
    class ROS_Interface {
        -ros::NodeHandle nh
        -bool init
        -ros::Publisher gps_path_pub
        -ros::Publisher estimated_path_pub
        -ros::Publisher estimated_pose_pub
        -ros::Subscriber gps_sub
        -ros::Subscriber imu_sub
        -tf::TransformBroadcaster odom_to_baselink_broadcaster
        -geometry_msgs::TransformStamped odom_to_baselink_trans
        -nav_msgs::Path gps_path
        -nav_msgs::Path estimated_path
        -nav_msgs::Odometry estimated_pose
        -State x
        -IMU_Data imu_data
        -GPS_Data gps_data
        -map_projection_reference map_ref
        -double lat0
        -double lon0
        -double alt0
        -ESKF eskf
        -GEOGRAPHY geography
        +ROS_Interface(ros::NodeHandle&, double, double)
        +~ROS_Interface()
        +void gps_callback(const sensor_msgs::NavSatFixConstPtr&)
        +void imu_callback(const sensor_msgs::ImuConstPtr&)
        +void data_conversion_imu(const sensor_msgs::ImuConstPtr&, IMU_Data&)
        +void data_conversion_gps(const sensor_msgs::NavSatFixConstPtr&, GPS_Data&)
    }
    ROS_Interface --|> ESKF: Uses
    ROS_Interface --|> GEOGRAPHY: Uses
    ROS_Interface --|> State: Uses
    ROS_Interface --|> IMU_Data: Uses
    ROS_Interface --|> GPS_Data: Uses
    ROS_Interface --|> map_projection_reference: Uses
    ESKF --|> State: Uses
    ESKF --|> IMU_Data: Uses
    ESKF --|> GPS_Data: Uses
    GEOGRAPHY --|> map_projection_reference: Uses

Loading

FlowChart

flowchart TD
    A[Start] --> B[Initialize ROS Node: eskf_localization]
    B --> C[Create ROS_Interface Object with Initial GPS Coordinates]
    C --> D[Set Up ROS Publishers and Subscribers]
    D --> E[Enter Main ROS Loop]
    E --> F[Check if ROS is OK]
    F -->|Yes| G[Wait for IMU and GPS Data]
    G -->|IMU Data Received| H[IMU Callback]
    G -->|GPS Data Received| I[GPS Callback]
    H --> J[Data Conversion: IMU]
    I --> K[Data Conversion: GPS]
    J --> L[Run ESKF Prediction]
    K --> M[Check Initialization Status]
    M -->|Not Initialized| N[Run ESKF Initialization]
    M -->|Initialized| O[Run ESKF Correction]
    O --> P[State Update and Error Reset]
    P --> Q[Publish Estimated Pose and Path]
    Q --> R[Publish Transform Data]
    R --> E
    N --> Q

    subgraph ROS_Interface
        S[Constructor: Initialize and Set Up ROS Interface]
        T[gps_callback: Process GPS Data]
        U[imu_callback: Process IMU Data]
        V[data_conversion_imu: Convert IMU Data]
        W[data_conversion_gps: Convert GPS Data]
    end

    subgraph ESKF
        X[Init: Initialize ESKF with GPS Data]
        Y[Predict: Predict State with IMU Data]
        Z[Correct: Correct State with GPS Data]
        AA[State_update: Update Estimated State]
        AB[Error_State_Reset: Reset Error State]
    end

    S --> T
    S --> U
    T --> W
    U --> V
    W --> Z
    V --> Y
    Y --> E
    Z --> AA
    AA --> AB
    AB --> Q

Loading

Software architecture

• gps trajectory plotter

• imu gnss sensor fusion

• odom imu gnss sensor fusion

nmea_navsat_driver

STEP1 Install nmea_msgs

cd catkin_ws/
cd src/
git clone https://github.com/ros-drivers/nmea_msgs
catkin_make
source ~/catkin_ws/devel/setup.bash

STEP2 Install nmea_navsat_driver

cd catkin_ws/
cd src/
git clone https://github.com/ros-drivers/nmea_navsat_driver
catkin_make
source ~/catkin_ws/devel/setup.bash

STEP3 Install rosbag file

https://epan-utbm.github.io/utbm_robocar_dataset/

utbm_robocar_dataset_20190131_noimage.bag (1.5 GB)

2019-01-31 (Fri, snow) 08:54-09:10 (15'59") 1 × Velodyne / ibeo / SICK / IMU / GPS / Bumblebee XB3 / fisheye

STEP4 rosbag play

Terminal1

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF eskf_localization.launch

Terminal 2

rosbag play --clock Downloads/utbm_robocar_dataset_20190131_noimage.bag

Terminal3

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
rostopic echo /nmea_sentence

Terminal4

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
rostopic echo /fix

gps_trajectory_plotter

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF gps_trajectory_plotter.launch 

result in rviz

eskf_localization

gnss imu sensor fusion

• gnss imu sensor fusion localization by ESKF

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF imu_gnss_eskf_localization.launch

result in rviz

• the green path is made by raw GPS
• the blue path is made by ESKF

gnss imu odom sensor fusion

• gnss imu odom sensor fusion localization by ESKF

cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF odom_imu_gnss_eskf_localization.launch

result in rviz

• the green path is made by raw GPS
• the red path is made by raw Odometry
• the blue path is made by ESKF

problem section with outlier

Todo outlier fix