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

robot_localization

asked 2016-12-11 04:57:20 -0600

tuandl
358 ●10 ●16 ●21

updated 2016-12-11 06:20:44 -0600

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:

image description

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.

image description

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:

image description

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 ...

(more)

edit retag flag offensive close merge delete

Comments

I can't fix my images so I put them here: https://drive.google.com/file/d/0B10C...https://drive.google.com/file/d/0B10C...https://drive.google.com/file/d/0B10C...

tuandl ( 2016-12-11 05:03:39 -0600 )edit

I've given you some karma. Please attach all images to this post, so we don't have to rely on your google drive.

gvdhoorn ( 2016-12-11 06:14:33 -0600 )edit

Thank you for your help! I edited my question!

tuandl ( 2016-12-11 06:21:52 -0600 )edit

For your problem, you'd better try to make the two nodes as same setting in two-D mode parameter. I saw one is false and another is true. By the way, when you are fusing odom and imu, did your imu2 has an absolute yaw w.r.t. ENU frame?

jerryzhchao ( 2016-12-19 02:32:36 -0600 )edit

@jerryzhchao Thank you for pointing that our. That's was my mistake. I was able to solve my problem. Both my IMUs have absolute yaw wrt to ENU frame.

tuandl ( 2016-12-21 05:25:31 -0600 )edit

add a comment

answered 2016-12-19 03:57:26 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

Looking at your configuration, the problem is that you have no input source measuring Z position. In addition, you are fuzing Z acceleration from the IMU. The double-integration required to turn linear acceleration into position leads to large amounts of drift. One of the many things on my list is to incorporate IMU bias calculations in the filter, but as it stands, we don't have that feature. Even if we did, using acceleration alone for a given pose variable is probably not going to ever work well. Telling navsat_transform_node to zero its altitude measurement just changes the output of navsat_transform_node; it doesn't affect the EKF at all. You still have to fuse that Z measurement in the EKF if you want to "clamp" Z to 0. You can also use two_d_mode, at least until I get my act together and put out a 2D version of the filter.

In any case, either fuse the 0 Z measurement from any of your input sources, or turn on two_d_mode. If your robot operates outside and you want your pose to be in 3D, go with the former option. If you want to assume a planar environment, go with the latter.

edit flag offensive delete link

Comments

Hi Tom, thank you for your help! I was able to solve the problem by setting the two_d_mode on for ekf map->odom node.May I ask for your elaboration on IMU bias? I understand that if we use some same type sensors, each sensor has its own bias. Does ekf weight all measurements and incline to the best?

tuandl ( 2016-12-21 05:23:22 -0600 )edit

The EKF doesn't account for biases directly, but an EKF is effectively just doing weighted averaging, and it uses the covariance matrices to do the weighting. If a sensor is less trustworthy, you can always increase its covariance values for the variables in question.

Tom Moore ( 2017-01-04 02:36:59 -0600 )edit

add a comment

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

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

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

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

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