├── README.md ├── CMakeLists.txt ├── launch ├── bag_tag.launch~ └── bag_tag.launch ├── cmake ├── FindGLIB2.cmake ├── FindGTHREAD2.cmake └── FindEigen3.cmake ├── package.xml ├── config └── ekf_config.rviz └── src └── ekf_node.cpp /README.md: -------------------------------------------------------------------------------- 1 | # data_fusion_ekf_imu_camera 2 | 3 | A training code for data fusion. States (world frame): position, orientation(Euler angles, ZXY), velocity, gyroscope bias, acceleration bias. 4 | 5 | Dependency: ROS, Eigen 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ekf) 3 | 4 | set(CMAKE_BUILD_TYPE "") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -march=native -DEIGEN_DONT_PARALLELIZE -g") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | nav_msgs 12 | sensor_msgs 13 | ) 14 | 15 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 16 | find_package(Eigen3 REQUIRED) 17 | include_directories( 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN3_INCLUDE_DIR} 20 | ) 21 | 22 | catkin_package( 23 | ) 24 | 25 | add_executable(ekf 26 | src/ekf_node.cpp) 27 | target_link_libraries(ekf ${catkin_LIBRARIES} ) 28 | 29 | -------------------------------------------------------------------------------- /launch/bag_tag.launch~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/bag_tag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /cmake/FindGLIB2.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find the GLIB2 libraries 2 | # Once done this will define 3 | # 4 | # GLIB2_FOUND - system has glib2 5 | # GLIB2_INCLUDE_DIR - the glib2 include directory 6 | # GLIB2_LIBRARY - glib2 library 7 | 8 | # Copyright (c) 2008 Laurent Montel, 9 | # 10 | # Redistribution aqqqnd use is allowed according to the terms of the BSD license. 11 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 12 | 13 | # From the KDE source tree 14 | 15 | 16 | if(GLIB2_INCLUDE_DIR AND GLIB2_LIBRARIES) 17 | # Already in cache, be silent 18 | set(GLIB2_FIND_QUIETLY TRUE) 19 | endif(GLIB2_INCLUDE_DIR AND GLIB2_LIBRARIES) 20 | 21 | if (NOT WIN32) 22 | include(UsePkgConfig) 23 | pkgconfig(glib-2.0 _LibGLIB2IncDir _LibGLIB2LinkDir _LibGLIB2LinkFlags _LibGLIB2Cflags) 24 | endif(NOT WIN32) 25 | 26 | find_path(GLIB2_MAIN_INCLUDE_DIR glib.h 27 | PATH_SUFFIXES glib-2.0 28 | PATHS ${_LibGLIB2IncDir} ) 29 | 30 | # search the glibconfig.h include dir under the same root where the library is found 31 | 32 | find_library(GLIB2_LIBRARY 33 | NAMES glib-2.0 34 | PATHS ${_LibGLIB2LinkDir} ) 35 | 36 | 37 | get_filename_component(glib2LibDir "${GLIB2_LIBRARY}" PATH) 38 | 39 | find_path(GLIB2_INTERNAL_INCLUDE_DIR glibconfig.h 40 | PATH_SUFFIXES glib-2.0/include 41 | PATHS ${_LibGLIB2IncDir} "${glib2LibDir}" ${CMAKE_SYSTEM_LIBRARY_PATH}) 42 | 43 | set(GLIB2_INCLUDE_DIR "${GLIB2_MAIN_INCLUDE_DIR}") 44 | 45 | # not sure if this include dir is optional or required 46 | # for now it is optional 47 | if(GLIB2_INTERNAL_INCLUDE_DIR) 48 | set(GLIB2_INCLUDE_DIR ${GLIB2_INCLUDE_DIR} "${GLIB2_INTERNAL_INCLUDE_DIR}") 49 | endif(GLIB2_INTERNAL_INCLUDE_DIR) 50 | 51 | include(FindPackageHandleStandardArgs) 52 | find_package_handle_standard_args(GLIB2 DEFAULT_MSG GLIB2_LIBRARY GLIB2_MAIN_INCLUDE_DIR) 53 | 54 | mark_as_advanced(GLIB2_INCLUDE_DIR GLIB2_LIBRARY) -------------------------------------------------------------------------------- /cmake/FindGTHREAD2.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find the GTHREAD2 libraries 2 | # Once done this will define 3 | # 4 | # GTHREAD2_FOUND - system has GTHREAD2 5 | # GTHREAD2_INCLUDE_DIR - the GTHREAD2 include directory 6 | # GTHREAD2_LIBRARY - GTHREAD2 library 7 | 8 | # Copyright (c) 2008 Laurent Montel, 9 | # 10 | # Redistribution aqqqnd use is allowed according to the terms of the BSD license. 11 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 12 | 13 | # From the KDE source tree 14 | 15 | 16 | if(GTHREAD2_INCLUDE_DIR AND GTHREAD2_LIBRARIES) 17 | # Already in cache, be silent 18 | set(GTHREAD2_FIND_QUIETLY TRUE) 19 | endif(GTHREAD2_INCLUDE_DIR AND GTHREAD2_LIBRARIES) 20 | 21 | if (NOT WIN32) 22 | include(UsePkgConfig) 23 | pkgconfig(gthread-2.0 _LibGTHREAD2IncDir _LibGTHREAD2LinkDir _LibGTHREAD2LinkFlags _LibGTHREAD2Cflags) 24 | endif(NOT WIN32) 25 | 26 | find_path(GTHREAD2_MAIN_INCLUDE_DIR glib.h 27 | PATH_SUFFIXES glib-2.0 28 | PATHS ${_LibGTHREAD2IncDir} ) 29 | 30 | # search the glibconfig.h include dir under the same root where the library is found 31 | find_library(GTHREAD2_LIBRARY 32 | NAMES gthread-2.0 33 | PATHS ${_LibGTHREAD2LinkDir} ) 34 | 35 | get_filename_component(gthread2LibDir "${GTHREAD2_LIBRARY}" PATH) 36 | 37 | find_path(GTHREAD2_INTERNAL_INCLUDE_DIR glibconfig.h 38 | PATH_SUFFIXES glib-2.0/include 39 | PATHS ${_LibGTHREAD2IncDir} "${GTHREAD2LibDir}" ${CMAKE_SYSTEM_LIBRARY_PATH}) 40 | 41 | set(GTHREAD2_INCLUDE_DIR "${GTHREAD2_MAIN_INCLUDE_DIR}") 42 | 43 | # not sure if this include dir is optional or required 44 | # for now it is optional 45 | if(GTHREAD2_INTERNAL_INCLUDE_DIR) 46 | set(GTHREAD2_INCLUDE_DIR ${GTHREAD2_INCLUDE_DIR} "${GTHREAD2_INTERNAL_INCLUDE_DIR}") 47 | endif(GTHREAD2_INTERNAL_INCLUDE_DIR) 48 | 49 | include(FindPackageHandleStandardArgs) 50 | find_package_handle_standard_args(GTHREAD2 DEFAULT_MSG GTHREAD2_LIBRARY GTHREAD2_MAIN_INCLUDE_DIR) 51 | 52 | mark_as_advanced(GTHREAD2_INCLUDE_DIR GTHREAD2_LIBRARY) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ekf 4 | 0.0.0 5 | The ekf package 6 | 7 | 8 | 9 | 10 | kunyue 11 | 12 | 13 | 14 | 15 | TODO 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 | catkin 42 | roscpp 43 | nav_msgs 44 | sensor_msgs 45 | roscpp 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /config/ekf_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /ekf_odom1 10 | - /camera_odom1 11 | Splitter Ratio: 0.5 12 | Tree Height: 435 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Angle Tolerance: 0.1 54 | Class: rviz/Odometry 55 | Color: 255; 25; 0 56 | Enabled: true 57 | Keep: 100 58 | Length: 1 59 | Name: ekf_odom 60 | Position Tolerance: 0.1 61 | Topic: /ekf/ekf_odom 62 | Value: true 63 | - Angle Tolerance: 0.1 64 | Class: rviz/Odometry 65 | Color: 85; 255; 0 66 | Enabled: true 67 | Keep: 100 68 | Length: 1 69 | Name: camera_odom 70 | Position Tolerance: 0.1 71 | Topic: /tage_detector/tag_odom 72 | Value: true 73 | Enabled: true 74 | Global Options: 75 | Background Color: 48; 48; 48 76 | Fixed Frame: map 77 | Frame Rate: 30 78 | Name: root 79 | Tools: 80 | - Class: rviz/Interact 81 | Hide Inactive Objects: true 82 | - Class: rviz/MoveCamera 83 | - Class: rviz/Select 84 | - Class: rviz/FocusCamera 85 | - Class: rviz/Measure 86 | - Class: rviz/SetInitialPose 87 | Topic: /initialpose 88 | - Class: rviz/SetGoal 89 | Topic: /move_base_simple/goal 90 | - Class: rviz/PublishPoint 91 | Single click: true 92 | Topic: /clicked_point 93 | Value: true 94 | Views: 95 | Current: 96 | Class: rviz/Orbit 97 | Distance: 19.7382 98 | Enable Stereo Rendering: 99 | Stereo Eye Separation: 0.06 100 | Stereo Focal Distance: 1 101 | Swap Stereo Eyes: false 102 | Value: false 103 | Focal Point: 104 | X: 0 105 | Y: 0 106 | Z: 0 107 | Name: Current View 108 | Near Clip Distance: 0.01 109 | Pitch: 0.785398 110 | Target Frame: 111 | Value: Orbit (rviz) 112 | Yaw: 0.785398 113 | Saved: ~ 114 | Window Geometry: 115 | Displays: 116 | collapsed: false 117 | Height: 716 118 | Hide Left Dock: false 119 | Hide Right Dock: false 120 | QMainWindow State: 000000ff00000000fd00000004000000000000013c00000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000242000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002590000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 121 | Selection: 122 | collapsed: false 123 | Time: 124 | collapsed: false 125 | Tool Properties: 126 | collapsed: false 127 | Views: 128 | collapsed: false 129 | Width: 1200 130 | X: 264 131 | Y: 14 132 | -------------------------------------------------------------------------------- /src/ekf_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | ros::Publisher odom_pub; 15 | ros::Publisher msm_pub; 16 | const double _PI = acos(-1); 17 | 18 | #ifndef _EKF_STATE_DIM_NAME 19 | #define _EKF_STATE_DIM_NAME 20 | 21 | #define _STT_LEN 15 22 | 23 | #define _POS_BEG 0 24 | #define _POS_LEN 3 25 | #define _POS_END 3 26 | 27 | #define _ORT_BEG 3 28 | #define _ORT_LEN 3 29 | #define _ORT_END 6 30 | 31 | #define _VEL_BEG 6 32 | #define _VEL_LEN 3 33 | #define _VEL_END 9 34 | 35 | #define _B_G_BEG 9 36 | #define _B_G_LEN 3 37 | #define _B_G_END 12 38 | 39 | #define _B_A_BEG 12 40 | #define _B_A_LEN 3 41 | #define _B_A_END 15 42 | 43 | #define _DIM_X 0 44 | #define _DIM_Y 1 45 | #define _DIM_Z 2 46 | #define _DIM_W 3 47 | #define _DIM_LEN 3 48 | 49 | 50 | #define _P_M_BEG 0 51 | #define _P_M_LEN 3 52 | #define _P_M_END 3 53 | 54 | #define _O_M_BEG 3 55 | #define _O_M_LEN 3 56 | #define _O_M_END 6 57 | 58 | #define _N_G_BEG 0 59 | #define _N_G_LEN 3 60 | #define _N_A_BEG 3 61 | #define _N_A_LEN 3 62 | #define _NBG_BEG 6 63 | #define _NBG_LEN 3 64 | #define _NBA_BEG 9 65 | #define _NBA_LEN 3 66 | 67 | #endif 68 | 69 | VectorXd x = VectorXd::Zero(_STT_LEN); 70 | MatrixXd cov_x = MatrixXd::Identity(_STT_LEN, _STT_LEN) * 10; 71 | bool _is_first_imu = true; 72 | ros::Time _last_imu_stamp; 73 | 74 | MatrixXd RPYtoR(double roll, double pitch, double yaw) 75 | { 76 | Matrix3d R(_DIM_LEN, _DIM_LEN); 77 | R << 78 | cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), -cos(roll)*sin(yaw), cos(yaw)*sin(pitch) + cos(pitch)*sin(roll)*sin(yaw), 79 | cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll), 80 | -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll); 81 | return R; 82 | } 83 | 84 | MatrixXd RPYtoR(const VectorXd &x) 85 | { 86 | return RPYtoR(x(0), x(1), x(2)); 87 | } 88 | 89 | Matrix3d RPYtoG(double roll, double pitch, double yaw) 90 | { 91 | MatrixXd G(_DIM_LEN, _DIM_LEN); 92 | G << 93 | cos(pitch), 0, -cos(roll)*sin(pitch), 94 | 0, 1, sin(roll), 95 | sin(pitch), 0, cos(pitch)*cos(roll); 96 | return G; 97 | } 98 | 99 | Matrix3d RPYtoG(const VectorXd &x) 100 | { 101 | return RPYtoG(x(0), x(1), x(2)); 102 | } 103 | 104 | Matrix3d RPYtoInvG(const VectorXd &x) 105 | { 106 | double roll = x(0), pitch = x(1); 107 | MatrixXd D(_DIM_LEN, _DIM_LEN); 108 | D << 109 | cos(pitch), 0, sin(pitch), 110 | (sin(pitch)*sin(roll))/cos(roll), 1, -(cos(pitch)*sin(roll))/cos(roll), 111 | -sin(pitch)/cos(roll), 0, cos(pitch)/cos(roll); 112 | return D; 113 | } 114 | 115 | double sqr(double x) 116 | { 117 | return x * x; 118 | } 119 | 120 | MatrixXd RPYtoInvGdR(const VectorXd &x) 121 | { 122 | double roll = x(0), pitch = x(1); 123 | MatrixXd D(_DIM_LEN, _DIM_LEN); 124 | D << 125 | 0, 0, 0, 126 | -sin(pitch)/(sqr(sin(roll)) - 1), 0, -cos(pitch)/sqr(cos(roll)), 127 | (sin(pitch)*sin(roll))/(sqr(sin(roll)) - 1), 0, (cos(pitch)*sin(roll))/sqr(cos(roll)); 128 | return D; 129 | } 130 | 131 | MatrixXd RPYtoInvGdP(const VectorXd &x) 132 | { 133 | double roll = x(0), pitch = x(1); 134 | MatrixXd D(_DIM_LEN, _DIM_LEN); 135 | D << 136 | -sin(pitch), 0, cos(pitch), 137 | (cos(pitch)*sin(roll))/cos(roll), 0, (sin(pitch)*sin(roll))/cos(roll), 138 | -cos(pitch)/cos(roll), 0, -sin(pitch)/cos(roll); 139 | return D; 140 | } 141 | 142 | MatrixXd RPYtoInvGdY(const VectorXd &x) 143 | { 144 | return Matrix3d::Zero(_DIM_LEN, _DIM_LEN); 145 | } 146 | 147 | MatrixXd RPYtoRdR(const VectorXd &x) 148 | { 149 | double roll = x(0), pitch = x(1), yaw = x(2); 150 | MatrixXd D(_DIM_LEN, _DIM_LEN); 151 | D << 152 | -cos(roll)*sin(pitch)*sin(yaw), sin(roll)*sin(yaw), cos(pitch)*cos(roll)*sin(yaw), 153 | cos(roll)*cos(yaw)*sin(pitch), -cos(yaw)*sin(roll), -cos(pitch)*cos(roll)*cos(yaw), 154 | sin(pitch)*sin(roll), cos(roll), -cos(pitch)*sin(roll); 155 | return D; 156 | } 157 | 158 | MatrixXd RPYtoRdP(const VectorXd &x) 159 | { 160 | double roll = x(0), pitch = x(1), yaw = x(2); 161 | MatrixXd D(_DIM_LEN, _DIM_LEN); 162 | D << 163 | - cos(yaw)*sin(pitch) - cos(pitch)*sin(roll)*sin(yaw), 0, cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), 164 | cos(pitch)*cos(yaw)*sin(roll) - sin(pitch)*sin(yaw), 0, cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), 165 | -cos(pitch)*cos(roll), 0, -cos(roll)*sin(pitch); 166 | return D; 167 | } 168 | 169 | MatrixXd RPYtoRdY(const VectorXd &x) 170 | { 171 | double roll = x(0), pitch = x(1), yaw = x(2); 172 | MatrixXd D(_DIM_LEN, _DIM_LEN); 173 | D << 174 | - cos(pitch)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll), -cos(roll)*cos(yaw), cos(pitch)*cos(yaw)*sin(roll) - sin(pitch)*sin(yaw), 175 | cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), -cos(roll)*sin(yaw), cos(yaw)*sin(pitch) + cos(pitch)*sin(roll)*sin(yaw), 176 | 0, 0, 0; 177 | return D; 178 | } 179 | 180 | VectorXd RotMtoRPY(const MatrixXd &m) 181 | { 182 | VectorXd rpy(3); 183 | double yaw = atan2(m(1,0), m(0,0)); 184 | double pitch = atan2(-m(2,0), m(0,0)*cos(yaw) * m(1,0)*sin(yaw)); 185 | double roll = atan2(m(0,2)*sin(yaw)-m(1,2)*cos(yaw),-m(0,1)*sin(yaw)+m(1,1)*cos(yaw)); 186 | 187 | roll = asin(m(1, 2)); 188 | yaw = atan2(-m(1, 0)/cos(roll), m(1, 1)/cos(roll)); 189 | pitch = atan2(-m(0, 2)/cos(roll), m(2, 2)/cos(roll)); 190 | 191 | rpy << roll, pitch, yaw; 192 | return rpy; 193 | } 194 | 195 | void pubOdometry() 196 | { 197 | nav_msgs::Odometry odom; 198 | { 199 | odom.header.stamp = _last_imu_stamp; 200 | odom.header.frame_id = "map"; 201 | 202 | odom.pose.pose.position.x = x(0); 203 | odom.pose.pose.position.y = x(1); 204 | odom.pose.pose.position.z = x(2); 205 | 206 | Quaterniond q; 207 | q = RPYtoR(x(3), x(4), x(5)).block<3,3>(0, 0); 208 | odom.pose.pose.orientation.x = q.x(); 209 | odom.pose.pose.orientation.y = q.y(); 210 | odom.pose.pose.orientation.z = q.z(); 211 | odom.pose.pose.orientation.w = q.w(); 212 | } 213 | 214 | //ROS_WARN("[update] publication done"); 215 | ROS_WARN_STREAM("[final] b_g = " << x.segment(_B_G_BEG, _B_G_LEN).transpose()); 216 | ROS_WARN_STREAM("[final] b_a = " << x.segment(_B_A_BEG, _B_A_LEN).transpose() << endl); 217 | ///ROS_WARN_STREAM("[final] cov_x = " << endl << cov_x << endl); 218 | odom_pub.publish(odom); 219 | } 220 | 221 | void imu_callback(const sensor_msgs::Imu::ConstPtr &msg) 222 | { 223 | 224 | // remember the first stamp; 225 | if (_is_first_imu) 226 | { 227 | _is_first_imu = false; 228 | _last_imu_stamp = msg->header.stamp; 229 | return ; 230 | } 231 | double dt = (msg->header.stamp - _last_imu_stamp).toSec(); 232 | _last_imu_stamp = msg->header.stamp; 233 | //ROS_WARN("[propagate] time stamp done"); 234 | 235 | VectorXd w_m(_DIM_LEN), a_m(_DIM_LEN); 236 | MatrixXd Q_t = MatrixXd::Zero(_DIM_LEN << 2, _DIM_LEN << 2); 237 | { // get imu measurement 238 | w_m << 239 | msg->angular_velocity.x, 240 | msg->angular_velocity.y, 241 | msg->angular_velocity.z; 242 | 243 | a_m << 244 | msg->linear_acceleration.x, 245 | msg->linear_acceleration.y, 246 | msg->linear_acceleration.z; 247 | 248 | //ROS_WARN("[propagate] w_m, a_m done"); 249 | 250 | Q_t(0, 0) = (5.0/180.0)*acos(-1)*(5.0/180.0)*acos(-1); 251 | Q_t(1, 1) = (5.0/180.0)*acos(-1)*(5.0/180.0)*acos(-1); 252 | Q_t(2, 2) = (5.0/180.0)*acos(-1)*(5.0/180.0)*acos(-1); 253 | Q_t(3, 3) = 1 * 1; 254 | Q_t(4, 4) = 1 * 1; 255 | Q_t(5, 5) = 1 * 1; 256 | Q_t(6, 6) = 0.5*0.5; 257 | Q_t(7, 7) = 0.5*0.5; 258 | Q_t(8, 8) = 0.5*0.5; 259 | Q_t(9, 9) = 0.5*0.5; 260 | Q_t(10, 10) = 0.5*0.5; 261 | Q_t(11, 11) = 0.5*0.5; 262 | //ROS_WARN("[propagate] Q_t done"); 263 | } 264 | 265 | //ROS_WARN("[propagate] preparation done"); 266 | 267 | { // update the mean value 268 | VectorXd x_dot = VectorXd::Zero(_STT_LEN); 269 | 270 | // update position 271 | x_dot.segment(_POS_BEG, _POS_LEN) = x.segment(_VEL_BEG, _VEL_LEN); 272 | 273 | // update orientation 274 | x_dot.segment(_ORT_BEG, _ORT_LEN) = 275 | RPYtoInvG(x.segment(_ORT_BEG, _ORT_LEN)) * (w_m - x.segment(_B_G_BEG, _B_G_LEN)); 276 | 277 | // update velocity 278 | VectorXd g = VectorXd::Zero(_DIM_LEN); 279 | g(2) = 9.8; 280 | x_dot.segment(_VEL_BEG, _VEL_LEN) = g + 281 | RPYtoR(x.segment(_ORT_BEG, _ORT_LEN)) * (a_m - x.segment(_B_A_BEG, _B_A_LEN)); 282 | 283 | x += x_dot * dt; 284 | } 285 | 286 | //ROS_WARN("[propagate] mean value propagation done"); 287 | 288 | // update the covariance 289 | { 290 | //> 1. compute the A_t, F_t matrix 291 | MatrixXd A_t = MatrixXd::Zero(_STT_LEN, _STT_LEN); 292 | 293 | // first block row 294 | A_t.block(_POS_BEG, _VEL_BEG, _POS_LEN, _VEL_LEN) << Matrix3d::Identity(); 295 | 296 | //ROS_WARN("[propagate] first block row done"); 297 | 298 | // second block row 299 | auto rpy = x.segment(_ORT_BEG, _ORT_LEN); 300 | auto b_g = x.segment(_B_G_BEG, _B_G_LEN); 301 | A_t.block(_ORT_BEG, _ORT_BEG, _ORT_LEN, _ORT_LEN) << 302 | RPYtoInvGdR(rpy) * (w_m - b_g), 303 | RPYtoInvGdP(rpy) * (w_m - b_g), 304 | RPYtoInvGdY(rpy) * (w_m - b_g); 305 | A_t.block(_ORT_BEG, _B_G_BEG, _ORT_LEN, _B_G_LEN) << 306 | -RPYtoInvG(rpy); 307 | 308 | //ROS_WARN("[propagate] second block row done"); 309 | 310 | // third block row 311 | auto b_a = x.segment(_B_A_BEG, _B_A_LEN); 312 | A_t.block(_VEL_BEG, _ORT_BEG, _VEL_LEN, _ORT_LEN) << 313 | RPYtoRdR(rpy) * (a_m - b_a), 314 | RPYtoRdP(rpy) * (a_m - b_a), 315 | RPYtoRdY(rpy) * (a_m - b_a); 316 | A_t.block(_VEL_BEG, _B_A_BEG, _VEL_LEN, _B_A_LEN) << 317 | -RPYtoR(rpy); 318 | 319 | //ROS_WARN("[propagate] A_t done"); 320 | 321 | //> 2. compute the U_t, V_t matrix 322 | MatrixXd U_t = MatrixXd::Zero(_STT_LEN, _DIM_LEN << 2); 323 | 324 | U_t.block(_ORT_BEG, _N_G_BEG, _ORT_LEN, _N_G_LEN) << 325 | -RPYtoInvG(rpy); 326 | U_t.block(_VEL_BEG, _N_A_BEG, _VEL_LEN, _N_A_LEN) << 327 | -RPYtoR(rpy); 328 | U_t.block(_B_G_BEG, _NBG_BEG, _B_G_LEN, _NBG_LEN) << 329 | Matrix3d::Identity(); 330 | U_t.block(_B_A_BEG, _NBA_BEG, _B_A_LEN, _NBA_LEN) << 331 | Matrix3d::Identity(); 332 | 333 | 334 | //ROS_WARN("[propagate] U_t done"); 335 | MatrixXd F_t = A_t * dt, V_t = U_t * dt; 336 | for (int i = 0; i < _STT_LEN; ++i) F_t(i, i) += 1.0; 337 | 338 | //ROS_WARN_STREAM("A_t = endl" << endl << A_t); 339 | //ROS_WARN_STREAM("F_t = endl" << endl << F_t); 340 | //ROS_WARN_STREAM("U_t = endl" << endl << U_t); 341 | //ROS_WARN_STREAM("V_t = endl" << endl << V_t); 342 | 343 | //ROS_WARN_STREAM("cov_x = " << endl << cov_x); 344 | //ROS_WARN_STREAM("cov_x_1 = " << (F_t * cov_x * F_t.transpose())); 345 | //ROS_WARN_STREAM("cov_x_2 = " << (V_t * Q_t * V_t.transpose())); 346 | cov_x = F_t * cov_x * F_t.transpose() + V_t * Q_t * V_t.transpose(); 347 | } 348 | pubOdometry(); 349 | //ROS_WARN("[propagate] covariance propagation done"); 350 | } 351 | 352 | //Rotation from the camera frame to the IMU frame 353 | 354 | void odom_callback(const nav_msgs::Odometry::ConstPtr &msg) 355 | { 356 | //your code for update 357 | //camera position in the IMU frame = (0, -0.05, +0.02) 358 | //camera orientaion in the IMU frame = Quaternion(0, 0, -1, 0); w x y z, respectively 359 | VectorXd z = VectorXd::Zero(_DIM_LEN << 1); 360 | MatrixXd R = MatrixXd::Zero(_DIM_LEN << 1, _DIM_LEN << 1); 361 | { // get the measurement from the msg 362 | auto q = msg->pose.pose.orientation; 363 | auto p = msg->pose.pose.position; 364 | Quaterniond q_ic(0, 0, -1, 0); 365 | Quaterniond q_cw(q.w, q.x, q.y, q.z); 366 | 367 | MatrixXd g_cw = MatrixXd::Zero(4, 4), g_ic = MatrixXd::Zero(4, 4); 368 | 369 | g_cw.block(0, 0, 3, 3) << q_cw.toRotationMatrix(); 370 | g_cw.block(0, 3, 3, 1) << p.x, p.y, p.z; 371 | g_cw(3, 3) = 1.0; 372 | 373 | g_ic.block(0, 0, 3, 3) << q_ic.toRotationMatrix(); 374 | g_ic.block(0, 3, 3, 1) << 0, -0.05, +0.02; 375 | g_ic(3, 3) = 1.0; 376 | 377 | MatrixXd g_wi = (g_ic * g_cw).inverse(); 378 | //ROS_WARN_STREAM("g_ic = " << endl << g_ic); 379 | //ROS_WARN_STREAM("g_cw = " << endl << g_cw); 380 | //ROS_WARN_STREAM("g_wi = " << endl << g_wi); 381 | 382 | 383 | z << 384 | g_wi.block(0, 3, 3, 1), 385 | RotMtoRPY(g_wi.block(0, 0, 3, 3)); 386 | 387 | nav_msgs::Odometry odom = *msg; 388 | Quaterniond qq ; 389 | qq = RPYtoR(z.segment(3, 3)).block<3, 3>(0, 0); 390 | odom.pose.pose.position.x = z(0); 391 | odom.pose.pose.position.y = z(1); 392 | odom.pose.pose.position.z = z(2); 393 | odom.pose.pose.orientation.w = qq.w(); 394 | odom.pose.pose.orientation.x = qq.x(); 395 | odom.pose.pose.orientation.y = qq.y(); 396 | odom.pose.pose.orientation.z = qq.z(); 397 | msm_pub.publish(odom); 398 | 399 | #if 1 400 | R(0, 0) = 0.02 * 0.02; 401 | R(1, 1) = 0.02 * 0.02; 402 | R(2, 2) = 0.02 * 0.02; 403 | R(3, 3) = (2.0/180.0)*acos(-1) * (2.0/180.0)*acos(-1); 404 | R(4, 4) = (2.0/180.0)*acos(-1) * (2.0/180.0)*acos(-1); 405 | R(5, 5) = (2.0/180.0)*acos(-1) * (2.0/180.0)*acos(-1); 406 | #endif 407 | } 408 | 409 | //ROS_WARN("[update] preparation done"); 410 | 411 | MatrixXd C_t = MatrixXd::Zero(_DIM_LEN << 1, _STT_LEN); 412 | MatrixXd K_t = MatrixXd::Zero(_STT_LEN, _DIM_LEN << 1); 413 | { // compute the C_t and K_t 414 | C_t.block(_P_M_BEG, _POS_BEG, _P_M_LEN, _POS_LEN) << 415 | Matrix3d::Identity(); 416 | C_t.block(_O_M_BEG, _ORT_BEG, _O_M_LEN, _ORT_LEN) << 417 | Matrix3d::Identity(); 418 | K_t = cov_x * C_t.transpose() * (C_t * cov_x * C_t.transpose() + R).inverse(); 419 | } 420 | 421 | //ROS_WARN("[update] C_t K_t done"); 422 | #if 1 423 | { 424 | VectorXd error = z - C_t * x; 425 | for (int i = _O_M_BEG; i < _O_M_END; ++i) 426 | { 427 | if (error(i) < -_PI) 428 | { 429 | //ROS_WARN_STREAM("error = " << error.transpose() << endl); 430 | error(i) += 2.0 * _PI; 431 | //ROS_WARN_STREAM("ERROR = " << error.transpose() << endl); 432 | } 433 | if (error(i) > _PI) 434 | { 435 | //ROS_WARN_STREAM("error = " << error.transpose() << endl); 436 | error(i) -= 2.0 * _PI; 437 | //ROS_WARN_STREAM("ERROR = " << error.transpose() << endl); 438 | } 439 | 440 | } 441 | x = x + K_t * error; 442 | cov_x = cov_x - K_t * C_t * cov_x; 443 | } 444 | #endif 445 | //ROS_WARN("[update] x cov_x done"); 446 | 447 | //pubOdometry(); 448 | } 449 | 450 | int main(int argc, char **argv) 451 | { 452 | ros::init(argc, argv, "ekf"); 453 | ros::NodeHandle n("~"); 454 | 455 | ros::Subscriber s1 = n.subscribe("imu", 100, imu_callback); 456 | ros::Subscriber s2 = n.subscribe("tag_odom", 100, odom_callback); 457 | 458 | odom_pub = n.advertise("ekf_odom", 100); 459 | msm_pub = n.advertise("rcv_odom", 100); 460 | 461 | ros::spin(); 462 | } 463 | --------------------------------------------------------------------------------