How to fuse IMU & visual odometry using robot_localization

edit

Hello,

I am doing a project that is combining the data from visual odometry (camera ASUS Xtion Pro live) with IMU (SparkFun razor). I want to combine some sources of information to have the best odometry and i start with visual odometry and IMU. I used package rtabmap_ros node rgbd_odometry for visual odometry and package razor_imu_9dof for IMU data. To combine 2 sources, i used package robot_localization node ekf. I published 2 static tf (/base_link /camera_link; /base_link /base_imu_link). I intend to fuse x, y, z, roll, pitch, yaw from visual odometry and roll, pitch, yaw from IMU. My filtered odometry is worse than the odometry from visual odometry. The pose is drift very quick even in case that i don't move the camera. I saw the covariance matrix of filtered odometry data is always increased. The covariance of imu data is set as constant. The covariance of visual odometry is changed frequently but it's small values (the type of visual odometry is nav_msg/odometry but data is existed in geometry_msgs/PoseWithCovariance, geometry_msgs/TwistWithCovariance is always 0). I don't understand whether it affect the system or not. Can you give me an advice for configuring the filter or other solution to fuse 2 informations.

Another question is about the orientation of camera and IMU board. I setup the platform like this. Is the axis are correct?

I attached here the NodeGraph,TFTree, node robot_localization configuration and static tf publish configuration.

Thank you in advance for your attention to this matter.

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <!-- ======== STANDARD PARAMETERS ======== -->

  <param name="frequency" value="10"/>

  <param name="sensor_timeout" value="0.5"/>

  <param name="two_d_mode" value="false"/>

  <param name="map_frame" value="map"/>
  <!-- Defaults to "odom" if unspecified -->
  <param name="odom_frame" value="odom"/>
  <!-- Defaults to "base_link" if unspecified -->
  <param name="base_link_frame" value="base_link"/>
  <!-- Defaults to the value of "odom_frame" if unspecified -->
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="rtabmap/odom"/>
  <param name="imu0" value="/imu"/>

  <rosparam param="odom0_config">[true, true, true,
                                  true, true, true,
                                  false,  false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

 <rosparam param="imu0_config">[false, false, false,
                                 true,  true,  true,
                                 false, false, false,
                                 false,  false,  false,
                                 false,  false,  false]</rosparam>

  <param name="odom0_differential" value="false"/>
  <param name="imu0_differential" value="false"/>

  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="false"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="odom0_queue_size" value="2"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="odom0_pose_rejection_threshold" value="0.1"/>
  <param name="odom0_twist_rejection_threshold" value="0"/>
  <param name="imu0_pose_rejection_threshold" value="0.5"/>
  <param name="imu0_twist_rejection_threshold" value="0"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>

  <param name="debug"           value="false"/>

  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="process_noise_covariance">[0.001, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.001, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.001, 0,    0,    0,    0,     0,     0,    0,    0 ...