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.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
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.

Improve this answer
$\endgroup$