Ask Your Question
0

Problem when fusing IMU and Odometry

asked 2016-06-06 06:54:54 -0500

tinokl gravatar image

updated 2016-06-07 06:42:06 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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?

chwimmer gravatar imagechwimmer ( 2016-06-06 08:26:26 -0500 )edit

for the odometry i set the covariance to a static value, the imu was already publishing a covariance matrix

tinokl gravatar imagetinokl ( 2016-06-06 09:18:01 -0500 )edit

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.

chwimmer gravatar imagechwimmer ( 2016-06-06 09:39:19 -0500 )edit

i added the covariance matrix to the question, i'll try some larger values and see how it performs

tinokl gravatar imagetinokl ( 2016-06-06 10:11:56 -0500 )edit

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.

Tom Moore gravatar imageTom Moore ( 2016-06-07 05:52:43 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2016-06-07 06:09:15 -0500

Tom Moore gravatar image

updated 2016-06-14 04:09:10 -0500

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.

edit flag offensive delete link more

Comments

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) :)

tinokl gravatar imagetinokl ( 2016-06-21 07:28:57 -0500 )edit
1

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.

Tom Moore gravatar imageTom Moore ( 2016-06-21 15:18:32 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

5 followers

Stats

Asked: 2016-06-06 06:54:54 -0500

Seen: 2,808 times

Last updated: Jun 14 '16