robot_localization: map -> odom drifts in z-direction with zero-altitude set to true

edit

Hello everyone,

I have been playing with robot_localization package recently. I tried to fuse a GPS, 2 identical IMUs (um7) and odometry (from a Seekur robot). I went through the r_l's tutorial and made sure that all the data are properly configured: 1) IMUs report in ENU coordinate frame. IMU's frame_id are imu1_link, imu2_link for the first IMU and second IMU respectively. One note here, I used this fork for my IMUs. As it appears, this driver also converts the imu's coordinate frame to

2) odometry topic that my robot publishes has correct frame_id as odom and child_frame_id as base_link. According to this question, I disable odom->base_link transform that is published by the robot's driver.

I know that imu1 drifts a lot and imu2 doesn't. However, I think r_l is still able to handle this situation.

I started to fuse just imu 2, GPS and robot's odometry and the result looks fine to me. I attached the picture here:

I did a second test when fusing just imu1, gps and robot's odometry. The result is as expected in the second picture. The robot moves in a wrong direction as I only feed drifted yaw measurement from IMU to the ekf_localization_node.

However, when I tried to fuse everything together: 2 IMUs, 1 GPS and 1 robot's odometry, the map->odom drifts in z direction aggressively. I tried everything that I could think off but can't solve this problem. The result is here:

As you can see, the odom frame "flies up" indefinitely.

Here is the link to my bag, configuration and launch files: https://drive.google.com/file/d/0B10C...

I post my configuration file here with covariance matrices cutoff to make thing easier to read:

    ekf_se_odom:
      frequency: 20
      sensor_timeout: 0.1
      two_d_mode: true
      transform_time_offset: 0.0
      print_diagnostics: true
      debug: true
      publish_tf: true

      map_frame: map
      odom_frame: odom
      base_link_frame: base_link
      world_frame: odom

      odom0: /RosAria/pose
      odom0_config: [ false, false, false, 
                      false, false, false, 
                      true, false, false, 
                      false, false, false,
                      false, false, false]
      odom0_queue_size: 10
      odom0_nodelay: true
      odom0_differential: false
      odom0_relative: false


      imu0: /imu2/data
      imu0_config: [false, false, false,
                    true, true, true,
                    false, false, false,
                    true, true, true,
                    false, false, false]
      imu0_nodelay: true
      imu0_differential: false
      imu0_relative: true
      imu0_queue_size: 10
      imu0_remove_gravitational_acceleration: true

      imu1: /imu1/data
      imu1_config: [false, false, false,
                    true, true, true,
                    false, false, false,
                    true, true, true,
                    true, true, true]
      imu1_nodelay: true
      imu1_differential: false
      imu1_relative: true
      imu1_queue_size: 10
      imu1_remove_gravitational_acceleration: true

      use_control: false

 ekf_se_map:
  frequency: 20
  sensor_timeout: 0.1
  two_d_mode: false
  transform_time_offset: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

  odom0: /RosAria/pose
  odom0_config: [ false, false, false, 
                  false, false, false, 
                  true, false, false, 
                  false, false, false,
                  false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  odom1: /gps_odom
  odom1_config: [true, true, false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

  imu0: /imu2/data
  imu0_config: [false, false, false,
                true, true, true,
                false, false, false,
                true, true, true,
                false ...