robot_localization rapid drift

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

mandad
1 ●1 ●1 ●2

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" />

</node>

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

header: 
  seq: 823
  stamp: 
    secs: 1442966487
    nsecs: 395684777
  frame_id: imu_link
orientation: 
  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]
angular_velocity: 
  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]
linear_acceleration: 
  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

Comments

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 ( 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.