robot_localization problem for fuse imu and odometry

edit

Hi all,

I'm trying to fuse IMU and VISP for a drone in Gazebo. The IMU is official plug-in from Gazebo tutorials (GazeboRosImuSensor), and VISP is a stand alone package which can return the pose (x, y, z, quaternion) of camera w.r.t. Apriltag. I have transformed the VISP-data into nav-msgs/Odometry, but I don't have twist data, so I didn't set it.

The update rate of IMU and VISP are set to 200Hz and 10Hz. The problem is the IMU seems like can't "catch up" the movement. The result of EKF jumped every 0.1 secs. For example, the pose increased every 0.005 secs very slowly (from 0.23 to 0.25), and every 0.1 secs the pose has a great increase (form 0.25 to 0.40). I think the possible reason is: every 0.1 secs the EKF update it's current measurement from VISP, which is more likely to be the real state of robot, but the IMU can't predict the robot pose correctly.

Are there anyone have met similar problems? Or anyone has some ideas about that? Thank you for your help.

The msgs and configuration of robot_localization are below. I'm new to this forum and can't upload image and files,sorry about that.

header: 
  seq: 118
  stamp: 
    secs: 21
    nsecs: 679000000
  frame_id: "odom"
child_frame_id: "base_link"

pose: 
  pose: 
    position: 
      x: -5.8628621273e-05
      y: 3.73823519429e-05
      z: 0.221914839373
    orientation: 
      x: 8.49205216555e-06
      y: 9.02411111027e-07
      z: -1.72706129435e-05
      w: 0.999999999814

  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]

A twist:

twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0

  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

The config file of robot_localization is here:

 - frequency: 200
 - sensor_timeout: 1.0
 - two_d_mode: false
 - transform_time_offset: 0.0
 - transform_timeout: 0.0
 - print_diagnostics: true
 - publish_tf: true
 - odom_frame: odom
 - base_link_frame: base_link
 - world_frame: odom
 - odom0: /vo
 - odom0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
               false, false, false,
               false, false, false]
 - odom0_differential: false
 - odom0_relative: false
 - imu0: /imu
 - imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
 - imu0_differential: false
 - imu0_relative: true ...