Ask Your Question

robot_localization rapid drift

asked 2015-09-22 19:19:37 -0500

mandad gravatar image

I am eventually trying to fuse GPS and IMU data with robot_localization but first want to make sure my IMU is working correctly. When I feed just the IMU data into ekf_localization node, I end up with an odometry/filtered message with rapidly growing positions, particularly in x and y. I am telling it to remove gravitational acceleration, but perhaps the axes are incorrect? The orientations should be ENU, but may be incorrect because I had to write the IMU driver myself and it natively outputs NED as far as I am aware (ChRobotics GP9). The orientation and rotations look correct to me when I visualize the IMU only in rviz, but I am not sure if this is complete verification.

 <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
    <param name="frequency" value="20"/>
    <param name="sensor_timeout" value="0.5"/>
    <param name="two_d_mode" value="false"/>
    <!-- Defaults to "map" if unspecified -->
    <!-- <param name="map_frame" value="map"/> -->
    <!-- Defaults to "odom" if unspecified -->
    <param name="odom_frame" value="odom"/>
    <!-- Defaults to "base_link" if unspecified -->
    <param name="base_link_frame" value="imu_link"/>
    <!-- Defaults to the value of "odom_frame" if unspecified -->
    <param name="world_frame" value="odom"/>
    <param name="imu0" value="/imu/data"/>
    <rosparam param="imu0_config">[false, false, false, 
                                 true,  true,  true, 
                                 false, false, false, 
                                 true,  true,  true,
                                 true,  true,  true]</rosparam>
    <param name="imu0_remove_gravitational_acceleration" value="true"/>
    <param name="imu0_relative" value="false" />


Here is the output from my IMU when on a table and close to level:

  seq: 823
    secs: 1442966487
    nsecs: 395684777
  frame_id: imu_link
  x: -0.016482543976
  y: 0.04296875008
  z: 0.027795410208
  w: 0.99851989932
orientation_covariance: [0.0002406090497970581, 0.0, 0.0, 0.0, 0.00024995996500365436, 0.0, 0.0, 0.0, 0.0012901085428893566]
  x: -0.000252186092119
  y: 0.000348960967625
  z: 0.000641519161213
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  x: -0.0311140995473
  y: -0.112905547023
  z: 9.8887090683
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
edit retag flag offensive close merge delete


I've had the same thing. I think that, since you're only fusing the IMU data, this is the expected behavior of the filter (integrating acceleration only to get position is not ideal).

Gary Servin gravatar image Gary Servin  ( 2015-09-23 00:48:55 -0500 )edit

I realize that is not an ideal circumstance, and I would expect it to drift slowly, but this is drifting to hundreds of meters away within about 10 seconds, which does not seem correct. It seems like gravitational acceleration is not being removed, as the direction of the movement is consistent.

mandad gravatar image mandad  ( 2015-09-23 09:19:21 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2015-09-24 07:20:38 -0500

Tom Moore gravatar image

Can you post a bag file? The problem with not having a velocity reference is that those accelerations are going to compound, so even if your acceleration is only slightly biased, it's going to take off pretty quickly. Double integration is really ugly. I imagine this will be exacerbated by the rapid growth of the covariance matrix for the variables that aren't directly measured.

This won't help, but just as an FYI, you should probably fill out the covariance matrices for the angular velocity and linear acceleration.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower


Asked: 2015-09-22 19:19:37 -0500

Seen: 348 times

Last updated: Sep 24 '15