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
OS : Ubuntu MATE with Raspberry pi4(8GB)
ROS : noetic
- 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)
- estimatid_pose messages /estimatid_pose(geometry_msgs/Pose)
- estimatid_path messages /estimatid_path(nav_msgs/Path)
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
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
cd catkin_ws/
cd src/
git clone https://github.com/ros-drivers/nmea_msgs
catkin_make
source ~/catkin_ws/devel/setup.bash
cd catkin_ws/
cd src/
git clone https://github.com/ros-drivers/nmea_navsat_driver
catkin_make
source ~/catkin_ws/devel/setup.bash
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
cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF eskf_localization.launch
rosbag play --clock Downloads/utbm_robocar_dataset_20190131_noimage.bag
cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
rostopic echo /nmea_sentence
cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
rostopic echo /fix
cd catkin_ws/
source ~/catkin_ws/devel/setup.bash
roslaunch gnss_imu_odom_ESKF gps_trajectory_plotter.launch
- 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
- the green path is made by raw GPS
- the blue path is made by ESKF
- 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
- the green path is made by raw GPS
- the red path is made by raw Odometry
- the blue path is made by ESKF