0
$\begingroup$

Rosanswers logo

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/0B10Cfn7wCwXfYVBWdE00b3FjX1k/view?usp=sharing

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

navsat_transform:
  frequency: 2
  delay: 3.0
  magnetic_declination_radians: 0.23614  # For lat/long 39.5296 N, 119.8138W
  yaw_offset: 0.354 # IMU reads 0.354 facing east
  zero_altitude: true
  broadcast_utm_transform: true
  publish_filtered_gps: false
  use_odometry_yaw: false
  wait_for_datum: false

Could anyone show me what I did wrong or there was something didn't understand correctly?

Thanks in advance!


Originally posted by tuandl on ROS Answers with karma: 358 on 2016-12-11

Post score: 0


Original comments

Comment by tuandl on 2016-12-11:
I can't fix my images so I put them here: https://drive.google.com/file/d/0B10Cfn7wCwXfTGVLTC1QSEZEQmM/view?usp=sharing https://drive.google.com/file/d/0B10Cfn7wCwXfb1F5WFRDbXY3eE0/view?usp=sharing https://drive.google.com/file/d/0B10Cfn7wCwXfczEyTmloNUJ3WGs/view?usp=sharing

Comment by gvdhoorn on 2016-12-11:
I've given you some karma. Please attach all images to this post, so we don't have to rely on your google drive.

Comment by tuandl on 2016-12-11:
Thank you for your help! I edited my question!

Comment by jerryzhchao on 2016-12-19:
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?

Comment by tuandl on 2016-12-21:
@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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Tom Moore with karma: 13689 on 2016-12-19

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by tuandl on 2016-12-21:
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?

Comment by Tom Moore on 2017-01-04:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.