├── CMakeLists.txt ├── README.md ├── config └── ins_eskf.yaml ├── images ├── system archetecture.png └── system_architectuire.drawio ├── include ├── global_definition.h └── ins_eskf │ ├── ins_eskf.h │ └── ins_eskf_ros_wrapper.h ├── launch ├── ins_eskf.launch └── test_initialization.launch ├── log ├── ins_eskf_node.INFO ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154006.54783 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154020.55035 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154100.55474 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154146.55927 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154237.56387 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154304.56745 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154324.57023 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154331.57225 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154411.57643 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154500.58123 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154516.58390 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154626.59066 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154644.59351 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154852.60605 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-154942.61088 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-155103.61749 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-155155.62262 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-155247.62747 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-155305.63036 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-155620.64303 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-160014.66133 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-160031.66390 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-160312.67462 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-160325.67704 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161032.70499 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161136.71186 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161207.71694 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161432.72717 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161448.73052 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-161541.73564 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-162033.75410 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171000.88608 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171021.88829 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171221.89209 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171230.89388 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171341.89649 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171419.89891 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171516.90134 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171625.90410 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171714.90668 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171724.91855 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171741.92055 ├── ins_eskf_node.m16.yyj.log.INFO.20230123-171748.92218 └── ins_eskf_node.m16.yyj.log.INFO.20230125-195959.40439 ├── package.xml ├── rviz └── ins_eskf.rviz └── src ├── ins_eskf.cpp ├── ins_eskf_ROS_wrapper.cpp ├── ins_eskf_node.cpp └── test_initialization.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ins_eskf) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | SET(CMAKE_BUILD_TYPE debug) 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | ) 16 | 17 | find_package(yaml-cpp REQUIRED) 18 | find_package (GeographicLib REQUIRED) 19 | 20 | 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # geometry_msgs# sensor_msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | # INCLUDE_DIRS include 113 | # LIBRARIES ins_eskf 114 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs 115 | # DEPENDS system_lib 116 | ) 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | ## Specify additional locations of header files 123 | ## Your package locations should be listed before other locations 124 | include_directories( 125 | include 126 | ${catkin_INCLUDE_DIRS} 127 | ${YAML_CPP_INCLUDE_DIRS} 128 | ${GeographicLib_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | # add_library(${PROJECT_NAME} 133 | # src/${PROJECT_NAME}/ins_eskf.cpp 134 | # ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | add_executable(ins_eskf_node 145 | src/ins_eskf_node.cpp 146 | src/ins_eskf_ROS_wrapper.cpp 147 | src/ins_eskf.cpp) 148 | target_link_libraries(ins_eskf_node 149 | ${catkin_LIBRARIES} 150 | -lglog 151 | ${GeographicLib_LIBRARIES} 152 | ${YAML_CPP_LIBRARIES} 153 | ) 154 | 155 | add_executable(test_initialization 156 | src/test_initialization.cpp 157 | ) 158 | target_link_libraries(test_initialization 159 | ${catkin_LIBRARIES} 160 | -lglog 161 | ) 162 | 163 | 164 | 165 | 166 | ## Rename C++ executable without prefix 167 | ## The above recommended prefix causes long target names, the following renames the 168 | ## target back to the shorter version for ease of user use 169 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 170 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 171 | 172 | ## Add cmake target dependencies of the executable 173 | ## same as for the library above 174 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 175 | 176 | ## Specify libraries to link a library or executable target against 177 | 178 | 179 | ############# 180 | ## Install ## 181 | ############# 182 | 183 | # all install targets should use catkin DESTINATION variables 184 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 185 | 186 | ## Mark executable scripts (Python etc.) for installation 187 | ## in contrast to setup.py, you can choose the destination 188 | # catkin_install_python(PROGRAMS 189 | # scripts/my_python_script 190 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 191 | # ) 192 | 193 | ## Mark executables for installation 194 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 195 | # install(TARGETS ${PROJECT_NAME}_node 196 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 197 | # ) 198 | 199 | ## Mark libraries for installation 200 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 201 | # install(TARGETS ${PROJECT_NAME} 202 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 203 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 204 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 205 | # ) 206 | 207 | ## Mark cpp header files for installation 208 | # install(DIRECTORY include/${PROJECT_NAME}/ 209 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 210 | # FILES_MATCHING PATTERN "*.h" 211 | # PATTERN ".svn" EXCLUDE 212 | # ) 213 | 214 | ## Mark other files for installation (e.g. launch and bag files, etc.) 215 | # install(FILES 216 | # # myfile1 217 | # # myfile2 218 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 219 | # ) 220 | 221 | ############# 222 | ## Testing ## 223 | ############# 224 | 225 | ## Add gtest based cpp test target and link libraries 226 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ins_eskf.cpp) 227 | # if(TARGET ${PROJECT_NAME}-test) 228 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 229 | # endif() 230 | 231 | ## Add folders to be run by python nosetests 232 | # catkin_add_nosetests(test) 233 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # INS_ESKF_KITTI 2 | 3 | **I barely found GPS-IMU fusion localization algorithm using real world dataset on github,most of them are using data generated from [gnss-imu-sim](https://github.com/Aceinna/gnss-ins-sim).So I developed [ins_eskf_kitti](https://github.com/leo6862/ins_eskf_kitti),a GPS-IMU fusion localization algorithm using error-state kalman filter based on kitti dataset.I Use the formula that shared By Dr.Gao Xiang in [Zhihu](https://zhuanlan.zhihu.com/p/441182819)** 4 | 5 | ## Menu 6 | 7 | - [INS\_ESKF\_KITTI](#ins_eskf_kitti) 8 | - [Menu](#menu) 9 | - [System architecture (Improtant!)](#system-architecture-improtant) 10 | - [Dependency](#dependency) 11 | - [Install](#install) 12 | - [Sample datasets](#sample-datasets) 13 | - [Run the package](#run-the-package) 14 | 15 | ## System architecture (Improtant!) 16 | I create a ROS-wrapper(for ROS-noetic),so the core algorithm is dependent from ROS,and easy for other developers to integrate into other platforms or ROS2. And I use ```/kitti/oxts/gps/vel``` and ```orientation in /kitti/oxts/imu/extract``` so I can initialize the whole system.BTW,I also plot the ```position in /kitti/oxts/imu/extract & oirentation in /kitti/oxts/imu/extract``` in rviz as a baseline th show the result of fusion algorithm. 17 | 18 | 19 | Here in the whole system arcgitecture: 20 | 21 | ![](images/system%20archetecture.png) 22 | 23 | 24 | 25 | 26 | ## Dependency 27 | ROS-notic 28 | 29 | Eigen: 30 | ``` 31 | sudo apt-get install libeigen3-dev 32 | ``` 33 | geographiclib : 34 | ``` 35 | sudo apt-get install libgeographic-dev 36 | ``` 37 | 38 | glog : 39 | ``` 40 | git clone https://github.com/google/glog.git 41 | cd glog 42 | mkdir build 43 | cd build 44 | cmake .. 45 | make 46 | ``` 47 | 48 | yaml-cpp: 49 | ``` 50 | sudo apt-get install libyaml-cpp-dev 51 | ``` 52 | 53 | ## Install 54 | 55 | Use the following commands to download and compile the package. 56 | **Before compiling the project , you have to edit the ```include/global_definition.h``` change the PROJECT_PATH to yours.** 57 | Then 58 | 59 | 60 | 61 | ``` 62 | cd ~/catkin_ws/src 63 | git clone https://github.com/leo6862/ins_eskf_kitti.git 64 | cd .. 65 | catkin_make 66 | ``` 67 | 68 | ## Sample datasets 69 | Using a Kitti Dataset which has imu data frequency higher than 100HZ. 70 | here's a dataset from mine.You can download it from [baidu net disk](https://pan.baidu.com/s/15V587gC7cC6YZp_250ShEQ) 提取码: wtu9. 71 | 72 | ## Run the package 73 | 74 | 1. Run the launch file: 75 | ``` 76 | roslaunch ins_eskf ins_eskf.launch 77 | ``` 78 | 79 | 2. Play existing bag files: 80 | ``` 81 | rosbag play your-bag.bag 82 | ``` 83 | 84 | -------------------------------------------------------------------------------- /config/ins_eskf.yaml: -------------------------------------------------------------------------------- 1 | 2 | #using dataset name,heres the only alternative: kitti 3 | dataset: kitti 4 | 5 | imu_topic: /kitti/oxts/imu/extract 6 | gps_topic: /kitti/oxts/gps/fix 7 | 8 | #only works when using kitti dataset 9 | kitti/vel_topic: /kitti/oxts/gps/vel 10 | 11 | #In p v q bg ba g order 12 | init_state_std: [ 13 | 0.001,0.001,0.001, 14 | 0.001,0.001,0.001, 15 | 0.00005,0.00005,0.00005, 16 | 0.001,0.001,0.001, 17 | 0.001,0.001,0.001, 18 | 0.001,0.001,0.001 19 | ] 20 | 21 | Q_gao_xiang/noise_a: 0.001 22 | Q_gao_xiang/noise_g: 0.00003 23 | Q_gao_xiang/noise_ba: 0.001 24 | Q_gao_xiang/noise_bg: 0.00001 25 | 26 | 27 | 28 | #GPS observation noise 29 | v_x: 0.0001 30 | v_y: 0.0001 31 | v_z: 0.001 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /images/system archetecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leo6862/ins_eskf_kitti/af27146cceb33b76ad94d86ebbcafe2b8c7d3799/images/system archetecture.png -------------------------------------------------------------------------------- /images/system_architectuire.drawio: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /include/global_definition.h: -------------------------------------------------------------------------------- 1 | #ifndef INS_ESKF_GLOBAL_DEFINITION_H 2 | #define INS_ESKF_GLOBAL_DEFINITION_H 3 | #include 4 | 5 | 6 | namespace ins_eskf{ 7 | static const std::string PROJECT_PATH = "/home/yyj/yyj_ws/src/ins_eskf"; 8 | } 9 | 10 | 11 | 12 | #endif -------------------------------------------------------------------------------- /include/ins_eskf/ins_eskf.h: -------------------------------------------------------------------------------- 1 | #ifndef INS_ESKF_INS_ESKF_H 2 | #define INS_ESKF_INS_ESKF_H 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | namespace ins_eskf{ 12 | class Ins_eskf{ 13 | public: 14 | struct IMU_data{ 15 | double stamp; 16 | Eigen::Vector3f linear_acc; 17 | Eigen::Vector3f angular_velo; 18 | }; 19 | 20 | struct GPS_data{ 21 | double stamp; 22 | Eigen::Vector3f lla; 23 | }; 24 | 25 | struct State{ 26 | Eigen::Quaternionf q; 27 | Eigen::Vector3f v; 28 | Eigen::Vector3f p; 29 | Eigen::Vector3f bg; 30 | Eigen::Vector3f ba; 31 | Eigen::Vector3f g; 32 | }; 33 | 34 | struct Measure{ 35 | std::vector imu_buf; 36 | GPS_data gps_data; 37 | }; 38 | 39 | Ins_eskf(YAML::Node &_node); 40 | void recieve_imu(const IMU_data _imu_data); 41 | void recieve_gps(const GPS_data _gps_data); 42 | void recieve_measure(Measure _measure); 43 | void specify_init_state(const State& _init_state,GPS_data _init_gps_data); 44 | inline State get_state() {return state;} 45 | inline double get_state_stamp() {return state_stamp;} 46 | Eigen::Matrix3f BuildSkewMatrix(const Eigen::Vector3f& vec); 47 | 48 | private: 49 | // void initialization_kitti(); 50 | void prediction(); //imu 惯性解算获得nomial state. 51 | void forward_propagation(IMU_data _imu_data); 52 | void correction(Eigen::Vector3f _observation_gps_cartesian); 53 | Eigen::Matrix make_Fx_matrix_from_imu_and_delta_t(IMU_data _imu_data,float _delta_t); 54 | Eigen::Matrix make_Q_matrix_from_imu_and_delta_t(IMU_data _imu_data,float _delta_t); 55 | 56 | Eigen::Matrix3f Exp(const Eigen::Vector3f &ang_vel, const float &dt); 57 | 58 | private: 59 | GeographicLib::LocalCartesian geo_converter; 60 | Measure current_measure; 61 | bool initialized = false; 62 | std::string dataset = "kitti"; 63 | // double initialization_stamp; 64 | double state_stamp = -1; 65 | State state; 66 | std::mutex mtx; 67 | 68 | 69 | Eigen::Matrix state_std; //当前状态的方差阵 70 | 71 | float noise_a = -1; //这个-1是一个标志位 判断是否已经通过YAML::Node给Q中的元素赋值 72 | float noise_g; 73 | float noise_ba; 74 | float noise_bg; 75 | 76 | //variables for V .Observation noise 77 | float v_x; 78 | float v_y; 79 | float v_z; 80 | 81 | }; 82 | 83 | } 84 | 85 | 86 | #endif -------------------------------------------------------------------------------- /include/ins_eskf/ins_eskf_ros_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef INS_ESKF_INS_ESKF_ROS_WRAPPER_H 2 | #define INS_ESKF_INS_ESKF_ROS_WRAPPER_H 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | namespace ins_eskf{ 18 | 19 | class Ins_eskf_ROS_Wrapper{ 20 | 21 | public: 22 | Ins_eskf_ROS_Wrapper(ros::NodeHandle &_nh,YAML::Node& _node); 23 | 24 | private: 25 | void imu_cb(const sensor_msgs::ImuConstPtr& imu_in); 26 | void gps_cb(const sensor_msgs::NavSatFixConstPtr& gps_in); 27 | void kitti_vel_cb(const geometry_msgs::TwistStampedConstPtr& twist_in); 28 | void register_sub_pub(); 29 | void initialization_kitti(); 30 | bool synce_measure(); 31 | Ins_eskf::IMU_data imu_msg_2_data(sensor_msgs::Imu _imu_msg); 32 | Ins_eskf::GPS_data gps_msg_2_data(sensor_msgs::NavSatFix _gps_msg); 33 | sensor_msgs::NavSatFix gps_data_2_msg(Ins_eskf::GPS_data& _gps_data); 34 | 35 | nav_msgs::Odometry state_to_odom_msg(Ins_eskf::State _state,double _stamp); 36 | void visualize_res_and_kitti_gps_magnetormeter(); 37 | void DEBUG_check_synce_measure(); 38 | private: 39 | ros::NodeHandle nh; 40 | ros::Subscriber sub_imu; 41 | ros::Subscriber sub_gps; 42 | ros::Subscriber sub_vel; 43 | ros::Publisher pub_pos; 44 | std::shared_ptr p_ins_eskf; 45 | 46 | std::string dataset = "kitti"; 47 | 48 | 49 | 50 | std::string imu_topic,gps_topic; 51 | std::string kitti_vel_topic; 52 | bool initialized = false; 53 | std::mutex mtx; 54 | std::deque gps_buf; 55 | std::deque imu_buf; 56 | std::vector init_twist_buf; 57 | Ins_eskf::State init_state; 58 | Ins_eskf::Measure measure; 59 | double initialization_stamp = -1; 60 | 61 | 62 | GeographicLib::LocalCartesian geo_converter_; 63 | nav_msgs::Odometry kitti_gps_odom_msg_; 64 | ros::Publisher pub_imu_odometrty_; 65 | ros::Publisher pub_kitti_gps_odometrty_; 66 | 67 | }; 68 | 69 | 70 | } 71 | 72 | 73 | 74 | #endif -------------------------------------------------------------------------------- /launch/ins_eskf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/test_initialization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /log/ins_eskf_node.INFO: -------------------------------------------------------------------------------- 1 | ins_eskf_node.m16.yyj.log.INFO.20230125-195959.40439 -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154006.54783: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:40:06 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:40:06.013487 54783 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154020.55035: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:40:20 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:40:20.517629 55035 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:40:20.517781 55035 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:40:20.517784 55035 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:40:20.517786 55035 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:40:20.517787 55035 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:40:20.979967 55035 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154100.55474: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:41:00 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:41:00.309226 55474 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:41:00.309350 55474 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:41:00.309353 55474 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:41:00.309355 55474 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:41:00.309355 55474 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:41:00.777281 55474 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154146.55927: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:41:46 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:41:46.905611 55927 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:41:46.905786 55927 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:41:46.905795 55927 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:41:46.905799 55927 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:41:46.905802 55927 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:41:47.355285 55927 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154237.56387: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:42:37 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:42:37.739306 56387 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154304.56745: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:43:04 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:43:04.684590 56745 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:43:04.684759 56745 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:43:04.684765 56745 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:43:04.684768 56745 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:43:04.684772 56745 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:43:05.116618 56745 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154324.57023: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:43:24 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:43:24.130154 57023 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154331.57225: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:43:31 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:43:31.257416 57225 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:43:31.257539 57225 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:43:31.257541 57225 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:43:31.257542 57225 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:43:31.257544 57225 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:43:31.699205 57225 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154411.57643: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:44:11 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:44:11.289191 57643 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:44:11.289301 57643 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:44:11.289304 57643 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:44:11.289304 57643 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:44:11.289306 57643 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:44:11.670590 57643 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154500.58123: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:45:00 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:45:00.404597 58123 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:45:00.404723 58123 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:45:00.404726 58123 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:45:00.404731 58123 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:45:00.404731 58123 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:45:00.784348 58123 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154516.58390: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:45:16 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:45:16.546789 58390 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:45:16.546917 58390 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:45:16.546921 58390 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:45:16.546921 58390 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:45:16.546922 58390 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:45:17.028793 58390 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154626.59066: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:46:26 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:46:26.703759 59066 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:46:26.703886 59066 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:46:26.703888 59066 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:46:26.703891 59066 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:46:26.703891 59066 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:46:27.186290 59066 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154644.59351: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:46:44 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:46:44.520809 59351 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:46:44.520927 59351 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:46:44.520929 59351 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:46:44.520931 59351 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:46:44.520932 59351 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:46:44.934355 59351 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154852.60605: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:48:52 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:48:52.140707 60605 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:48:52.140828 60605 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:48:52.140831 60605 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:48:52.140833 60605 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:48:52.140836 60605 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:48:52.622674 60605 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-154942.61088: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:49:42 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:49:42.449234 61088 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:49:42.449401 61088 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:49:42.449407 61088 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:49:42.449410 61088 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:49:42.449414 61088 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:49:42.841773 61088 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-155103.61749: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:51:03 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:51:03.507131 61749 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:51:03.507249 61749 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:51:03.507252 61749 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:51:03.507253 61749 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:51:03.507256 61749 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:51:03.887163 61749 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-155155.62262: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:51:55 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:51:55.785599 62262 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:51:55.785733 62262 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:51:55.785737 62262 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:51:55.785737 62262 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:51:55.785739 62262 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:51:56.177209 62262 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-155247.62747: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:52:47 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:52:47.270220 62747 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:52:47.270350 62747 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:52:47.270354 62747 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:52:47.270355 62747 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:52:47.270357 62747 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:52:47.672766 62747 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-155305.63036: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:53:05 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:53:05.966040 63036 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:53:05.966167 63036 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:53:05.966169 63036 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:53:05.966171 63036 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:53:05.966172 63036 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:53:06.447739 63036 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-155620.64303: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 15:56:20 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 15:56:20.997274 64303 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 15:56:20.997520 64303 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 15:56:20.997530 64303 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 15:56:20.997534 64303 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 15:56:20.997539 64303 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 15:56:21.487769 64303 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-160014.66133: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:00:14 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:00:14.180756 66133 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:00:14.180881 66133 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:00:14.180882 66133 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:00:14.180884 66133 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:00:14.180886 66133 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:00:14.540940 66133 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-160031.66390: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:00:31 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:00:31.588984 66390 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:00:31.589115 66390 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:00:31.589118 66390 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:00:31.589119 66390 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:00:31.589123 66390 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:00:31.958257 66390 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-160312.67462: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:03:12 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:03:12.523954 67462 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-160325.67704: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:03:25 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:03:25.531810 67704 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:03:25.531939 67704 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:03:25.531941 67704 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:03:25.531944 67704 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:03:25.531946 67704 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:03:25.951306 67704 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161032.70499: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:10:32 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:10:32.923472 70499 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:10:32.923591 70499 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:10:32.923594 70499 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:10:32.923596 70499 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:10:32.923597 70499 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:10:33.342243 70499 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161136.71186: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:11:36 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:11:36.410295 71186 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:11:36.410415 71186 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:11:36.410418 71186 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:11:36.410421 71186 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:11:36.410424 71186 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:11:36.743877 71186 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161207.71694: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:12:07 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:12:07.189673 71694 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:12:07.189807 71694 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:12:07.189810 71694 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:12:07.189811 71694 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:12:07.189812 71694 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:12:07.608240 71694 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161432.72717: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:14:32 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:14:32.395910 72717 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:14:32.396031 72717 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:14:32.396034 72717 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:14:32.396035 72717 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:14:32.396037 72717 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:14:32.809029 72717 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161448.73052: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:14:48 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:14:48.114007 73052 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:14:48.114123 73052 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:14:48.114126 73052 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:14:48.114127 73052 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:14:48.114130 73052 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:14:48.545419 73052 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-161541.73564: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:15:41 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:15:41.559303 73564 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:15:41.559422 73564 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:15:41.559425 73564 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:15:41.559427 73564 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:15:41.559428 73564 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:15:41.977469 73564 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-162033.75410: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 16:20:33 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 16:20:33.562237 75410 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 16:20:33.562361 75410 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 16:20:33.562363 75410 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 16:20:33.562366 75410 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 16:20:33.562366 75410 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 16:20:33.989563 75410 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171000.88608: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:10:00 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:10:00.478066 88608 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:10:00.478180 88608 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:10:00.478183 88608 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:10:00.478184 88608 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:10:00.478185 88608 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:10:00.822140 88608 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171021.88829: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:10:21 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:10:21.713102 88829 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:10:21.713232 88829 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:10:21.713234 88829 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:10:21.713236 88829 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:10:21.713238 88829 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:10:22.182610 88829 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171221.89209: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:12:21 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:12:21.655618 89209 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171230.89388: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:12:30 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:12:30.715556 89388 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:12:30.715682 89388 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:12:30.715685 89388 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:12:30.715687 89388 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:12:30.715688 89388 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:12:31.073684 89388 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171341.89649: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:13:41 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:13:41.822798 89649 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:13:41.822919 89649 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:13:41.822924 89649 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:13:41.822925 89649 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:13:41.822926 89649 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:13:42.179030 89649 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171419.89891: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:14:19 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:14:19.090092 89891 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:14:19.090212 89891 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:14:19.090214 89891 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:14:19.090215 89891 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:14:19.090219 89891 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:14:19.554013 89891 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171516.90134: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:15:16 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:15:16.793536 90134 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:15:16.793668 90134 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:15:16.793673 90134 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:15:16.793674 90134 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:15:16.793676 90134 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:15:17.154842 90134 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171625.90410: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:16:25 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:16:25.419245 90410 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:16:25.419400 90410 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:16:25.419406 90410 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:16:25.419407 90410 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:16:25.419409 90410 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:16:25.774246 90410 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171714.90668: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:17:14 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:17:14.986251 90668 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:17:14.986364 90668 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:17:14.986367 90668 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:17:14.986369 90668 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:17:14.986371 90668 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171724.91855: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:17:24 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:17:24.421334 91855 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:17:24.421465 91855 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:17:24.421469 91855 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:17:24.421471 91855 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:17:24.421473 91855 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:17:24.759778 91855 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171741.92055: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:17:41 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:17:41.689570 92055 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230123-171748.92218: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/23 17:17:48 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0123 17:17:48.306571 92218 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0123 17:17:48.306748 92218 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0123 17:17:48.306754 92218 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0123 17:17:48.306757 92218 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0123 17:17:48.306761 92218 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0123 17:17:48.766059 92218 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /log/ins_eskf_node.m16.yyj.log.INFO.20230125-195959.40439: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/01/25 19:59:59 2 | Running on machine: m16 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0125 19:59:59.231798 40439 ins_eskf_node.cpp:22] INS ESKF STARTED. 5 | I0125 19:59:59.232398 40439 ins_eskf_ROS_wrapper.cpp:14] imu topic : /kitti/oxts/imu/extract 6 | I0125 19:59:59.232403 40439 ins_eskf_ROS_wrapper.cpp:15] gps topic : /kitti/oxts/gps/fix 7 | I0125 19:59:59.232404 40439 ins_eskf_ROS_wrapper.cpp:16] using dataset: kitti 8 | I0125 19:59:59.232406 40439 ins_eskf_ROS_wrapper.cpp:19] Using imu and vel in kitti for initialization. 9 | I0125 19:59:59.695120 40439 ins_eskf_ROS_wrapper.cpp:97] initialization completed. 10 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ins_eskf 4 | 0.0.0 5 | The ins_eskf package 6 | 7 | 8 | 9 | 10 | yyj 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 | geometry_msgs 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | geometry_msgs 57 | roscpp 58 | rospy 59 | sensor_msgs 60 | geometry_msgs 61 | roscpp 62 | rospy 63 | sensor_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /rviz/ins_eskf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | - /Odometry2 12 | Splitter Ratio: 0.6878849864006042 13 | Tree Height: 2287 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Angle Tolerance: 0.10000000149011612 59 | Class: rviz/Odometry 60 | Covariance: 61 | Orientation: 62 | Alpha: 0.5 63 | Color: 255; 255; 127 64 | Color Style: Unique 65 | Frame: Local 66 | Offset: 1 67 | Scale: 1 68 | Value: true 69 | Position: 70 | Alpha: 0.30000001192092896 71 | Color: 204; 51; 204 72 | Scale: 1 73 | Value: true 74 | Value: true 75 | Enabled: true 76 | Keep: 10000 77 | Name: Odometry 78 | Position Tolerance: 0.10000000149011612 79 | Queue Size: 10 80 | Shape: 81 | Alpha: 1 82 | Axes Length: 1 83 | Axes Radius: 0.10000000149011612 84 | Color: 138; 226; 52 85 | Head Length: 0.30000001192092896 86 | Head Radius: 0.10000000149011612 87 | Shaft Length: 1 88 | Shaft Radius: 0.05000000074505806 89 | Value: Arrow 90 | Topic: /kitti_gps_odometry 91 | Unreliable: false 92 | Value: true 93 | - Angle Tolerance: 0.10000000149011612 94 | Class: rviz/Odometry 95 | Covariance: 96 | Orientation: 97 | Alpha: 0.5 98 | Color: 255; 255; 127 99 | Color Style: Unique 100 | Frame: Local 101 | Offset: 1 102 | Scale: 1 103 | Value: true 104 | Position: 105 | Alpha: 0.30000001192092896 106 | Color: 204; 51; 204 107 | Scale: 1 108 | Value: true 109 | Value: true 110 | Enabled: true 111 | Keep: 10000 112 | Name: Odometry 113 | Position Tolerance: 0.10000000149011612 114 | Queue Size: 10 115 | Shape: 116 | Alpha: 1 117 | Axes Length: 1 118 | Axes Radius: 0.10000000149011612 119 | Color: 255; 25; 0 120 | Head Length: 0.30000001192092896 121 | Head Radius: 0.10000000149011612 122 | Shaft Length: 1 123 | Shaft Radius: 0.05000000074505806 124 | Value: Arrow 125 | Topic: /imu_odometry 126 | Unreliable: false 127 | Value: true 128 | Enabled: true 129 | Global Options: 130 | Background Color: 48; 48; 48 131 | Default Light: true 132 | Fixed Frame: odom 133 | Frame Rate: 30 134 | Name: root 135 | Tools: 136 | - Class: rviz/Interact 137 | Hide Inactive Objects: true 138 | - Class: rviz/MoveCamera 139 | - Class: rviz/Select 140 | - Class: rviz/FocusCamera 141 | - Class: rviz/Measure 142 | - Class: rviz/SetInitialPose 143 | Theta std deviation: 0.2617993950843811 144 | Topic: /initialpose 145 | X std deviation: 0.5 146 | Y std deviation: 0.5 147 | - Class: rviz/SetGoal 148 | Topic: /move_base_simple/goal 149 | - Class: rviz/PublishPoint 150 | Single click: true 151 | Topic: /clicked_point 152 | Value: true 153 | Views: 154 | Current: 155 | Class: rviz/Orbit 156 | Distance: 70.50507354736328 157 | Enable Stereo Rendering: 158 | Stereo Eye Separation: 0.05999999865889549 159 | Stereo Focal Distance: 1 160 | Swap Stereo Eyes: false 161 | Value: false 162 | Field of View: 0.7853981852531433 163 | Focal Point: 164 | X: 12.305017471313477 165 | Y: -2.8204190731048584 166 | Z: 11.23323917388916 167 | Focal Shape Fixed Size: true 168 | Focal Shape Size: 0.05000000074505806 169 | Invert Z Axis: false 170 | Name: Current View 171 | Near Clip Distance: 0.009999999776482582 172 | Pitch: 0.48479682207107544 173 | Target Frame: 174 | Yaw: 3.3621721267700195 175 | Saved: ~ 176 | Window Geometry: 177 | Displays: 178 | collapsed: false 179 | Height: 2817 180 | Hide Left Dock: false 181 | Hide Right Dock: false 182 | QMainWindow State: 000000ff00000000fd0000000400000000000003f2000009edfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000071000009ed0000018800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000157000009edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000071000009ed0000013b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000140000000066fc0100000002fb0000000800540069006d0065010000000000001400000004ee00fffffffb0000000800540069006d0065010000000000000450000000000000000000000e9f000009ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 183 | Selection: 184 | collapsed: false 185 | Time: 186 | collapsed: false 187 | Tool Properties: 188 | collapsed: false 189 | Views: 190 | collapsed: false 191 | Width: 5120 192 | X: 0 193 | Y: 63 194 | -------------------------------------------------------------------------------- /src/ins_eskf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | 5 | namespace ins_eskf{ 6 | 7 | Ins_eskf::Ins_eskf(YAML::Node& _node){ 8 | dataset = _node["dataset"].as(); 9 | 10 | std::vector state_std_vec = _node["init_state_std"].as>(); 11 | 12 | state_std.setZero(); 13 | state_std.diagonal() = Eigen::Map>(state_std_vec.data()); 14 | noise_a = _node["Q_gao_xiang/noise_a"].as(); 15 | noise_ba = _node["Q_gao_xiang/noise_ba"].as(); 16 | noise_bg= _node["Q_gao_xiang/noise_bg"].as(); 17 | noise_g = _node["Q_gao_xiang/noise_g"].as(); 18 | 19 | v_x = _node["v_x"].as(); 20 | v_y = _node["v_y"].as(); 21 | v_z = _node["v_z"].as(); 22 | 23 | } 24 | 25 | //abandoned : use recieve_measure instead 26 | void Ins_eskf::recieve_gps(const GPS_data _gps_data){ 27 | if(!initialized){ 28 | return; 29 | } 30 | 31 | 32 | } 33 | 34 | //abandoned ： use recieve_measure instead 35 | void Ins_eskf::recieve_imu(const IMU_data _imu_data){ 36 | 37 | if(!initialized){ 38 | return; 39 | } 40 | 41 | double time_inverval_ = _imu_data.stamp - state_stamp; 42 | CHECK(time_inverval_ > 0) << "imu data time ealier than state stamp,time seq error."; 43 | 44 | } 45 | 46 | void Ins_eskf::specify_init_state(const State& _init_state,Ins_eskf::GPS_data _init_gps_data){ 47 | mtx.lock(); 48 | state = _init_state; 49 | initialized = true; 50 | mtx.unlock(); 51 | 52 | state_stamp = _init_gps_data.stamp; 53 | 54 | CHECK(state_stamp != -1) << " Havn't specify initialization_stamp in ROS_wrapper."; 55 | 56 | geo_converter.Reset(_init_gps_data.lla[0],_init_gps_data.lla[1],_init_gps_data.lla[2]); 57 | 58 | } 59 | 60 | 61 | /* 62 | 此函数相当于measure的回调函数 63 | */ 64 | void Ins_eskf::recieve_measure(Measure _measure){ 65 | current_measure = _measure; 66 | 67 | CHECK(!_measure.imu_buf.empty()) << "No IMU Data in coming measure."; 68 | CHECK(_measure.gps_data.stamp - state_stamp > 0) << "Coming gps data stamp earlier than current state.Time Seq Error."; 69 | if(initialized){ 70 | CHECK(_measure.gps_data.stamp - state_stamp < 1.1) << "Too large time gap between current state and coming gps data."; 71 | } 72 | 73 | prediction(); 74 | 75 | double observation_x,observation_y,observation_z; 76 | geo_converter.Forward(_measure.gps_data.lla[0],_measure.gps_data.lla[1],_measure.gps_data.lla[2],observation_x,observation_y,observation_z); 77 | Eigen::Vector3f observation_gps_cartesian_(observation_x,observation_y,observation_z); 78 | correction(observation_gps_cartesian_); 79 | 80 | 81 | } 82 | 83 | void Ins_eskf::correction(Eigen::Vector3f _observation_gps_cartesian){ 84 | 85 | 86 | //观测矩阵的维度为 18*3 87 | Eigen::Matrix H_matrix_ = Eigen::Matrix::Zero(); 88 | H_matrix_.block<3,3>(0,0) = Eigen::Matrix3f::Identity(); 89 | Eigen::Matrix3f V_matrix_; 90 | V_matrix_ << v_x,0,0, 91 | 0,v_y,0, 92 | 0 ,0,v_z; 93 | Eigen::Matrix kalman_gain_ = state_std * H_matrix_.transpose() * ((H_matrix_ * state_std * (H_matrix_.transpose()) + V_matrix_).inverse()); 94 | Eigen::Matrix delta_x_ = kalman_gain_ * (_observation_gps_cartesian - state.p); 95 | state_std = (Eigen::Matrix::Identity() - kalman_gain_ * H_matrix_) * state_std; 96 | 97 | 98 | //进行状态的更新 将误差状态叠加到nominal state上 99 | Eigen::Vector3f error_state_rot = delta_x_.block<3,1>(6,0); 100 | float error_state_rot_norm = error_state_rot.norm(); 101 | if(error_state_rot_norm > 0.0000001){ 102 | error_state_rot /= error_state_rot_norm; 103 | error_state_rot *= std::sin(error_state_rot_norm/2); 104 | Eigen::Quaternionf error_state_q_(std::cos(error_state_rot_norm/2),error_state_rot[0],error_state_rot[1],error_state_rot[2]); 105 | state.q = state.q * error_state_q_; 106 | } 107 | state.q.normalize(); 108 | state.p += delta_x_.block<3,1>(0,0); 109 | state.v += delta_x_.block<3,1>(3,0); 110 | state.bg += delta_x_.block<3,1>(9,0); 111 | state.ba += delta_x_.block<3,1>(12,0); 112 | state.g += delta_x_.block<3,1>(15,0); 113 | } 114 | 115 | 116 | 117 | 118 | void Ins_eskf::prediction(){ 119 | /* 120 | 根据 当前最新的measure 以及 当前的状态 进行IMU惯性结算 获得nominal state 121 | */ 122 | 123 | for(int i = 0;i <= current_measure.imu_buf.size();i++){ 124 | /* 125 | 当前状态与measure之间的时序的关系： 126 | state.stamp 127 | . . .. .. . . . IMU DATA.. . . . . . .. . . GPS DATA 128 | 中间大部分的用于惯性结算的数据由插值产生 129 | 头尾两端不用插值 130 | 其中_imu_data 的stamp 表示 本次惯性结算的终点的时间戳 131 | */ 132 | IMU_data imu_data_; 133 | if(i == 0){ 134 | imu_data_ = current_measure.imu_buf[0]; 135 | } 136 | else if(i == current_measure.imu_buf.size()){ 137 | imu_data_ = current_measure.imu_buf.back(); 138 | imu_data_.stamp = current_measure.gps_data.stamp; 139 | } 140 | else{ 141 | imu_data_.linear_acc = (current_measure.imu_buf[i - 1].linear_acc + current_measure.imu_buf[i].linear_acc)/2; 142 | imu_data_.angular_velo = (current_measure.imu_buf[i - 1].angular_velo + current_measure.imu_buf[i].angular_velo)/2; 143 | imu_data_.stamp = current_measure.imu_buf[i].stamp; 144 | } 145 | // LOG(INFO) << "current integration imu data stamp = " << imu_data_.stamp; 146 | forward_propagation(imu_data_); 147 | } 148 | 149 | 150 | } 151 | 152 | void Ins_eskf::forward_propagation(IMU_data _imu_data){ 153 | /* 154 | 将当前状态 state 上根据_imu_data进行惯性解算 155 | 其中需要进行状态递推的时间gap = _imu_data.stamp - state_stamp; 156 | */ 157 | _imu_data.angular_velo -= state.bg; 158 | _imu_data.linear_acc -= state.ba; 159 | double time_gap = _imu_data.stamp - state_stamp; 160 | 161 | state.p += state.v * time_gap; 162 | state.v += (state.q * _imu_data.linear_acc + state.g) * time_gap; 163 | state.q = state.q * Exp(_imu_data.angular_velo,time_gap); 164 | 165 | state_stamp = _imu_data.stamp; 166 | 167 | // 在此处完成状态方差的递推 168 | Eigen::Matrix fx_ = make_Fx_matrix_from_imu_and_delta_t(_imu_data,time_gap); 169 | Eigen::Matrix Q_ = make_Q_matrix_from_imu_and_delta_t(_imu_data,time_gap); 170 | state_std = fx_ * state_std * fx_.transpose() + Q_; 171 | 172 | } 173 | 174 | Eigen::Matrix Ins_eskf::make_Q_matrix_from_imu_and_delta_t(IMU_data _imu_data,float _delta_t){ 175 | 176 | //q以及g的噪声为0 ， 只需要对其余的项进行设置 177 | Eigen::Matrix Q_matrix_ = Eigen::Matrix::Zero(); 178 | 179 | Q_matrix_.block<3,3>(3,3) = Eigen::Matrix3f::Identity() * (_delta_t * noise_a) * (_delta_t * noise_a); 180 | Q_matrix_.block<3,3>(6,6) = Eigen::Matrix3f::Identity() * (_delta_t * noise_g) * (_delta_t * noise_g); 181 | Q_matrix_.block<3,3>(9,9) = Eigen::Matrix3f::Identity() * (_delta_t * noise_bg * noise_bg); 182 | Q_matrix_.block<3,3>(12,12) = Eigen::Matrix3f::Identity() * (_delta_t * noise_ba * noise_ba); 183 | 184 | return Q_matrix_; 185 | 186 | } 187 | 188 | //Fx的构造方式参考高博的ESKF知乎帖子 189 | //! 高博的Fx矩阵有点问题 bg ba 的项搞混了 ,状态量和上文的公式不太一样，帖子中的F矩阵对应的状态量顺序为 p v q ba bg g 190 | Eigen::Matrix Ins_eskf::make_Fx_matrix_from_imu_and_delta_t(IMU_data _imu_data,float _delta_t){ 191 | Eigen::Matrix Fx_ = Eigen::Matrix::Identity(); 192 | Fx_.block<3,3>(0,3) = Eigen::Matrix3f::Identity() * _delta_t; 193 | Fx_.block<3,3>(3,6) = -state.q.toRotationMatrix() * BuildSkewMatrix(_imu_data.linear_acc) * _delta_t; 194 | Fx_.block<3,3>(3,12) = -state.q.toRotationMatrix() * _delta_t; 195 | Fx_.block<3,3>(3,15) = state.q.toRotationMatrix() * _delta_t; 196 | Fx_.block<3,3>(6,6) = Exp(-_imu_data.angular_velo,_delta_t); 197 | Fx_.block<3,3>(6,9) = -Eigen::Matrix3f::Identity() * _delta_t; 198 | return Fx_; 199 | } 200 | 201 | 202 | Eigen::Matrix3f Ins_eskf::BuildSkewMatrix(const Eigen::Vector3f& vec){ 203 | Eigen::Matrix3f matrix; 204 | matrix << 0.0, -vec[2], vec[1], 205 | vec[2], 0.0, -vec[0], 206 | -vec[1], vec[0], 0.0; 207 | 208 | return matrix; 209 | } 210 | 211 | //I copied it from FAST-LIO lol. Thanks to HKU. 212 | Eigen::Matrix3f Ins_eskf::Exp(const Eigen::Vector3f &ang_vel, const float &dt){ 213 | float ang_vel_norm = ang_vel.norm(); 214 | Eigen::Matrix3f Eye3 = Eigen::Matrix3f::Identity(); 215 | if (ang_vel_norm > 0.0000001) 216 | { 217 | Eigen::Vector3f r_axis = ang_vel / ang_vel_norm; 218 | Eigen::Matrix3f K = BuildSkewMatrix(r_axis); 219 | float r_ang = ang_vel_norm * dt; 220 | /// Roderigous Tranformation 221 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 222 | } 223 | else 224 | { 225 | return Eye3; 226 | } 227 | } 228 | 229 | } -------------------------------------------------------------------------------- /src/ins_eskf_ROS_wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | namespace ins_eskf{ 5 | 6 | Ins_eskf_ROS_Wrapper::Ins_eskf_ROS_Wrapper(ros::NodeHandle &_nh,YAML::Node& _node){ 7 | nh = _nh; 8 | 9 | imu_topic = _node["imu_topic"].as(); 10 | gps_topic = _node["gps_topic"].as(); 11 | dataset = _node["dataset" ].as(); 12 | 13 | 14 | LOG(INFO) << "imu topic : " << imu_topic; 15 | LOG(INFO) << "gps topic : " << gps_topic; 16 | LOG(INFO) << "using dataset: " << dataset; 17 | 18 | if(dataset == "kitti"){ 19 | LOG(INFO) << "Using imu and vel in kitti for initialization."; 20 | kitti_vel_topic = _node["kitti/vel_topic"].as(); 21 | } 22 | 23 | 24 | p_ins_eskf = std::make_shared(_node); 25 | 26 | register_sub_pub(); 27 | } 28 | 29 | void Ins_eskf_ROS_Wrapper::register_sub_pub(){ 30 | 31 | sub_imu = nh.subscribe(imu_topic,1000,&Ins_eskf_ROS_Wrapper::imu_cb,this); 32 | sub_gps = nh.subscribe(gps_topic,100,&Ins_eskf_ROS_Wrapper::gps_cb,this); 33 | 34 | 35 | pub_imu_odometrty_ = nh.advertise("imu_odometry",10); 36 | pub_kitti_gps_odometrty_ = nh.advertise("kitti_gps_odometry",10); 37 | 38 | if(dataset == "kitti"){ 39 | sub_vel = nh.subscribe(kitti_vel_topic,1000,&Ins_eskf_ROS_Wrapper::kitti_vel_cb,this); 40 | } 41 | 42 | } 43 | 44 | void Ins_eskf_ROS_Wrapper::DEBUG_check_synce_measure(){ 45 | 46 | LOG(INFO) << std::setprecision(14) << "measure.imu_buf.size() = " << measure.imu_buf.size(); 47 | for(int i = 0;i < measure.imu_buf.size();i++){ 48 | LOG(INFO) << std::setprecision(14) << "No. " << i << " imu data stamp = " << measure.imu_buf[i].stamp; 49 | } 50 | LOG(INFO) << std::setprecision(14) << "measure.gps_data.stamp = " << measure.gps_data.stamp; 51 | LOG(INFO) << "-------------------------------------------------"; 52 | 53 | 54 | } 55 | 56 | void Ins_eskf_ROS_Wrapper::gps_cb(const sensor_msgs::NavSatFixConstPtr& gps_in){ 57 | 58 | mtx.lock(); 59 | sensor_msgs::NavSatFix gps_data_ros(*gps_in); 60 | 61 | gps_buf.push_back(gps_data_ros); 62 | mtx.unlock(); 63 | 64 | 65 | 66 | if(synce_measure()){ 67 | p_ins_eskf->recieve_measure(measure); 68 | 69 | // 主要用于将组合导航的结果进行可视化的验证 与kittti数据集中的GPS+九轴IMU姿态的组合在一起的结果进行比较 70 | visualize_res_and_kitti_gps_magnetormeter(); 71 | } 72 | 73 | } 74 | 75 | void Ins_eskf_ROS_Wrapper::imu_cb(const sensor_msgs::ImuConstPtr& imu_in){ 76 | mtx.lock(); 77 | sensor_msgs::Imu imu_data_ros(*imu_in); 78 | if(!initialized){ 79 | 80 | imu_buf.push_back(imu_data_ros); 81 | //打印一下各个init容器是否正常接收到了数据 82 | // LOG(INFO) << "init_gps_buf.size() = " << init_gps_buf.size(); 83 | // LOG(INFO) << "init_imu_buf.size() = " << init_imu_buf.size(); 84 | // LOG(INFO) << "init_twist_buf.size() = " << init_twist_buf.size(); 85 | 86 | // 检查init容器中的数据的数量是否已经达到了初始化的要求 87 | if(dataset == "kitti"){ 88 | if(gps_buf.size() > 0 && imu_buf.size() > 10 && init_twist_buf.size() > 0){ 89 | initialization_kitti(); 90 | initialized = true; 91 | p_ins_eskf->specify_init_state(init_state,gps_msg_2_data(gps_buf.back())); 92 | 93 | 94 | //TODO DEBUG 95 | geo_converter_.Reset(gps_buf.back().latitude,gps_buf.back().longitude,gps_buf.back().altitude); 96 | 97 | LOG(INFO) << "initialization completed."; 98 | imu_buf.clear(); 99 | gps_buf.clear(); 100 | } 101 | } 102 | else{ 103 | // 对于其他数据集额外指定其初始化的方式 104 | LOG(INFO) << "Please specify an initialization method for dataset: " << dataset; 105 | } 106 | 107 | mtx.unlock(); 108 | 109 | } 110 | 111 | imu_buf.push_back(imu_data_ros); 112 | mtx.unlock(); 113 | 114 | 115 | Ins_eskf::IMU_data imu_data = imu_msg_2_data(*imu_in); 116 | 117 | } 118 | 119 | void Ins_eskf_ROS_Wrapper::kitti_vel_cb(const geometry_msgs::TwistStampedConstPtr& twist_in){ 120 | mtx.lock(); 121 | if(!initialized){ 122 | geometry_msgs::TwistStamped twist_in_ros(*twist_in); 123 | init_twist_buf.push_back(twist_in_ros); 124 | } 125 | mtx.unlock(); 126 | } 127 | 128 | 129 | 130 | void Ins_eskf_ROS_Wrapper::initialization_kitti(){ 131 | /* 132 | 使用init容器中的数据对 init_state进行初始化 133 | 速度的值直接使用vel的值 134 | 位置直接设为原点 135 | 旋转使用磁力计 136 | 重力直接设为(0,0,-9.81) 137 | */ 138 | CHECK(init_twist_buf.back().header.stamp == gps_buf.back().header.stamp) << "GPS fix and vel time not synchronized."; 139 | init_state.v = Eigen::Vector3f(init_twist_buf.back().twist.linear.x, 140 | init_twist_buf.back().twist.linear.y, 141 | init_twist_buf.back().twist.linear.z); 142 | init_state.p = Eigen::Vector3f::Zero(); 143 | double gps_data_stamp = gps_buf.back().header.stamp.toSec(); 144 | //遍历imu队列，寻找与gps时间相同的imu数据 145 | sensor_msgs::Imu imu_data_; 146 | int imu_data_index = 0; 147 | for(; imu_data_index < imu_buf.size();imu_data_index++){ 148 | if(imu_buf[imu_data_index].header.stamp.toSec() >= gps_data_stamp){ 149 | break; 150 | } 151 | } 152 | if(imu_data_index == imu_buf.size() || imu_data_index == 0){ 153 | LOG(INFO) << "Do not have imu data later than gps data.Initialization failed."; 154 | ros::shutdown(); 155 | } 156 | CHECK(imu_buf[imu_data_index - 1].header.stamp.toSec() < gps_data_stamp) << "imu_buf[imu_data_index - 1].header.stamp.toSec() >= gps_data_stamp,initialization failed."; 157 | float imu_data_inverval_ = imu_buf[imu_data_index].header.stamp.toSec() - imu_buf[imu_data_index-1].header.stamp.toSec(); 158 | float front_propotion = (gps_data_stamp - imu_buf[imu_data_index - 1].header.stamp.toSec())/imu_data_inverval_; 159 | float back_propotion = 1 - front_propotion; 160 | Eigen::Quaternionf q_front_imu( imu_buf[imu_data_index-1].orientation.w, 161 | imu_buf[imu_data_index-1].orientation.x, 162 | imu_buf[imu_data_index-1].orientation.y, 163 | imu_buf[imu_data_index-1].orientation.z); 164 | Eigen::Quaternionf q_back_imu( imu_buf[imu_data_index].orientation.w, 165 | imu_buf[imu_data_index].orientation.x, 166 | imu_buf[imu_data_index].orientation.y, 167 | imu_buf[imu_data_index].orientation.z); 168 | init_state.q = q_front_imu.slerp(back_propotion,q_back_imu); 169 | init_state.v = init_state.q * init_state.v; //注意原来的速度是b系下的 乘以姿态转化到东北天坐标系下 170 | init_state.bg = Eigen::Vector3f::Zero(); 171 | init_state.ba = Eigen::Vector3f::Zero(); 172 | init_state.g = Eigen::Vector3f(0,0,-9.81); 173 | 174 | initialization_stamp = gps_data_stamp; 175 | 176 | } 177 | 178 | 179 | bool Ins_eskf_ROS_Wrapper::synce_measure(){ 180 | /* 181 | 0.确保gps_buf以及 imu_buf都不为空 182 | 1.如果最新的imu数据时间戳仍然早于gps数据的时间戳，则继续等待 183 | 2.当前的imu数据已经覆盖了最老的gps数据的时间，将该帧GPS数据以及该帧GPS之前的所有IMU数据一起打包 184 | */ 185 | 186 | if(!initialized) return false; 187 | 188 | if(gps_buf.empty() || imu_buf.empty()){ 189 | return false; 190 | } 191 | 192 | double recent_gps_stamp = gps_buf.front().header.stamp.toSec(); 193 | if(imu_buf.back().header.stamp.toSec() < recent_gps_stamp){ 194 | return false; 195 | } 196 | 197 | double kitti_gps_oodm_x,kitti_gps_oodm_y,kitti_gps_oodm_z; 198 | geo_converter_.Forward(gps_buf.back().latitude,gps_buf.back().longitude,gps_buf.back().altitude,kitti_gps_oodm_x,kitti_gps_oodm_y,kitti_gps_oodm_z); 199 | kitti_gps_odom_msg_.header.stamp = gps_buf.back().header.stamp; 200 | kitti_gps_odom_msg_.header.frame_id = "odom"; 201 | kitti_gps_odom_msg_.child_frame_id = "imu"; 202 | kitti_gps_odom_msg_.pose.pose.position.x = kitti_gps_oodm_x; 203 | kitti_gps_odom_msg_.pose.pose.position.y = kitti_gps_oodm_y; 204 | kitti_gps_odom_msg_.pose.pose.position.z = kitti_gps_oodm_z; 205 | 206 | 207 | 208 | 209 | measure.imu_buf.clear(); 210 | 211 | sensor_msgs::Imu last_imu_msg; 212 | while(imu_buf.front().header.stamp.toSec() < recent_gps_stamp){ 213 | measure.imu_buf.push_back(imu_msg_2_data(imu_buf.front())); 214 | last_imu_msg = imu_buf.front(); 215 | imu_buf.pop_front(); 216 | } 217 | 218 | 219 | kitti_gps_odom_msg_.pose.pose.orientation = last_imu_msg.orientation; 220 | 221 | 222 | 223 | measure.gps_data = gps_msg_2_data(gps_buf.front()); 224 | gps_buf.pop_front(); 225 | 226 | 227 | return true; 228 | 229 | } 230 | 231 | 232 | 233 | 234 | 235 | Ins_eskf::IMU_data Ins_eskf_ROS_Wrapper::imu_msg_2_data(sensor_msgs::Imu _imu_msg){ 236 | 237 | Ins_eskf::IMU_data imu_data_; 238 | 239 | 240 | imu_data_.stamp = _imu_msg.header.stamp.toSec(); 241 | imu_data_.linear_acc << _imu_msg.linear_acceleration.x , 242 | _imu_msg.linear_acceleration.y , 243 | _imu_msg.linear_acceleration.z ; 244 | 245 | imu_data_.angular_velo << _imu_msg.angular_velocity.x , 246 | _imu_msg.angular_velocity.y , 247 | _imu_msg.angular_velocity.z ; 248 | 249 | return imu_data_; 250 | 251 | 252 | } 253 | Ins_eskf::GPS_data Ins_eskf_ROS_Wrapper::gps_msg_2_data(sensor_msgs::NavSatFix _gps_msg){ 254 | Ins_eskf::GPS_data gps_data_; 255 | gps_data_.stamp = _gps_msg.header.stamp.toSec(); 256 | gps_data_.lla << _gps_msg.latitude , _gps_msg.longitude,_gps_msg.altitude; 257 | return gps_data_; 258 | } 259 | 260 | sensor_msgs::NavSatFix Ins_eskf_ROS_Wrapper::gps_data_2_msg(Ins_eskf::GPS_data& _gps_data){ 261 | sensor_msgs::NavSatFix gps_msg_; 262 | gps_msg_.header.stamp = ros::Time(_gps_data.stamp); 263 | gps_msg_.latitude = _gps_data.lla[0]; 264 | gps_msg_.longitude = _gps_data.lla[1]; 265 | gps_msg_.altitude = _gps_data.lla[2]; 266 | 267 | return gps_msg_; 268 | } 269 | 270 | void Ins_eskf_ROS_Wrapper::visualize_res_and_kitti_gps_magnetormeter(){ 271 | /* 272 | 1.从p_ins_eskf中拿到最新的state 273 | 2.根据当前measure的GPS以及IMU的数据获得kitti的GPS里程计 274 | 3.将两者进行可视化的比较 275 | */ 276 | 277 | Ins_eskf::State current_state_ = p_ins_eskf->get_state(); 278 | double state_stamp = p_ins_eskf->get_state_stamp(); 279 | sensor_msgs::NavSatFix gps_msg_ = gps_data_2_msg(measure.gps_data); 280 | nav_msgs::Odometry imu_odom_ = state_to_odom_msg(current_state_,state_stamp); 281 | 282 | //将kitti_gps_odom_msg_以及 imu_odom_发布出去 283 | pub_imu_odometrty_.publish(imu_odom_); 284 | pub_kitti_gps_odometrty_.publish(kitti_gps_odom_msg_); 285 | 286 | } 287 | 288 | nav_msgs::Odometry Ins_eskf_ROS_Wrapper::state_to_odom_msg(Ins_eskf::State _state,double _stamp){ 289 | nav_msgs::Odometry odom_msg_; 290 | odom_msg_.header.frame_id = "odom"; 291 | odom_msg_.child_frame_id = "imu"; 292 | odom_msg_.header.stamp = ros::Time(_stamp); 293 | 294 | 295 | odom_msg_.pose.pose.position.x = _state.p[0]; 296 | odom_msg_.pose.pose.position.y = _state.p[1]; 297 | odom_msg_.pose.pose.position.z = _state.p[2]; 298 | 299 | odom_msg_.pose.pose.orientation.w = _state.q.w(); 300 | odom_msg_.pose.pose.orientation.x = _state.q.x(); 301 | odom_msg_.pose.pose.orientation.y = _state.q.y(); 302 | odom_msg_.pose.pose.orientation.z = _state.q.z(); 303 | 304 | return odom_msg_; 305 | 306 | } 307 | } -------------------------------------------------------------------------------- /src/ins_eskf_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "global_definition.h" 4 | #include 5 | 6 | 7 | #include 8 | 9 | using namespace ins_eskf; 10 | 11 | 12 | int main(int argc,char** argv){ 13 | google::InitGoogleLogging(argv[0]); 14 | FLAGS_alsologtostderr = true; 15 | FLAGS_log_dir = PROJECT_PATH + "/log"; 16 | 17 | ros::init(argc,argv,"ins_eskf_node"); 18 | ros::NodeHandle nh; 19 | 20 | YAML::Node node = YAML::LoadFile(PROJECT_PATH +"/config/ins_eskf.yaml"); 21 | 22 | LOG(INFO) << "INS ESKF STARTED."; 23 | Ins_eskf_ROS_Wrapper ins_eskf_ros(nh,node); 24 | 25 | 26 | ros::spin(); 27 | 28 | 29 | } -------------------------------------------------------------------------------- /src/test_initialization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "global_definition.h" 5 | #include 6 | 7 | 8 | using namespace ins_eskf; 9 | 10 | #define INITIALIZATION_IMU_NUM 1000 11 | 12 | std::vector imu_buf; 13 | 14 | void imu_cb(const sensor_msgs::ImuConstPtr& imu_msg){ 15 | if(imu_buf.size() < INITIALIZATION_IMU_NUM){ 16 | sensor_msgs::Imu imu_data = sensor_msgs::Imu(*imu_msg); 17 | imu_buf.push_back(imu_data); 18 | return; 19 | } 20 | 21 | 22 | 23 | if(imu_buf.size() == INITIALIZATION_IMU_NUM){ 24 | //TODO计算b系的平均加速度、平均角速度、以及重力×自转 25 | Eigen::Vector3f ave_acc = Eigen::Vector3f::Zero(); 26 | Eigen::Vector3f ave_gyr = Eigen::Vector3f::Zero(); 27 | Eigen::Vector3f b_g_c_w; 28 | for(size_t i = 0;i < imu_buf.size();i++){ 29 | ave_acc[0] += imu_buf[i].linear_acceleration.x; 30 | ave_acc[1] += imu_buf[i].linear_acceleration.y; 31 | ave_acc[2] += imu_buf[i].linear_acceleration.z; 32 | 33 | ave_gyr[0] += imu_buf[i].angular_velocity.x; 34 | ave_gyr[1] += imu_buf[i].angular_velocity.y; 35 | ave_gyr[2] += imu_buf[i].angular_velocity.z; 36 | } 37 | ave_acc /= imu_buf.size(); 38 | ave_acc *= 9.81; 39 | ave_gyr /= imu_buf.size(); 40 | b_g_c_w = ave_acc.cross(ave_gyr); 41 | // LOG(INFO) << "ave_acc = \n" << ave_acc; 42 | // LOG(INFO) << "ave_gyr = \n" << ave_gyr; 43 | // LOG(INFO) << "b_g_c_w = \n" << b_g_c_w; 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | //TODO获得n系的地球自转角速度，重力，重力×自转。 52 | double w = 7.27220521664304e-05; // 地球自转速度 53 | Eigen::Vector3f w_ie_n( w * std::cos(43.8 * M_PI / 180),0, 54 | -w * std::sin(43.8 * M_PI / 180)); 55 | Eigen::Vector3f g_n(0,0,-9.81); 56 | Eigen::Vector3f n_g_c_w = g_n.cross(w_ie_n); 57 | 58 | Eigen::Matrix3f mat1,mat2; 59 | mat1.col(0) = ave_acc; 60 | mat1.col(1) = ave_gyr; 61 | mat1.col(2) = b_g_c_w; 62 | mat2.col(0) = g_n; 63 | mat2.col(1) = w_ie_n; 64 | mat2.col(2) = n_g_c_w; 65 | 66 | Eigen::Matrix3f c_n_b = (mat1*mat2.inverse()).transpose(); 67 | 68 | float tan_yaw = c_n_b(1,0)/c_n_b(0,0); 69 | float yaw = atan(tan_yaw)/3.1415 * 180; 70 | // LOG(INFO) << "tan_yaw = " << tan_yaw; 71 | 72 | float roll = atan(-c_n_b(2,0)/sqrt(c_n_b(2,1)*c_n_b(2,1) + c_n_b(2,2) * c_n_b(2,2)))/3.1415 * 180; 73 | float pitch = atan(c_n_b(2,1)/c_n_b(2,2))/3.14 * 180; 74 | 75 | LOG(INFO) << "yaw = " << yaw; 76 | LOG(INFO) << "roll = " << roll; 77 | LOG(INFO) << "pitch = " << pitch; 78 | LOG(INFO) << "------------------------------" ; 79 | imu_buf.clear(); 80 | // ros::shutdown(); 81 | } 82 | 83 | } 84 | 85 | 86 | int main(int argc,char** argv){ 87 | google::InitGoogleLogging(argv[0]); 88 | FLAGS_alsologtostderr = true; 89 | FLAGS_log_dir = PROJECT_PATH + "/log/test_initialization"; 90 | 91 | 92 | 93 | //使用IMU进行姿态的初始化 94 | ros::init(argc,argv,"test_initialization_node"); 95 | ros::NodeHandle nh; 96 | 97 | 98 | ros::Subscriber sub_imu = nh.subscribe("/livox/imu",10000,&imu_cb); 99 | 100 | ros::spin(); 101 | } --------------------------------------------------------------------------------