├── rviz.png ├── kf_gins_ros ├── launch │ └── gins_rviz.launch ├── gins │ ├── INSMechan.h │ ├── basictype.h │ ├── visualization.h │ ├── parameters.h │ ├── gins_engine.h │ ├── parameters.cpp │ ├── rotation.h │ ├── visualization.cpp │ ├── INSMechan.cpp │ ├── earth.h │ └── gins_engine.cpp ├── CMakeLists.txt ├── config │ ├── gins.yaml │ └── gins_rviz_config.rviz ├── package.xml └── gins.cpp ├── data2bag └── src │ ├── CMakeLists.txt │ ├── package.xml │ └── data2bag.cpp └── README.md /rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slender1031/kf-gins-ros/HEAD/rviz.png -------------------------------------------------------------------------------- /kf_gins_ros/launch/gins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/INSMechan.h: -------------------------------------------------------------------------------- 1 | #ifndef INSMECHAN_H_ 2 | #define INSMECHAN_H_ 3 | 4 | #include "basictype.h" 5 | #include "rotation.h" 6 | 7 | class INSMechan 8 | { 9 | public: 10 | 11 | static void insMechan(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur); 12 | 13 | static void VelocityUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur); 14 | static void PositionUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur); 15 | static void AttitudeUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur); 16 | 17 | }; 18 | 19 | 20 | 21 | 22 | #endif // !INSMECHAN_H_ 23 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/basictype.h: -------------------------------------------------------------------------------- 1 | #ifndef BASICTYPE_H_ 2 | #define BASICTYPE_H_ 3 | 4 | 5 | #include 6 | 7 | using Eigen::Matrix3d; 8 | using Eigen::Quaterniond; 9 | using Eigen::Vector3d; 10 | 11 | 12 | struct PVA 13 | { 14 | double time; 15 | Vector3d blh; 16 | Vector3d vel; 17 | Quaterniond qnb; 18 | }; 19 | 20 | struct GNSS { 21 | double time; 22 | Vector3d blh; 23 | Matrix3d poscov; 24 | 25 | bool isvalid; 26 | } ; 27 | 28 | struct IMU 29 | { 30 | double time; 31 | double dt; 32 | Vector3d acc; 33 | Vector3d gyro; 34 | Vector3d dvel; 35 | Vector3d dtheta; 36 | } ; 37 | 38 | 39 | #endif //BASICTYPE_H_ 40 | 41 | -------------------------------------------------------------------------------- /data2bag/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0 FATAL_ERROR) 2 | project(data_convert) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ") 7 | 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | rosbag 11 | roscpp 12 | std_msgs 13 | cv_bridge 14 | image_transport 15 | sensor_msgs 16 | ) 17 | 18 | 19 | catkin_package( 20 | ) 21 | 22 | 23 | include_directories( 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | 28 | add_executable(data_convert_node data2bag.cpp) 29 | add_dependencies(data_convert_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 30 | 31 | target_link_libraries(data_convert_node 32 | ${catkin_LIBRARIES} 33 | ) 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /kf_gins_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0 FATAL_ERROR) 2 | project(gins) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_STANDARD 14) 6 | #-DEIGEN_USE_MKL_ALL") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | cv_bridge 14 | image_transport 15 | sensor_msgs 16 | ) 17 | 18 | find_package(OpenCV REQUIRED) 19 | find_package(Eigen3) 20 | include_directories( 21 | gins 22 | ${catkin_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIR} 24 | ) 25 | 26 | catkin_package() 27 | 28 | add_library(gins_lib 29 | gins/gins_engine.cpp 30 | gins/INSMechan.cpp 31 | gins/parameters.cpp 32 | gins/visualization.cpp 33 | ) 34 | target_link_libraries(gins_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ) 35 | 36 | 37 | add_executable(gins_node gins.cpp) 38 | target_link_libraries(gins_node gins_lib) 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef VISUALIZATION_H_ 2 | #define VISUALIZATION_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include "gins_engine.h" 22 | 23 | 24 | extern ros::Publisher pub_ins_odometry; 25 | extern ros::Publisher pub_ins_path; 26 | 27 | extern ros::Publisher pub_gins_blh; 28 | extern ros::Publisher pub_gins_ned; 29 | 30 | void pubINSMech(const GINSEngine &gins_engine, const std_msgs::Header &header); 31 | void pubGINS(const GINSEngine &gins_engine, const std_msgs::Header &header); 32 | 33 | 34 | #endif -------------------------------------------------------------------------------- /kf_gins_ros/gins/parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETERS_H_ 2 | #define PARAMETERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using Eigen::Vector3d; 14 | 15 | 16 | extern string IMU_TOPIC; 17 | extern string GNSS_TOPIC; 18 | 19 | extern Vector3d GYR_BIAS, GYR_BIAS_STD; 20 | extern Vector3d ACC_BIAS, ACC_BIAS_STD; 21 | 22 | extern Vector3d GYR_SCALE, GYR_SCALE_STD; 23 | extern Vector3d ACC_SCALE, ACC_SCALE_STD; 24 | 25 | 26 | extern Vector3d INITPOS; 27 | extern Vector3d INITVEL; 28 | extern Vector3d INITATT; 29 | 30 | extern Vector3d POS_STD; 31 | extern Vector3d VEL_STD; 32 | extern Vector3d ATT_STD; 33 | 34 | extern Vector3d ACC_W, ACC_N; 35 | extern Vector3d GYR_W, GYR_N; 36 | extern double CORRTIME; 37 | 38 | extern Vector3d ANTLEVER; 39 | extern std::string GINS_RESULT_PATH; 40 | 41 | void readParameters(std::string config_file); 42 | 43 | 44 | #endif // ! PARAMETERS_H_ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kf-gins-ros 2 | kf-gins-ros 3 | 4 | The main works are as follows: 5 | 1. Convert the datasets to rosbag 6 | 2. A ros version of kf-gins (based on C++) 7 | 8 | ### Configuration 9 | * Ubuntu18.04 10 | * ros-melodic 11 | * Eigen3 12 | 13 | 14 | ### Program Compilation and Execution 15 | 16 | **Attention:** 17 | you need to check File **PATH** and ROS **Topics**. 18 | 19 | Execute the following commands to compile the project: 20 | ```shell 21 | cd ~/gins_ws/src 22 | git clone https://github.com/slender1031/kf-gins-ros.git 23 | cd ../ 24 | catkin_make 25 | ``` 26 | 27 | Run commands: 28 | ```shell 29 | source devel/setup.bash 30 | rosrun data_convert data_convert_node 31 | rosrun gins gins_node [path to YAML] 32 | ``` 33 | 34 | Rviz for visualization: 35 | ```shell 36 | roslaunch gins gins_rviz.launch 37 | ``` 38 | 39 | 40 | 41 | 42 | Thanks for the team of Prof. Xiaoji Niu of the Integrated and Intelligent Navigation (i2Nav) group from GNSS Research Center of Wuhan University for providing the open-source KF-GINS software. 43 | 44 | 感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台 45 | (https://github.com/i2Nav-WHU/KF-GINS) 46 | 47 | Thanks for the github user **zzzzyp-sgg** who open-source the tools for data format conversion. 48 | (https://github.com/zzzzyp-sgg/SLAM-Tool) 49 | -------------------------------------------------------------------------------- /kf_gins_ros/config/gins.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # KF-GINS configuration file 4 | imu_topic: "/imu/data" 5 | gnss_topic: "/gnss" 6 | output_path: "/home/slender/torch_ws/GINS_ws/src/traj/gins_result.txt" 7 | 8 | # IMU零偏和比例因子, IMU的三个轴(前、右、下) 9 | gyrbias: [ 0, 0, 0 ] # [deg/h] 10 | accbias: [ 0, 0, 0 ] # [mGal] 11 | gyrscale: [ 0, 0, 0 ] # [ppm] 12 | accscale: [ 0, 0, 0 ] # [ppm] 13 | 14 | # 初始状态 15 | initpos: [ 30.4447873701, 114.4718632047, 20.899 ] # 位置, 大地坐标系BLH [deg, deg, m] 16 | initvel: [ 0.0, 0.0, 0.0 ] # 速度, 导航坐标系NED速度 [m/s, m/s, m/s] 17 | initatt: [0.85521958, -2.03747499 , 185.69847184] # 姿态欧拉角 [deg] 18 | 19 | 20 | # 初始状态标准差 21 | initposstd: [ 0.005, 0.004, 0.008 ] # 位置, 导航坐标系下 北向, 东向和垂向 [m, m, m] 22 | initvelstd: [ 0.003, 0.004, 0.004 ] # 速度, 导航坐标系下北向、东向和垂向速度 [m/s, m/s, m/s] 23 | initattstd: [ 0.003, 0.003, 0.023 ] # 姿态, 横滚、俯仰、航向角标准差 [deg, deg, deg] 24 | 25 | 26 | # IMU噪声建模参数, IMU的三个轴 27 | # 零偏随机游走 28 | arw: [0.003, 0.003, 0.003] # [deg/sqrt(hr)] 29 | vrw: [0.03, 0.03, 0.03] # [m/s/sqrt(hr)] 30 | # bias标准差 31 | gbstd: [0.027, 0.027, 0.027] # [deg/hr] 32 | abstd: [15.0, 15.0, 15.0] # [mGal] 33 | # 比例因子标准差 34 | gsstd: [300.0, 300.0, 300.0] # [ppm] 35 | asstd: [300.0, 300.0, 300.0] # [ppm] 36 | corrtime: 4.0 # [hr] 37 | 38 | # 天线杆臂, IMU坐标系前右下方向 39 | antlever: [ 0.136, -0.301, -0.184 ] # [m] 40 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/gins_engine.h: -------------------------------------------------------------------------------- 1 | #ifndef GINS_ENGINE_H_ 2 | #define GINS_ENGINE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "INSMechan.h" 10 | #include "earth.h" 11 | #include "parameters.h" 12 | 13 | 14 | 15 | class GINSEngine 16 | { 17 | public: 18 | GINSEngine(); 19 | ~GINSEngine()=default; 20 | 21 | void processIMU(double dt, sensor_msgs::ImuConstPtr &imu_msg); 22 | void processGNSS(sensor_msgs::NavSatFixConstPtr &gnss_msg); 23 | 24 | void INSInitialization(double timestamp); 25 | void INSPropagation(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur); //imu递推 26 | void IMUInterpolate(const double timestamp, const sensor_msgs::ImuConstPtr imu_data); 27 | 28 | void EKFPredict(Eigen::MatrixXd &Phi, Eigen::MatrixXd &Qd); 29 | void EKFUpdate(Eigen::MatrixXd &dz, Eigen::MatrixXd &H, Eigen::MatrixXd &R); 30 | void GNSSUpdate(GNSS &gnssdata); 31 | void StateFeedback(); 32 | 33 | 34 | //imu相邻两个历元的输出 35 | IMU imupre; 36 | IMU imucur; 37 | 38 | //gnss定位历元结果 39 | GNSS gnssdata; 40 | 41 | //两个历元的递推结果 42 | PVA pvapre; 43 | PVA pvacur; 44 | 45 | //imu Bias和尺度误差 46 | Vector3d gyrbias; 47 | Vector3d accbias; 48 | Vector3d gyrscale; 49 | Vector3d accscale; 50 | 51 | // Kalman滤波相关 52 | Eigen::MatrixXd Cov_; 53 | Eigen::MatrixXd Qc_; //系统状态噪声方差阵 54 | Eigen::MatrixXd dx_; // 55 | 56 | 57 | }; 58 | 59 | #endif // !GINS_ENGINE_H_ 60 | 61 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | string IMU_TOPIC, GNSS_TOPIC; 4 | Vector3d GYR_BIAS, GYR_BIAS_STD; 5 | Vector3d ACC_BIAS, ACC_BIAS_STD; 6 | Vector3d GYR_SCALE, GYR_SCALE_STD; 7 | Vector3d ACC_SCALE, ACC_SCALE_STD; 8 | 9 | Vector3d INITPOS, INITVEL, INITATT; 10 | Vector3d POS_STD, VEL_STD, ATT_STD; 11 | 12 | Vector3d ACC_W, ACC_N, GYR_W, GYR_N; 13 | double CORRTIME; 14 | Vector3d ANTLEVER; 15 | std::string GINS_RESULT_PATH; 16 | 17 | void readParameters(std::string config_file) 18 | { 19 | FILE *fh = fopen(config_file.c_str(),"r"); 20 | if(fh == NULL){ 21 | ROS_WARN("config_file dosen't exist; wrong config_file path"); 22 | ROS_BREAK(); 23 | return; 24 | } 25 | fclose(fh); 26 | 27 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 28 | if(!fsSettings.isOpened()) 29 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 30 | 31 | fsSettings["imu_topic"]>>IMU_TOPIC; 32 | fsSettings["gnss_topic"]>>GNSS_TOPIC; 33 | 34 | fsSettings["output_path"] >> GINS_RESULT_PATH; 35 | std::ofstream fout(GINS_RESULT_PATH, std::ios::out); 36 | fout.close(); 37 | 38 | cv::Vec3d cv_T; 39 | fsSettings["gyrbias"]>>cv_T; cv::cv2eigen(cv_T, GYR_BIAS); 40 | fsSettings["accbias"]>>cv_T; cv::cv2eigen(cv_T, ACC_BIAS); 41 | fsSettings["gyrscale"]>>cv_T; cv::cv2eigen(cv_T, GYR_SCALE); 42 | fsSettings["accscale"]>>cv_T; cv::cv2eigen(cv_T, ACC_SCALE); 43 | 44 | 45 | fsSettings["initpos"]>>cv_T; cv::cv2eigen(cv_T, INITPOS); 46 | fsSettings["initvel"]>>cv_T; cv::cv2eigen(cv_T, INITVEL); 47 | fsSettings["initatt"]>>cv_T; cv::cv2eigen(cv_T, INITATT); 48 | 49 | 50 | fsSettings["initposstd"]>>cv_T; cv::cv2eigen(cv_T, POS_STD); 51 | fsSettings["initvelstd"]>>cv_T; cv::cv2eigen(cv_T, VEL_STD); 52 | fsSettings["initattstd"]>>cv_T; cv::cv2eigen(cv_T, ATT_STD); 53 | 54 | fsSettings["arw"]>>cv_T; cv::cv2eigen(cv_T, ACC_W); 55 | fsSettings["vrw"]>>cv_T; cv::cv2eigen(cv_T, GYR_W); 56 | 57 | fsSettings["gbstd"]>>cv_T; cv::cv2eigen(cv_T, GYR_BIAS_STD); 58 | fsSettings["abstd"]>>cv_T; cv::cv2eigen(cv_T, ACC_BIAS_STD); 59 | 60 | fsSettings["gsstd"]>>cv_T; cv::cv2eigen(cv_T, GYR_SCALE_STD); 61 | fsSettings["asstd"]>>cv_T; cv::cv2eigen(cv_T, ACC_SCALE_STD); 62 | CORRTIME=fsSettings["corrtime"]; 63 | 64 | fsSettings["antlever"]>>cv_T; cv::cv2eigen(cv_T, ANTLEVER); 65 | 66 | fsSettings.release(); 67 | 68 | 69 | } -------------------------------------------------------------------------------- /kf_gins_ros/gins/rotation.h: -------------------------------------------------------------------------------- 1 | #ifndef ROTATION_H_ 2 | #define ROTATION_H_ 3 | 4 | #include 5 | 6 | using Eigen::Matrix3d; 7 | using Eigen::Quaterniond; 8 | using Eigen::Vector3d; 9 | 10 | class Rotation 11 | { 12 | public: 13 | static Matrix3d skewSymmetric(const Vector3d &vector) 14 | { 15 | Matrix3d mat; 16 | mat << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), vector(0), 0; 17 | return mat; 18 | } 19 | 20 | static Vector3d Quaternion2Vector(const Quaterniond &quaternion) 21 | { 22 | Eigen::AngleAxisd axisd(quaternion); 23 | return axisd.angle() * axisd.axis(); 24 | } 25 | 26 | static Quaterniond Vector2Quaternion(const Vector3d &rotvec) 27 | { 28 | double angle = rotvec.norm(); 29 | Vector3d vec = rotvec.normalized(); 30 | return Quaterniond(Eigen::AngleAxisd(angle, vec)); 31 | } 32 | 33 | static Quaterniond euler2quaternion(const Vector3d &euler) 34 | { 35 | return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) * 36 | Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) * 37 | Eigen::AngleAxisd(euler[0], Vector3d::UnitX())); 38 | } 39 | 40 | 41 | // ZYX旋转顺序, 前右下的IMU, 输出RPY 42 | static Vector3d matrix2euler(const Eigen::Matrix3d &dcm) 43 | { 44 | Vector3d euler; 45 | 46 | euler[1] = atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2))); 47 | 48 | if (dcm(2, 0) <= -0.999) { 49 | euler[0] = 0; 50 | euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1))); 51 | //std::cout << "[WARNING] Rotation::matrix2euler: Singular Euler Angle! Set the roll angle to 0!" << std::endl; 52 | } else if (dcm(2, 0) >= 0.999) { 53 | euler[0] = 0; 54 | euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1))); 55 | //std::cout << "[WARNING] Rotation::matrix2euler: Singular Euler Angle! Set the roll angle to 0!" << std::endl; 56 | } else { 57 | euler[0] = atan2(dcm(2, 1), dcm(2, 2)); 58 | euler[2] = atan2(dcm(1, 0), dcm(0, 0)); 59 | } 60 | 61 | // heading 0~2PI 62 | if (euler[2] < 0) { 63 | euler[2] = M_PI * 2 + euler[2]; 64 | } 65 | 66 | return euler; 67 | } 68 | 69 | static Vector3d quaternion2euler(const Quaterniond &quaternion) 70 | { 71 | return matrix2euler(quaternion.toRotationMatrix()); 72 | } 73 | 74 | }; 75 | 76 | 77 | 78 | #endif // !ROTATION_H_ 79 | 80 | -------------------------------------------------------------------------------- /kf_gins_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gins 4 | 0.0.0 5 | The pic_pkg package 6 | 7 | 8 | 9 | 10 | slender 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | image_transport 55 | camera_models 56 | 57 | roscpp 58 | image_transport 59 | camera_models 60 | cv_bridge 61 | sensor_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /data2bag/src/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_convert 4 | 0.0.0 5 | The pic_pkg package 6 | 7 | 8 | 9 | 10 | slender 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | image_transport 55 | camera_models 56 | 57 | roscpp 58 | image_transport 59 | camera_models 60 | cv_bridge 61 | sensor_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /data2bag/src/data2bag.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | double dt=1.0/200; //imu数据采样间隔 14 | 15 | /* imu数据转bag，bag包以IMU数据为基础(因为通常imu数据是时间最长的) */ 16 | void imu2bag(rosbag::Bag &bag, const std::string imuFile, const std::string outBag, int gpsWeek) 17 | { 18 | std::ifstream file(imuFile); 19 | if (!file.is_open()) 20 | { 21 | ROS_ERROR_STREAM("Failed to open file!"); 22 | return; 23 | } 24 | 25 | bag.open(outBag, rosbag::bagmode::Write); 26 | std::string line; 27 | 28 | while (std::getline(file, line)) 29 | { 30 | // 将每行数据分割为各个字段 31 | std::istringstream iss(line); 32 | double time, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z; 33 | if (!(iss >> time >> gyro_x >> gyro_y >> gyro_z >> accel_x >> accel_y >> accel_z)) 34 | { 35 | ROS_WARN_STREAM("Failed to parse line: " << line); 36 | continue; 37 | } 38 | 39 | // 创建IMU消息 40 | sensor_msgs::Imu imu_msg; 41 | // 315964800是GPS起始时间和计算机起始时间的一个固定差值 42 | time = time + 315964800 + 604800 * gpsWeek - 8 * 3600; 43 | imu_msg.header.stamp = ros::Time(time); 44 | imu_msg.angular_velocity.x = gyro_x/dt; 45 | imu_msg.angular_velocity.y = gyro_y/dt; 46 | imu_msg.angular_velocity.z = gyro_z/dt; 47 | imu_msg.linear_acceleration.x = accel_x/dt; 48 | imu_msg.linear_acceleration.y = accel_y/dt; 49 | imu_msg.linear_acceleration.z = accel_z/dt; 50 | 51 | // 写入ROSbag文件 52 | bag.write("/imu/data", ros::Time(time), imu_msg); 53 | } 54 | 55 | bag.close(); 56 | file.close(); 57 | std::cout << "imu data convert finished!" << std::endl; 58 | } 59 | 60 | 61 | /* gnss数据转bag */ 62 | void gnss2bag(rosbag::Bag &bag, const std::string gnssFile, const std::string outBag, int gpsWeek) 63 | { 64 | std::ifstream file(gnssFile); 65 | if (!file.is_open()) 66 | { 67 | ROS_ERROR_STREAM("Failed to open file!"); 68 | return; 69 | } 70 | 71 | bag.open(outBag, rosbag::bagmode::Append); 72 | 73 | std::string line; 74 | while (std::getline(file, line)) 75 | { 76 | std::istringstream iss(line); 77 | double time, lat, lon, h, vn, ve, vd; 78 | if (!(iss >> time >> lat >>lon >>h >>vn >>ve >>vd)) 79 | { 80 | ROS_WARN_STREAM("Failed to parse line: " << line); 81 | continue; 82 | } 83 | 84 | // 创建gnss消息 85 | sensor_msgs::NavSatFix gnss_msg; 86 | // 315964800是GPS起始时间和计算机起始时间的一个固定差值 87 | time = time + 315964800 + 604800 * gpsWeek - 8 * 3600; 88 | gnss_msg.header.stamp = ros::Time(time); 89 | gnss_msg.latitude=lat; 90 | gnss_msg.longitude=lon; 91 | gnss_msg.altitude=h; 92 | 93 | gnss_msg.position_covariance[0]=0.005*0.005; 94 | gnss_msg.position_covariance[3]=0.004*0.004; 95 | gnss_msg.position_covariance[6]=0.008*0.008; 96 | 97 | // 写入ROSbag文件 98 | bag.write("/gnss", ros::Time(time), gnss_msg); 99 | } 100 | 101 | bag.close(); 102 | file.close(); 103 | std::cout << "gnss data convert finished!" << std::endl; 104 | } 105 | 106 | 107 | int main(int argc, char **argv) 108 | { 109 | ros::init(argc, argv, "data_to_rosbag"); 110 | ros::NodeHandle nh; 111 | 112 | // 创建rosbag文件 113 | rosbag::Bag bag; 114 | int gpsWeek = 2017; 115 | std::string imuFile = "src/data2bag/Leador-A15.txt"; //imu数据文件 116 | std::string gnssFile = "src/data2bag/GNSS-RTK.txt"; //gnss数据文件 117 | 118 | std::string outBag ="src/data2bag/output.bag"; //生成的结果文件 119 | 120 | imu2bag(bag, imuFile, outBag, gpsWeek); // imu转bag 121 | gnss2bag(bag, gnssFile, outBag, gpsWeek); // gnss转bag 122 | 123 | 124 | return 0; 125 | } -------------------------------------------------------------------------------- /kf_gins_ros/gins/visualization.cpp: -------------------------------------------------------------------------------- 1 | #include "visualization.h" 2 | 3 | ros::Publisher pub_ins_odometry, pub_ins_path; 4 | ros::Publisher pub_gins_blh, pub_gins_ned; 5 | 6 | nav_msgs::Path gins_ned_path; 7 | nav_msgs::Path ins_blh_path; 8 | 9 | bool first_gins=false; 10 | Vector3d ned_first; 11 | 12 | void pubINSMech(const GINSEngine &gins_engine, const std_msgs::Header &header) 13 | { 14 | nav_msgs::Odometry odometry; 15 | odometry.header=header; 16 | odometry.header.frame_id="world"; 17 | 18 | Vector3d P=gins_engine.pvacur.blh; 19 | odometry.pose.pose.position.x=P[0]; 20 | odometry.pose.pose.position.y=P[1]; 21 | odometry.pose.pose.position.z=P[2]; 22 | 23 | Vector3d V=gins_engine.pvacur.vel; 24 | 25 | Quaterniond Q=gins_engine.pvacur.qnb; 26 | odometry.pose.pose.orientation.x=Q.x(); 27 | odometry.pose.pose.orientation.y=Q.y(); 28 | odometry.pose.pose.orientation.z=Q.z(); 29 | odometry.pose.pose.orientation.w=Q.w(); 30 | pub_ins_odometry.publish(odometry); 31 | 32 | 33 | Vector3d vec=Q.toRotationMatrix().eulerAngles(2,1,0)*180/M_PI; // [yaw, pitch, roll] 34 | Vector3d euler (vec[2], vec[1], vec[0]); 35 | 36 | //Vector3d vec=quaternion2euler(Q)*180/M_PI; 37 | 38 | printf("IMU time: %f, t: %f %f %f q: %f %f %f \n", header.stamp.toSec(), P.x()*180/M_PI, P.y()*180/M_PI, P.z(), euler(0), euler(1), euler(2)); 39 | 40 | geometry_msgs::PoseStamped pose_stamped; 41 | pose_stamped.header = header; 42 | pose_stamped.header.frame_id = "world"; 43 | pose_stamped.pose = odometry.pose.pose; 44 | ins_blh_path.header = header; 45 | ins_blh_path.header.frame_id = "world"; 46 | ins_blh_path.poses.push_back(pose_stamped); 47 | pub_ins_path.publish(ins_blh_path); 48 | 49 | // write result to file 50 | ofstream foutC(GINS_RESULT_PATH, ios::app); 51 | foutC.setf(ios::fixed, ios::floatfield); 52 | foutC.precision(0); 53 | foutC << header.stamp.toSec() * 1e9 << " "; 54 | foutC.precision(5); 55 | foutC << P.x() << " "<< P.y() << " "<< P.z() << " " 56 | << V.x() << " "<< V.y() << " "<< V.z() << " " 57 | << Q.w() << " " << Q.x() << " " << Q.y() << " " << Q.z() << endl; 58 | foutC.close(); 59 | } 60 | 61 | void pubGINS(const GINSEngine &gins_engine, const std_msgs::Header &header) 62 | { 63 | sensor_msgs::NavSatFix gps_position; 64 | gps_position.header=header; 65 | gps_position.header.frame_id = "world"; 66 | 67 | Vector3d blh = gins_engine.pvacur.blh; 68 | gps_position.latitude = blh[0]; 69 | gps_position.longitude = blh[1]; 70 | gps_position.altitude = blh[2]; 71 | for(int i=0; i<9; i++){ 72 | gps_position.position_covariance[i]=gins_engine.Cov_(i/3, i%3); 73 | } 74 | 75 | pub_gins_blh.publish(gps_position); //发布更新后的BLH坐标 76 | 77 | //BLH转NED 78 | if(!first_gins){ 79 | first_gins=true; 80 | ned_first=blh; 81 | } 82 | 83 | geometry_msgs::PoseStamped pose_stamped; 84 | pose_stamped.header = header; 85 | pose_stamped.header.frame_id = "world"; 86 | 87 | Vector3d ned=Earth::global2local(ned_first, blh); 88 | pose_stamped.pose.position.x=ned(0); 89 | pose_stamped.pose.position.y=ned(1); 90 | pose_stamped.pose.position.z=ned(2); 91 | 92 | Quaterniond Q=gins_engine.pvacur.qnb; 93 | pose_stamped.pose.orientation.x=Q.x(); 94 | pose_stamped.pose.orientation.y=Q.y(); 95 | pose_stamped.pose.orientation.z=Q.z(); 96 | pose_stamped.pose.orientation.w=Q.w(); 97 | 98 | Vector3d vec=Q.toRotationMatrix().eulerAngles(2,1,0)*180/M_PI; // [yaw, pitch, roll] 99 | Vector3d euler (vec[2], vec[1], vec[0]); 100 | 101 | //Vector3d vec=quaternion2euler(Q)*180/M_PI; 102 | 103 | printf("GINS time: %f, t: %f %f %f q: %f %f %f \n", header.stamp.toSec(), blh[0]*180/M_PI, blh[1]*180/M_PI, blh[2], euler(0), euler(1), euler(2)); 104 | 105 | gins_ned_path.header = header; 106 | gins_ned_path.header.frame_id = "world"; 107 | gins_ned_path.poses.push_back(pose_stamped); 108 | pub_gins_ned.publish(gins_ned_path); //发布更新后的ned坐标 109 | } -------------------------------------------------------------------------------- /kf_gins_ros/gins/INSMechan.cpp: -------------------------------------------------------------------------------- 1 | #include "INSMechan.h" 2 | #include "earth.h" 3 | 4 | void INSMechan::insMechan(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur) 5 | { 6 | 7 | VelocityUpdate(imupre, imucur, pvapre, pvacur); 8 | PositionUpdate(imupre, imucur, pvapre, pvacur); 9 | AttitudeUpdate(imupre, imucur, pvapre, pvacur); 10 | } 11 | 12 | 13 | 14 | //导航系NED速度推导 15 | void INSMechan::VelocityUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur) 16 | { 17 | double RM, RN; 18 | Vector3d w_n_ie, w_n_en; 19 | RM=Earth::RM(pvapre.blh[0]); 20 | RN=Earth::RN(pvapre.blh[0]); 21 | w_n_ie=Earth::w_n_ie(pvapre.blh[0]); 22 | w_n_en=Earth::w_n_en(pvapre.blh, pvapre.vel); 23 | 24 | // v_nk=v_nk-1+delta_v_nf+delta_v_ng 25 | //三项分别对应速度积分初值、比力f积分项、重力g积分项 26 | 27 | //1、计算比力积分项delta_v_nf 28 | //旋转补偿项(rotation compensation term) 29 | //划桨效应补偿项(sculling compensation term) 30 | Vector3d RotationTerm=imucur.dtheta.cross(imucur.dvel)/2; 31 | Vector3d ScullingTerm=(imupre.dtheta.cross(imucur.dvel)+imupre.dvel.cross(imucur.dtheta))/12; 32 | Vector3d d_vbf=imucur.dvel+RotationTerm+ScullingTerm; 33 | 34 | //b系转n系 35 | Vector3d zeta=(w_n_ie+w_n_en)*imucur.dt/2; 36 | Matrix3d Cnb=pvapre.qnb.matrix(); 37 | Matrix3d Cnn=Matrix3d::Identity()-0.5*Rotation::skewSymmetric(zeta); 38 | Vector3d d_vnf=Cnn*Cnb*d_vbf; 39 | 40 | 41 | //2、计算重力积分项delta_v_ng（缓慢变化项，取tk-1/2，中间时刻） 42 | double gravity=Earth::gravity(pvapre.blh); 43 | Vector3d gnl(0,0,gravity); 44 | Vector3d d_vng=(gnl-(2*w_n_ie+w_n_en).cross(pvapre.vel))*imucur.dt; 45 | 46 | //3、计算中间时刻的位置和速度 47 | Vector3d midvel=pvapre.vel+(d_vnf+d_vng)/2; 48 | 49 | //计算中间时刻的位置（因为位置还没更新，所以还没有当前时刻的位置，只能通过外推） 50 | Quaterniond qnn=Rotation::Vector2Quaternion(zeta); 51 | Vector3d zeta2(0, 0, -WGS84_WIE * imucur.dt / 2); 52 | Quaterniond qee=Rotation::Vector2Quaternion(zeta2); 53 | Quaterniond qen=Earth::qen(pvapre.blh); 54 | qen=qee*qen*qnn; 55 | 56 | Vector3d midpos; 57 | midpos[2]=pvapre.blh[2] - midpos[2]*imucur.dt/2; 58 | midpos = Earth::blh(qen, midpos[2]); 59 | 60 | //4、重新计算速度各项分量 61 | // 重新计算中间时刻tk-1/2的wie_e, wen_n 62 | w_n_ie=Earth::w_n_ie(midpos[0]); 63 | w_n_en=Earth::w_n_en(midpos, midvel); 64 | 65 | // 重新计算n系比力积分项 66 | zeta=(w_n_ie+w_n_en)*imucur.dt/2; 67 | Cnn=Matrix3d::Identity()-0.5*Rotation::skewSymmetric(zeta); 68 | d_vnf=Cnn*Cnb*d_vbf; 69 | 70 | //重新计算重力积分项d_vng 71 | gravity=Earth::gravity(midpos); 72 | gnl<<0, 0, gravity; 73 | d_vng=(gnl-(2*w_n_ie+w_n_en).cross(midvel))*imucur.dt; 74 | 75 | //5、更新速度 76 | pvacur.vel=pvapre.vel+d_vnf+d_vng; 77 | } 78 | 79 | 80 | //导航系NED位置推导 81 | void INSMechan::PositionUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur) 82 | { 83 | //计算中间时刻tk-1/2速度和位置 84 | Vector3d midvel=(pvapre.vel+pvacur.vel)/2; 85 | Vector3d midpos=pvapre.blh+Earth::DRi(pvapre.blh) * midvel * imucur.dt / 2; 86 | 87 | // 计算中间时刻地理参数 88 | double RM, RN; 89 | Vector3d w_n_ie, w_n_en; 90 | RM=Earth::RM(midpos[0]); 91 | RN=Earth::RN(midpos[0]); 92 | w_n_ie=Earth::w_n_ie(midpos[0]); 93 | w_n_en=Earth::w_n_en(midpos, midvel); 94 | 95 | //计算tk-1到tk时刻旋转矢量 96 | Vector3d zeta=(w_n_ie+w_n_en)*imucur.dt; 97 | Quaterniond qnn=Rotation::Vector2Quaternion(zeta); 98 | Vector3d zeta2(0, 0, -WGS84_WIE * imucur.dt); 99 | Quaterniond qee=Rotation::Vector2Quaternion(zeta2); 100 | 101 | Quaterniond qen=Earth::qen(pvapre.blh); 102 | qen=qee*qen*qnn; 103 | 104 | //更新位置 105 | pvacur.blh[2]=pvapre.blh[2]-midvel[2]*imucur.dt; 106 | pvacur.blh = Earth::blh(qen, pvacur.blh[2]); 107 | } 108 | 109 | 110 | //导航系NED姿态推导 111 | void INSMechan::AttitudeUpdate(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur) 112 | { 113 | //计算中间时刻tk-1/2的位置和速度 114 | Vector3d midvel=(pvapre.vel+pvacur.vel)/2; 115 | Quaterniond qen_pre=Earth::qen(pvapre.blh); 116 | Quaterniond qen_cur=Earth::qen(pvacur.blh); 117 | Vector3d zeta=Rotation::Quaternion2Vector(qen_cur.inverse()*qen_pre); 118 | 119 | Quaterniond qen_mid(qen_pre*Rotation::Vector2Quaternion(zeta/2).inverse()); 120 | Vector3d midpos; 121 | midpos[2] = (pvacur.blh[2] + pvapre.blh[2]) / 2; 122 | midpos = Earth::blh(qen_mid, midpos[2]); 123 | 124 | //计算中间时刻地理参数 125 | double RM, RN; 126 | Vector3d w_n_ie, w_n_en; 127 | RM=Earth::RM(midpos[0]); 128 | RN=Earth::RN(midpos[0]); 129 | w_n_ie=Earth::w_n_ie(midpos[0]); 130 | w_n_en=Earth::w_n_en(midpos, midvel); 131 | 132 | //计算n系tk-1到tk时刻旋转矢量 133 | Quaterniond qnn=Rotation::Vector2Quaternion(-(w_n_ie+w_n_en)*imucur.dt); 134 | 135 | // 计算b系旋转四元数 补偿二阶圆锥误差 136 | Quaterniond qbb=Rotation::Vector2Quaternion(imucur.dtheta + imupre.dtheta.cross(imucur.dtheta) / 12); 137 | 138 | //姿态更新 139 | pvacur.qnb=qnn*pvapre.qnb*qbb; 140 | } 141 | 142 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/earth.h: -------------------------------------------------------------------------------- 1 | #ifndef EARTH_H_ 2 | #define EARTH_H_ 3 | 4 | #include 5 | 6 | using Eigen::Matrix3d; 7 | using Eigen::Quaterniond; 8 | using Eigen::Vector3d; 9 | 10 | // WGS84椭球模型参数 11 | const double WGS84_WIE = 7.2921151467E-5; /* 地球自转角速度*/ 12 | const double WGS84_F = 0.0033528106647474805; /* 扁率 */ 13 | const double WGS84_RA = 6378137.0000000000; /* 长半轴a */ 14 | const double WGS84_RB = 6356752.3142451793; /* 短半轴b */ 15 | const double WGS84_GM0 = 398600441800000.00; /* 地球引力常数 */ 16 | const double WGS84_E1 = 0.0066943799901413156; /* 第一偏心率平方 */ 17 | const double WGS84_E2 = 0.0067394967422764341; /* 第二偏心率平方 */ 18 | 19 | 20 | class Earth 21 | { 22 | public: 23 | 24 | //RM子午圈曲率半径 25 | static double RM(const double lat) 26 | { 27 | return double(WGS84_RA*(1-WGS84_E1)/pow(1-WGS84_E1*sin(lat)*sin(lat) ,3/2)); 28 | } 29 | 30 | //RN卯酉圈曲率半径 31 | static double RN(const double lat) 32 | { 33 | return double(WGS84_RA/sqrt(1-WGS84_E1*sin(lat)*sin(lat))); 34 | } 35 | 36 | static Vector3d w_n_ie(const double lat) 37 | { 38 | return Vector3d(WGS84_WIE*cos(lat), 0, -WGS84_WIE*sin(lat)); 39 | } 40 | 41 | static Vector3d w_n_en(const Vector3d blh, const Vector3d vel) 42 | { 43 | double rn=RN(blh[0]); 44 | double rm=RM(blh[0]); 45 | return Vector3d(vel[1]/(rn+blh[2]), -vel[0]/(rm+blh[2]), -vel[1]*tan(blh[0])/(rn+blh[2]) ); 46 | } 47 | 48 | static double gravity(const Vector3d blh) 49 | { 50 | double sin2 = sin(blh[0]); 51 | sin2 *= sin2; 52 | 53 | return 9.7803267715 * (1 + 0.0052790414 * sin2 + 0.0000232718 * sin2 * sin2) + 54 | blh[2] * (0.0000000043977311 * sin2 - 0.0000030876910891) + 0.0000000000007211 * blh[2] * blh[2]; 55 | } 56 | 57 | //n系转e系 58 | static Quaterniond qen(const Vector3d &blh) { 59 | Quaterniond quat; 60 | 61 | double coslon, sinlon, coslat, sinlat; 62 | 63 | coslon = cos(blh[1] * 0.5); 64 | sinlon = sin(blh[1] * 0.5); 65 | coslat = cos(-M_PI * 0.25 - blh[0] * 0.5); 66 | sinlat = sin(-M_PI * 0.25 - blh[0] * 0.5); 67 | 68 | quat.w() = coslat * coslon; 69 | quat.x() = -sinlat * sinlon; 70 | quat.y() = sinlat * coslon; 71 | quat.z() = coslat * sinlon; 72 | 73 | return quat; 74 | } 75 | 76 | /* 从n系到e系转换四元数得到纬度和经度 */ 77 | static Vector3d blh(const Quaterniond &qne, double height) { 78 | return {-2 * atan(qne.y() / qne.w()) - M_PI * 0.5, 2 * atan2(qne.z(), qne.w()), height}; 79 | } 80 | 81 | /* n系相对位置转大地坐标相对位置 */ 82 | static Matrix3d DRi(const Vector3d &blh) { 83 | Matrix3d dri = Matrix3d::Zero(); 84 | 85 | double rm=RM(blh[0]); 86 | double rn=RN(blh[0]); 87 | dri(0, 0) = 1.0 / (rm + blh[2]); 88 | dri(1, 1) = 1.0 / ((rn + blh[2]) * cos(blh[0])); 89 | dri(2, 2) = -1; 90 | return dri; 91 | } 92 | 93 | /* 大地坐标相对位置转n系相对位置 */ 94 | static Matrix3d DR(const Vector3d &blh) { 95 | Matrix3d dr = Matrix3d::Zero(); 96 | 97 | double rm=RM(blh[0]); 98 | double rn=RN(blh[0]); 99 | 100 | dr(0, 0) = rm + blh[2]; 101 | dr(1, 1) = (rn + blh[2]) * cos(blh[0]); 102 | dr(2, 2) = -1; 103 | return dr; 104 | } 105 | 106 | 107 | /* 大地坐标(纬度、经度和高程)转地心地固坐标 */ 108 | static Vector3d blh2ecef(const Vector3d &blh) { 109 | double coslat, sinlat, coslon, sinlon; 110 | double rnh, rn; 111 | 112 | coslat = cos(blh[0]); 113 | sinlat = sin(blh[0]); 114 | coslon = cos(blh[1]); 115 | sinlon = sin(blh[1]); 116 | 117 | rn = RN(blh[0]); 118 | rnh = rn + blh[2]; 119 | 120 | return {rnh * coslat * coslon, rnh * coslat * sinlon, (rnh - rn * WGS84_E1) * sinlat}; 121 | } 122 | 123 | 124 | /* n系(导航坐标系)到e系(地心地固坐标系)转换矩阵 */ 125 | static Matrix3d Ren(const Vector3d &blh) { 126 | double coslon, sinlon, coslat, sinlat; 127 | sinlat = sin(blh[0]); 128 | sinlon = sin(blh[1]); 129 | coslat = cos(blh[0]); 130 | coslon = cos(blh[1]); 131 | 132 | Matrix3d dcm; 133 | dcm <<-sinlat * coslon, -sinlon, -coslat * coslon, 134 | -sinlat * sinlon, coslon, -coslat * sinlon, 135 | coslat, 0, -sinlat; 136 | 137 | return dcm; 138 | } 139 | 140 | 141 | /* 大地坐标转局部坐标(在origin处展开) */ 142 | static Vector3d global2local(const Vector3d &origin, const Vector3d &global) { 143 | Vector3d ecef0 = blh2ecef(origin); 144 | Matrix3d Ren0 = Ren(origin); 145 | 146 | Vector3d ecef1 = blh2ecef(global); 147 | 148 | return Ren0.transpose() * (ecef1 - ecef0); 149 | } 150 | 151 | }; 152 | 153 | 154 | 155 | 156 | #endif // !EARTH_H_ 157 | 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /kf_gins_ros/config/gins_rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Path1 10 | Splitter Ratio: 0.5 11 | Tree Height: 514 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Buffer Length: 1 58 | Class: rviz/Path 59 | Color: 25; 255; 0 60 | Enabled: true 61 | Head Diameter: 0.30000001192092896 62 | Head Length: 0.20000000298023224 63 | Length: 0.30000001192092896 64 | Line Style: Lines 65 | Line Width: 0.029999999329447746 66 | Name: Path 67 | Offset: 68 | X: 0 69 | Y: 0 70 | Z: 0 71 | Pose Color: 255; 85; 255 72 | Pose Style: Axes 73 | Radius: 0.029999999329447746 74 | Shaft Diameter: 0.10000000149011612 75 | Shaft Length: 0.10000000149011612 76 | Topic: /gins_ned_path 77 | Unreliable: false 78 | Value: true 79 | - Class: rviz/Axes 80 | Enabled: true 81 | Length: 1 82 | Name: Axes 83 | Radius: 0.10000000149011612 84 | Reference Frame: 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 48; 48; 48 89 | Default Light: true 90 | Fixed Frame: world 91 | Frame Rate: 30 92 | Name: root 93 | Tools: 94 | - Class: rviz/Interact 95 | Hide Inactive Objects: true 96 | - Class: rviz/MoveCamera 97 | - Class: rviz/Select 98 | - Class: rviz/FocusCamera 99 | - Class: rviz/Measure 100 | - Class: rviz/SetInitialPose 101 | Theta std deviation: 0.2617993950843811 102 | Topic: /initialpose 103 | X std deviation: 0.5 104 | Y std deviation: 0.5 105 | - Class: rviz/SetGoal 106 | Topic: /move_base_simple/goal 107 | - Class: rviz/PublishPoint 108 | Single click: true 109 | Topic: /clicked_point 110 | Value: true 111 | Views: 112 | Current: 113 | Class: rviz/Orbit 114 | Distance: 12.175732612609863 115 | Enable Stereo Rendering: 116 | Stereo Eye Separation: 0.05999999865889549 117 | Stereo Focal Distance: 1 118 | Swap Stereo Eyes: false 119 | Value: false 120 | Focal Point: 121 | X: 0 122 | Y: 0 123 | Z: 0 124 | Focal Shape Fixed Size: true 125 | Focal Shape Size: 0.05000000074505806 126 | Invert Z Axis: false 127 | Name: Current View 128 | Near Clip Distance: 0.009999999776482582 129 | Pitch: 0.6297966241836548 130 | Target Frame: 131 | Value: Orbit (rviz) 132 | Yaw: 3.4085793495178223 133 | Saved: ~ 134 | Window Geometry: 135 | Displays: 136 | collapsed: false 137 | Height: 846 138 | Hide Left Dock: false 139 | Hide Right Dock: false 140 | QMainWindow State: 000000ff00000000fd0000000400000000000001610000029efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000470000029e000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000470000029e000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005bd00000041fc0100000002fb0000000800540069006d00650100000000000005bd0000037100fffffffb0000000800540069006d006501000000000000045000000000000000000000033f0000029e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 141 | Selection: 142 | collapsed: false 143 | Time: 144 | collapsed: false 145 | Tool Properties: 146 | collapsed: false 147 | Views: 148 | collapsed: false 149 | Width: 1469 150 | X: 566 151 | Y: 158 152 | -------------------------------------------------------------------------------- /kf_gins_ros/gins.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include "std_msgs/String.h" 13 | #include 14 | #include 15 | 16 | #include "gins_engine.h" 17 | #include "visualization.h" 18 | 19 | using namespace std; 20 | 21 | std::condition_variable con; 22 | std::queue imu_buf; 23 | std::queue gnss_buf; 24 | std::mutex m_buf; 25 | 26 | ros::Publisher pub_ins_odometry, pub_ins_path; 27 | ros::Publisher pub_gins_blh, pub_gins_ned; 28 | 29 | 30 | double current_time = -1; 31 | 32 | void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) 33 | { 34 | m_buf.lock(); 35 | imu_buf.push(imu_msg); 36 | m_buf.unlock(); 37 | con.notify_one(); 38 | } 39 | 40 | void gnss_callback(const sensor_msgs::NavSatFixConstPtr &gnss_msg) 41 | { 42 | m_buf.lock(); 43 | gnss_buf.push(gnss_msg); 44 | m_buf.unlock(); 45 | con.notify_one(); 46 | } 47 | 48 | 49 | //IMU与GNSS的数据流做同步，以GNSS的时间间隔，确定一组imu_msg 50 | bool getMeasurements(std::vector &imu_msg, sensor_msgs::NavSatFixConstPtr &gnss_msg) 51 | { 52 | //当前imu和gnss数据都为空 53 | if(imu_buf.empty() || gnss_buf.empty()){ 54 | return false; 55 | } 56 | 57 | //imu最新的时刻仍然小于gnss最早时刻 58 | if(imu_buf.back()->header.stamp.toSec()header.stamp.toSec()){ 59 | return false; 60 | } 61 | 62 | double front_imu_ts=imu_buf.front()->header.stamp.toSec(); 63 | double front_gnss_ts=gnss_buf.front()->header.stamp.toSec(); 64 | 65 | //第一个imu时间戳大于gnss时间戳，就把gnss数据丢掉 66 | while(!gnss_buf.empty() && front_imu_ts>front_gnss_ts ) 67 | { 68 | ROS_WARN("throw gnss_msg, only should happen at the beginning"); 69 | gnss_buf.pop(); 70 | 71 | if(gnss_buf.empty()) return false; 72 | front_gnss_ts=gnss_buf.front()->header.stamp.toSec(); 73 | } 74 | 75 | gnss_msg=gnss_buf.front(); 76 | gnss_buf.pop(); 77 | 78 | //截取两个gnss时刻中间的imu，放入imu_buf，作为一组imu数据 79 | while (!imu_buf.empty() && imu_buf.front()->header.stamp.toSec() < gnss_msg->header.stamp.toSec()) 80 | { 81 | imu_msg.emplace_back(imu_buf.front()); 82 | imu_buf.pop(); 83 | } 84 | 85 | //多取出一个gnss时刻之后的imu数据，用来做内插，对齐到gnss时刻 86 | if(!imu_buf.empty()){ 87 | imu_msg.emplace_back(imu_buf.front()); 88 | imu_buf.pop(); 89 | } 90 | 91 | if (imu_msg.empty()){ 92 | ROS_WARN("no imu between two GNSS"); 93 | } 94 | 95 | return true; 96 | } 97 | 98 | 99 | void process() 100 | { 101 | GINSEngine gins_engine; 102 | 103 | while(true) 104 | { 105 | std::vector imu_msg; 106 | sensor_msgs::NavSatFixConstPtr gnss_msg; 107 | 108 | std::unique_lock lk(m_buf); 109 | con.wait(lk, [&] 110 | { 111 | return getMeasurements(imu_msg, gnss_msg); 112 | }); 113 | lk.unlock(); 114 | 115 | for(int i=0; iheader.stamp.toSec(); 119 | if(current_time<0) //第一个历元 current_time=-1 120 | current_time=t; 121 | 122 | double dt=t-current_time; 123 | ROS_ASSERT(dt >= 0); //判断imu数据时间戳有没有错乱 124 | current_time = t; 125 | 126 | gins_engine.processIMU(dt, imu_data); //IMU递推 127 | pubINSMech(gins_engine, imu_data->header); 128 | } 129 | 130 | //利用最后两个imu时刻对gnss时刻做内插，计算内插时刻IMU递推结果 131 | sensor_msgs::ImuConstPtr imu_data_back=imu_msg.back(); 132 | 133 | double gnss_t=gnss_msg->header.stamp.toSec(); 134 | gins_engine.IMUInterpolate(gnss_t, imu_data_back); 135 | gins_engine.processGNSS(gnss_msg); //gnss量测更新 136 | pubGINS(gins_engine, gnss_msg->header); 137 | 138 | //计算gnss时刻到最后一个imu时刻的递推结果 139 | double dt=imu_data_back->header.stamp.toSec()-gnss_t; 140 | gins_engine.processIMU(dt, imu_data_back); 141 | pubINSMech(gins_engine, imu_data_back->header); 142 | 143 | } 144 | } 145 | 146 | 147 | 148 | int main(int argc, char **argv) 149 | { 150 | ros::init(argc, argv, "gins"); 151 | ros::NodeHandle n("~"); 152 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 153 | 154 | if(argc != 2) 155 | { 156 | printf("please intput: rosrun gins gins_node [config file] \n"); 157 | return 1; 158 | } 159 | 160 | string config_file = argv[1]; 161 | printf("config_file: %s\n", argv[1]); 162 | 163 | readParameters(config_file); 164 | ROS_WARN("waiting for gnss and imu..."); 165 | 166 | ros::Subscriber sub_imu = n.subscribe(GNSS_TOPIC, 100, gnss_callback); 167 | ros::Subscriber sub_gnss = n.subscribe(IMU_TOPIC, 1000, imu_callback); 168 | 169 | pub_ins_odometry = n.advertise("/insmech_odom", 1000); 170 | pub_ins_path = n.advertise("/insmech_path", 1000); 171 | pub_gins_blh = n.advertise("/gins_blh", 1000); 172 | pub_gins_ned = n.advertise("/gins_ned_path", 1000); 173 | 174 | std::thread measurement_process{process}; 175 | 176 | ros::spin(); 177 | return 0; 178 | } 179 | -------------------------------------------------------------------------------- /kf_gins_ros/gins/gins_engine.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "gins_engine.h" 4 | 5 | bool first_imu=false; 6 | 7 | //构造函数，定义协方差 8 | GINSEngine::GINSEngine() { 9 | 10 | // 设置协方差矩阵，系统噪声阵和系统误差状态矩阵大小 11 | Cov_.resize(21, 21); //EKF状态量协方差（pos vel att bg ba sg sa） 12 | Qc_.resize(18, 18); //噪声方差(vrw arw bg ba sg sa ) 13 | dx_.resize(21, 1); //EKF状态误差 14 | Cov_.setZero(); 15 | Qc_.setZero(); 16 | dx_.setZero(); 17 | 18 | // 初始化系统噪声阵 19 | Vector3d arw=ACC_W*M_PI/180/60; // deg/sqrt(hr) -> rad/s 20 | Vector3d vrw=GYR_W*M_PI/180/60; // m/s/sqrt(hr) -> m/s 21 | Vector3d gyrbias_std=GYR_BIAS_STD*M_PI/180/3600; // deg/hr -> rad/s 22 | Vector3d accbias_std=ACC_BIAS_STD*1e-5; // mGal -> m/s^2 10e+5 mGal = 1 m/s^2 23 | Vector3d gyrscale_std=GYR_SCALE_STD*1e-6; 24 | Vector3d accscale_std=ACC_SCALE_STD*1e-6; 25 | 26 | 27 | Qc_.block(0, 0, 3, 3) = vrw.cwiseProduct(vrw).asDiagonal(); 28 | Qc_.block(3, 3, 3, 3) = arw.cwiseProduct(arw).asDiagonal(); 29 | Qc_.block(6, 6, 3, 3) = 2 / CORRTIME *gyrbias_std.cwiseProduct(gyrbias_std).asDiagonal(); 30 | Qc_.block(9, 9, 3, 3) = 2 / CORRTIME * accbias_std.cwiseProduct(accbias_std).asDiagonal(); 31 | Qc_.block(12, 12, 3, 3) = 2 / CORRTIME * gyrscale_std.cwiseProduct(gyrscale_std).asDiagonal(); 32 | Qc_.block(15, 15, 3, 3) = 2 / CORRTIME * accscale_std.cwiseProduct(accscale_std).asDiagonal(); 33 | 34 | // 设置系统状态(位置、速度、姿态和IMU误差)初值和初始协方差 35 | 36 | // 初始化协方差 37 | Vector3d initpos_std=POS_STD; //m 38 | Vector3d initvel_std=VEL_STD; // m/s 39 | Vector3d initatt_std=ATT_STD*M_PI/180; //deg -> rad 40 | 41 | Vector3d initgyrbias_std=gyrbias_std; 42 | Vector3d initaccbias_std=accbias_std; 43 | Vector3d initgyrscale_std=gyrbias_std; 44 | Vector3d initaccscale_std=accbias_std; 45 | 46 | Cov_.block(0, 0, 3, 3) = initpos_std.cwiseProduct(initpos_std).asDiagonal(); 47 | Cov_.block(3, 3, 3, 3) = initvel_std.cwiseProduct(initvel_std).asDiagonal(); 48 | Cov_.block(6, 6, 3, 3) = initatt_std.cwiseProduct(initatt_std).asDiagonal(); 49 | Cov_.block(9, 9, 3, 3) = initgyrbias_std.cwiseProduct(initgyrbias_std).asDiagonal(); 50 | Cov_.block(12, 12, 3, 3) = initaccbias_std.cwiseProduct(initaccbias_std).asDiagonal(); 51 | Cov_.block(15, 15, 3, 3) =initgyrscale_std.cwiseProduct(initgyrscale_std).asDiagonal(); 52 | Cov_.block(18, 18, 3, 3) = initaccscale_std.cwiseProduct(initaccscale_std).asDiagonal(); 53 | 54 | } 55 | 56 | 57 | //第1个历元状态初始化 58 | void GINSEngine::INSInitialization(double timestamp) 59 | { 60 | pvacur.time=timestamp; 61 | pvacur.blh << INITPOS[0]*M_PI/180, INITPOS[1]*M_PI/180, INITPOS[2]; 62 | pvacur.vel<linear_acceleration.x; 71 | dy = imu_msg->linear_acceleration.y; 72 | dz = imu_msg->linear_acceleration.z; 73 | rx = imu_msg->angular_velocity.x; 74 | ry = imu_msg->angular_velocity.y; 75 | rz = imu_msg->angular_velocity.z; 76 | 77 | imucur.dt=dt; 78 | imucur.time=imu_msg->header.stamp.toSec(); 79 | imucur.acc<linear_acceleration.x; 109 | dy = imu_msg->linear_acceleration.y; 110 | dz = imu_msg->linear_acceleration.z; 111 | rx = imu_msg->angular_velocity.x; 112 | ry = imu_msg->angular_velocity.y; 113 | rz = imu_msg->angular_velocity.z; 114 | 115 | IMU imu0=imupre; 116 | IMU imu1, imu_mid; 117 | imu1.time = imu_msg->header.stamp.toSec(); 118 | imu1.gyro << rx, ry, rz; 119 | imu1.acc << dx, dy, dz; 120 | 121 | //加权平均值计算内插时刻 122 | double lamda1=(timestamp-imu0.time) / (imu1.time-imu0.time); 123 | double lamda2=(imu1.time - timestamp) / (imu1.time-imu0.time); 124 | 125 | imu_mid.time=timestamp; 126 | imu_mid.dt=timestamp-imu0.time; 127 | imu_mid.acc = lamda1*imu0.acc+lamda2*imu1.acc; 128 | imu_mid.gyro = lamda1*imu0.gyro+lamda2*imu1.gyro; 129 | imu_mid.dvel=0.5*(imu_mid.acc+imu0.acc)*imu_mid.dt; 130 | imu_mid.dtheta=0.5*(imu_mid.gyro+imu0.gyro)*imu_mid.dt; 131 | 132 | INSPropagation(imu0, imu_mid, pvapre, pvacur); 133 | 134 | //更新时刻状态 135 | imupre=imu_mid; 136 | pvapre=pvacur; 137 | } 138 | 139 | 140 | 141 | void GINSEngine::processGNSS(sensor_msgs::NavSatFixConstPtr &gnss_msg) 142 | { 143 | 144 | double lat=gnss_msg->latitude; 145 | double lon=gnss_msg->longitude; 146 | double h=gnss_msg->altitude; 147 | gnssdata.blh<< lat*M_PI/180, lon*M_PI/180, h; 148 | gnssdata.time=gnss_msg->header.stamp.toSec(); 149 | 150 | gnssdata.poscov(0, 0)=gnss_msg->position_covariance[0]; //N 151 | gnssdata.poscov(1, 1)=gnss_msg->position_covariance[3]; //E 152 | gnssdata.poscov(2, 2)=gnss_msg->position_covariance[6]; //D 153 | 154 | 155 | GNSSUpdate(gnssdata); 156 | StateFeedback(); //GNSS做完量测更新后，做一次误差状态反馈 157 | 158 | pvapre=pvacur;//更新上一时刻的状态 159 | } 160 | 161 | 162 | void GINSEngine:: INSPropagation(IMU &imupre, IMU &imucur, PVA &pvapre, PVA &pvacur) 163 | { 164 | //INS状态递推 165 | INSMechan::insMechan(imupre, imucur, pvapre, pvacur); 166 | 167 | // 系统噪声传播，姿态误差采用phi角误差模型 168 | Eigen::MatrixXd Phi, F, Qd, G; 169 | 170 | // 初始化Phi阵(状态转移矩阵)，F阵，Qd阵(传播噪声阵)，G阵(噪声驱动阵) 171 | Phi.resizeLike(Cov_); 172 | F.resizeLike(Cov_); 173 | Qd.resizeLike(Cov_); 174 | G.resize(21, 18); 175 | Phi.setIdentity(); 176 | F.setZero(); 177 | Qd.setZero(); 178 | G.setZero(); 179 | 180 | // 使用上一历元状态计算状态转移矩阵Phi 181 | double rm,rn; 182 | Vector3d w_n_ie, w_n_en; 183 | rm=Earth::RM(pvapre.blh[0]); 184 | rn=Earth::RN(pvapre.blh[0]); 185 | w_n_ie=Earth::w_n_ie(pvapre.blh[0]); 186 | w_n_en=Earth::w_n_en(pvapre.blh, pvapre.vel); 187 | double gravity=Earth::gravity(pvapre.blh); 188 | 189 | Eigen::Matrix3d temp; 190 | Eigen::Vector3d accel, omega; 191 | double rmh, rnh; 192 | 193 | rmh = rm + pvapre.blh[2]; 194 | rnh = rn + pvapre.blh[2]; 195 | accel = imucur.acc; 196 | omega = imucur.gyro; 197 | 198 | 199 | //计算F矩阵 200 | // 1. 位置误差 position error 201 | temp.setZero(); 202 | temp(0, 0) = -pvapre.vel[2] / rmh; 203 | temp(0, 2) = pvapre.vel[0] / rmh; 204 | temp(1, 0) = pvapre.vel[1] * tan(pvapre.blh[0]) / rnh; 205 | temp(1, 1) = -(pvapre.vel[2] + pvapre.vel[0] * tan(pvapre.blh[0])) / rnh; 206 | temp(1, 2) = pvapre.vel[1] / rnh; 207 | F.block(0, 0, 3, 3) = temp; //Frr 208 | F.block(0, 3, 3, 3) = Eigen::Matrix3d::Identity(); //Frv 209 | 210 | // 2. 速度误差 velocity error 211 | temp.setZero(); 212 | temp(0, 0) = -2 * pvapre.vel[1] * WGS84_WIE * cos(pvapre.blh[0]) / rmh - 213 | pow(pvapre.vel[1], 2) / rmh / rnh / pow(cos(pvapre.blh[0]), 2); 214 | temp(0, 2) = pvapre.vel[0] * pvapre.vel[2] / rmh / rmh - pow(pvapre.vel[1], 2) * tan(pvapre.blh[0]) / rnh / rnh; 215 | temp(1, 0) = 2 * WGS84_WIE * (pvapre.vel[0] * cos(pvapre.blh[0]) - pvapre.vel[2] * sin(pvapre.blh[0])) / rmh + 216 | pvapre.vel[0] * pvapre.vel[1] / rmh / rnh / pow(cos(pvapre.blh[0]), 2); 217 | temp(1, 2) = (pvapre.vel[1] * pvapre.vel[2] + pvapre.vel[0] * pvapre.vel[1] * tan(pvapre.blh[0])) / rnh / rnh; 218 | temp(2, 0) = 2 * WGS84_WIE * pvapre.vel[1] * sin(pvapre.blh[0]) / rmh; 219 | temp(2, 2) = -pow(pvapre.vel[1], 2) / rnh / rnh - pow(pvapre.vel[0], 2) / rmh / rmh + 220 | 2 * gravity / (sqrt(rm * rn) + pvapre.blh[2]); 221 | F.block(3, 0, 3, 3) = temp; //Fvr 222 | 223 | temp.setZero(); 224 | temp(0, 0) = pvapre.vel[2] / rmh; 225 | temp(0, 1) = -2 * (WGS84_WIE * sin(pvapre.blh[0]) + pvapre.vel[1] * tan(pvapre.blh[0]) / rnh); 226 | temp(0, 2) = pvapre.vel[0] / rmh; 227 | temp(1, 0) = 2 * WGS84_WIE * sin(pvapre.blh[0]) + pvapre.vel[1] * tan(pvapre.blh[0]) / rnh; 228 | temp(1, 1) = (pvapre.vel[2] + pvapre.vel[0] * tan(pvapre.blh[0])) / rnh; 229 | temp(1, 2) = 2 * WGS84_WIE * cos(pvapre.blh[0]) + pvapre.vel[1] / rnh; 230 | temp(2, 0) = -2 * pvapre.vel[0] / rmh; 231 | temp(2, 1) = -2 * (WGS84_WIE * cos(pvapre.blh(0)) + pvapre.vel[1] / rnh); 232 | F.block(3, 3, 3, 3) = temp; //Fvv 233 | 234 | 235 | F.block(3, 6, 3, 3) = Rotation::skewSymmetric(pvapre.qnb.toRotationMatrix()* accel); 236 | F.block(3, 12, 3, 3) =pvapre.qnb.toRotationMatrix(); 237 | F.block(3, 18, 3, 3) = pvapre.qnb.toRotationMatrix() * (accel.asDiagonal()); 238 | 239 | // 3. 姿态误差 240 | // attitude error 241 | temp.setZero(); 242 | temp(0, 0) = -WGS84_WIE * sin(pvapre.blh[0]) / rmh; 243 | temp(0, 2) = pvapre.vel[1] / rnh / rnh; 244 | temp(1, 2) = -pvapre.vel[0] / rmh / rmh; 245 | temp(2, 0) = -WGS84_WIE * cos(pvapre.blh[0]) / rmh - pvapre.vel[1] / rmh / rnh / pow(cos(pvapre.blh[0]), 2); 246 | temp(2, 2) = -pvapre.vel[1] * tan(pvapre.blh[0]) / rnh / rnh; 247 | F.block(6, 0, 3, 3) = temp; 248 | temp.setZero(); 249 | temp(0, 1) = 1 / rnh; 250 | temp(1, 0) = -1 / rmh; 251 | temp(2, 1) = -tan(pvapre.blh[0]) / rnh; 252 | F.block(6, 3, 3, 3) = temp; 253 | F.block(6, 6, 3, 3) = -Rotation::skewSymmetric(w_n_en+w_n_ie); 254 | F.block(6, 9, 3, 3) = -pvapre.qnb.toRotationMatrix(); 255 | F.block(6, 15, 3, 3) = -pvapre.qnb.toRotationMatrix() * (omega.asDiagonal()); 256 | 257 | // IMU零偏误差和比例因子误差，建模成一阶高斯-马尔科夫过程 258 | F.block(9, 9, 3, 3) = -1 / CORRTIME * Eigen::Matrix3d::Identity(); 259 | F.block(12, 12, 3, 3) = -1 / CORRTIME * Eigen::Matrix3d::Identity(); 260 | F.block(15, 15, 3, 3) = -1 / CORRTIME * Eigen::Matrix3d::Identity(); 261 | F.block(18, 18, 3, 3) = -1 / CORRTIME * Eigen::Matrix3d::Identity(); 262 | 263 | // 系统噪声驱动矩阵 264 | G.block(3, 0, 3, 3) = pvapre.qnb.toRotationMatrix(); 265 | G.block(6, 3, 3, 3) = pvapre.qnb.toRotationMatrix(); 266 | G.block(9, 6, 3, 3) = Eigen::Matrix3d::Identity(); 267 | G.block(12, 9, 3, 3) = Eigen::Matrix3d::Identity(); 268 | G.block(15, 12, 3, 3) = Eigen::Matrix3d::Identity(); 269 | G.block(18, 15, 3, 3) = Eigen::Matrix3d::Identity(); 270 | 271 | // 计算状态转移矩阵Phi 272 | //一步转移矩阵式 Phi(tk/k-1)=I+F(tk-1)*dt 273 | Phi.setIdentity(); 274 | Phi = Phi + F * imucur.dt; 275 | 276 | // 计算系统传播噪声 277 | Qd = G * Qc_ * G.transpose() ; 278 | Qd = (Phi * Qd * Phi.transpose() + Qd) * imucur.dt/ 2; 279 | 280 | // EKF预测传播系统协方差和系统误差状态 281 | EKFPredict(Phi, Qd); 282 | } 283 | 284 | 285 | void GINSEngine::StateFeedback() 286 | { 287 | Eigen::Vector3d vectemp; 288 | 289 | // 1. 位置误差反馈 290 | Eigen::Vector3d delta_r = dx_.block(0, 0, 3, 1); 291 | Eigen::Matrix3d Dr_inv = Earth::DRi(pvacur.blh); 292 | pvacur.blh -= Dr_inv * delta_r; 293 | 294 | // 2. 速度误差反馈 295 | vectemp = dx_.block(3, 0, 3, 1); 296 | pvacur.vel -= vectemp; 297 | 298 | // 3. 姿态误差反馈 299 | vectemp = dx_.block(6, 0, 3, 1); 300 | Eigen::Quaterniond qnp = Rotation::Vector2Quaternion(vectemp); 301 | pvacur.qnb=qnp*pvacur.qnb; 302 | 303 | // 4. IMU零偏误差反馈 304 | vectemp = dx_.block(9, 0, 3, 1); 305 | gyrbias += vectemp; 306 | vectemp = dx_.block(12, 0, 3, 1); 307 | accbias += vectemp; 308 | 309 | // 5. IMU比例因子误差反馈 310 | vectemp = dx_.block(15, 0, 3, 1); 311 | gyrscale += vectemp; 312 | vectemp = dx_.block(18, 0, 3, 1); 313 | accscale += vectemp; 314 | 315 | // 误差状态反馈到系统状态后,将误差状态清零 316 | dx_.setZero(); 317 | 318 | } 319 | 320 | 321 | 322 | void GINSEngine::GNSSUpdate(GNSS &gnssdata) 323 | { 324 | // IMU位置转到GNSS天线相位中心位置 325 | // convert IMU position to GNSS antenna phase center position 326 | Eigen::Vector3d antenna_pos; 327 | Eigen::Matrix3d Dr, Dr_inv; 328 | Dr_inv = Earth::DRi(pvacur.blh); 329 | Dr = Earth::DR(pvacur.blh); 330 | 331 | // 安装参数(就是天线杆臂) 332 | Eigen::Vector3d antlever = ANTLEVER; 333 | antenna_pos = pvacur.blh + Dr_inv * pvacur.qnb.toRotationMatrix() * antlever; 334 | 335 | // GNSS位置测量新息 delta_zr 336 | Eigen::MatrixXd dz; 337 | dz = Dr * (antenna_pos - gnssdata.blh); 338 | 339 | // 构造GNSS位置观测矩阵Hr 340 | Eigen::MatrixXd H_gnsspos; 341 | H_gnsspos.resize(3, 21); 342 | H_gnsspos.setZero(); 343 | H_gnsspos.block(0, 0, 3, 3) = Eigen::Matrix3d::Identity(); 344 | H_gnsspos.block(0, 6, 3, 3) = Rotation::skewSymmetric(pvacur.qnb.toRotationMatrix() * antlever); 345 | 346 | // 位置观测噪声阵 347 | Eigen::MatrixXd R_gnsspos=gnssdata.poscov; 348 | 349 | // EKF更新协方差和误差状态 350 | EKFUpdate(dz, H_gnsspos, R_gnsspos); 351 | 352 | // GNSS更新之后设置为不可用 353 | gnssdata.isvalid = false; 354 | } 355 | 356 | 357 | void GINSEngine::EKFPredict(Eigen::MatrixXd &Phi, Eigen::MatrixXd &Qd) 358 | { 359 | 360 | assert(Phi.rows() == Cov_.rows()); 361 | assert(Qd.rows() == Cov_.rows()); 362 | 363 | // 传播系统协方差和误差状态 364 | Cov_ = Phi * Cov_ * Phi.transpose() + Qd; 365 | dx_ = Phi * dx_; 366 | } 367 | 368 | 369 | void GINSEngine:: EKFUpdate(Eigen::MatrixXd &dz, Eigen::MatrixXd &H, Eigen::MatrixXd &R) 370 | { 371 | // 计算Kalman增益 372 | auto temp = H * Cov_ * H.transpose() + R; 373 | Eigen::MatrixXd K = Cov_ * H.transpose() * temp.inverse(); 374 | 375 | // 更新系统误差状态和协方差 376 | Eigen::MatrixXd I; 377 | I.resizeLike(Cov_); 378 | I.setIdentity(); 379 | I = I - K * H; 380 | // 如果每次更新后都进行状态反馈，则更新前dx_一直为0，下式可以简化为：dx_ = K * dz; 381 | dx_ = dx_ + K * (dz - H * dx_); 382 | Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose(); 383 | 384 | } --------------------------------------------------------------------------------