Problem when fusing IMU and Odometry

Question

Hi, I am trying to use robot localization to fuse the data from an XSense IMU and Odometry Source (Pioneer). The output is not what i expect actually. The resulting odometry looks similar (almost exact) to the odometry of the pioneer? So the filter is not improving or at least changing the result?

This is my launch file:

<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="two_d_mode" value="true"/>

<param name="imu0" value="imu/data"/>
<rosparam param="imu0_config">[false, false, false,
                           false, false,  true,
                           false, false, false,
                           false, false, false,
                           false,  false, false]</rosparam>
<param name="imu0_differential" value="true"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>

<param name="odom0" value="/odom/fixed_wheel"/>
<rosparam param="odom0_config">[true, true,  false, 
                            false, false, true, 
                            false, false, false, 
                            false, false, false,
                            false, false, false]</rosparam>

  <param name="odom0_relative" value="true"/>
  <param name="imu0_relative" value="true"/>

Here is an sample bagfile together with robot localization: https://www.dropbox.com/s/kalrug9s48j5w3x/odom_imu_fuse.bag?dl=0

Topic: /imu/data == xsense imu data
Topic: /odom/fixed_wheel == pioneer improved odometry (by hand added covariances)
Topic: /odom/ukf == current results of robot localization
Topic: /scan/sick == laser scanner for reference
Topic: /tf
Topic: /tf_static

Here is the same bagfile without robot localization: https://www.dropbox.com/s/01xtvgmv7gchb63/odom_imu_unfused.bag?dl=0 --- Update ---

I added this fixed odometry covariance:

odom.pose.covariance =  boost::assign::list_of(1e-2) (0) (0)  (0)  (0)  (0)
                                             (0) (1e-2)  (0)  (0)  (0)  (0)
                                             (0)   (0)  (1e6) (0)  (0)  (0)
                                             (0)   (0)   (0) (1e6) (0)  (0)
                                             (0)   (0)   (0)  (0) (1e6) (0)
                                             (0)   (0)   (0)  (0)  (0)  (1e-3) ;

Maybe i am missing something obvious? Thanks in advance!

--- Update ---

I tried some different covariance matrices but nothing changed but when i changed my launch file like so:

    <param name="imu0_differential" value="true"/>
    <param name="odom0_differential" value="true"/>

then i got following result: image its an image, green is the wheel odometry and red the ukf result and blue is the ground truth (i know, really bad drawn^^)

What i do not understand is why the imu is making the result worse? This data is captures outside on a sidewalk, could this be a problem? I am pretty sure that the imu is directed in the right direction but it looks weird...

-- Update --

Here is the sample output of each source, first odometry, then imu:

header: 
 seq: 4343
 stamp: 
  secs: 1464844455
  nsecs: 543424495
 frame_id: odom
 child_frame_id: ''
pose: 
 pose: 
 position: 
  x: -68.2792701104
  y: 122.427959131
  z: 0.0
 orientation: 
  x: 0.0
  y: -0.0
  z: -0.629156547298
  w: -0.777278610919
 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, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
 twist: 
 twist: 
 linear: 
  x: 0.72982
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.01071
 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, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]

now imu:

header: 
seq: 107424
stamp: 
 secs: 1464844843
 nsecs: 499480009
 frame_id: imu
orientation: 
 x: -0.0360126234591
 y: -0.000914743170142
 z: -0.993979632854
 w: 0.10347366333
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
 x: 0.0237344596535
 y: -0.0165882073343
 z: -0.000813889899291
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
 x: 0.847484827042
 y: 0.130491986871
 z: 10.2731733322
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

---

Originally posted by tinokl on ROS Answers with karma: 13 on 2016-06-06

Post score: 1

---

Original comments

Comment by chwimmer on 2016-06-06:
so you set the covariances of the Odometry by hand? How you do that. Are you actually calculating them or set them to a static value?

Comment by tinokl on 2016-06-06:
for the odometry i set the covariance to a static value, the imu was already publishing a covariance matrix

Comment by chwimmer on 2016-06-06:
I think thats the main problem. What values did you choose? When you got small values in the covariancematrix of the odometry and high values in the imu covariancemartrix the robot_localisation will "trust" the odometry data much more than the imu data und so the imu data has almost no influence.

Comment by tinokl on 2016-06-06:
i added the covariance matrix to the question, i'll try some larger values and see how it performs

Comment by Tom Moore on 2016-06-07:
Please post a sample message from each input. I realize there's a bag, but it's often enough to just see the raw data. See my answer for more information, though.

Answer 1

I can see the following issues in your config:

1. You are fusing two absolute sources of absolute pose data: you have yaw set to true for both of your inputs. Unless you have your covariances properly tuned, I would advise against this. Pick your best source of yaw data (likely the IMU) and only set yaw to true for that. Technically, since you have differential mode turned on for your IMU, you are really fusing yaw from the robot and yaw "velocity" (differentiated yaw) from the IMU, so you won't see the heading jumping back and forth, but see below for information on why this is probably backwards.
2. You have relative and differential turned on for the IMU, which is a bit meaningless, as I believe differential mode is applied in that case.
3. You're not using any velocity data. Unlike pose data, fusing disparate source of velocity data, even when the covariances aren't all that well tuned, will cause less erratic behavior.

Regardless, the main crux of your issue is that you are essentially telling the filter to fuse only the data from the Pioneer odometry. Absolute pose data will have a greater effect on filter output than velocity data, so your differentiated IMU pose (which gets turned into velocity by differential mode) is effectively being ignored (really, it's just smoothing the transition between yaw measurements from your wheel odometry). Also, if your IMU provides angular velocity, it typically comes from a completely different sensor (i.e., a gyroscope as opposed to a magnetometer), so you should really be fusing yaw velocity. Try this instead:

<param name="imu0" value="imu/data"/>
<rosparam param="imu0_config">[false, false, false,
                               false, false, true,
                               false, false, false,
                               false, false, true,
                               false,  false, false]</rosparam>
<param name="imu0_differential" value="false"/>

<param name="odom0" value="/odom/fixed_wheel"/>
<rosparam param="odom0_config">[false, false, false, 
                                false, false, false, 
                                true, true, false, 
                                false, false, true,
                                false, false, false]</rosparam>

<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="true"/>

You may want to disable yaw in your IMU altogether if you find it suddenly shifts for no reason while driving. Magnetometers are notoriously awful in the presence of sources of distortion. You may also want to re-enable X and Y in your odometry and disable X and Y velocity. Experiment with it, but whatever you do, don't enable yaw on your Pioneer wheel odometry.

EDIT 1:

Your Pioneer odometry appears to be missing a child_frame_id. Not sure why that is, but it will prevent the EKF from using the velocity data in the odometry message.

---

Originally posted by Tom Moore with karma: 13689 on 2016-06-07

This answer was ACCEPTED on the original site

Post score: 5

---

Original comments

Comment by tinokl on 2016-06-21:
thank you very much, i tried what you suggested and mostly it works quite fine. As you already mentioned i had sudden shifts, probably because of tall buildings next to the robot... all in all i found a configuration which works out for now (and thanks for the child frame id hint) :)

Comment by Tom Moore on 2016-06-21:
I'm guessing it's because your IMU "odometry" sensor has a lot of drift. Compare the outputs of each filter and I'm sure you'll see that. If you want to validate it, try integrating your velocity measurements from your IMU odometry topic yourself.