Robotics StackExchange | Archived questions

Linear velocity drift robot_localization with imu

Hi all,

I am using robot_localization to fuse odometry and imu measurements. While my robot is standing still, the estimated pose blows up towards the x and/or y direction and the velocity increases steadily. I believe this is because my IMU messages are reporting that there is acceleration in the x-y direction when there is not. The acceleration that is seen on the imu axes are from gravity because on my robot it is impossible to mount the imu perfectly level. The angle is accurately reflected in the orientation.

Odometry message:

header: 
  seq: 180
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: -4.57017288436e-23
      y: -1.37577341871e-27
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]

IMU message:

header: 
  seq: 3565
  stamp: 
    secs: 1437400985
    nsecs: 945771932
  frame_id: base_imu_link
orientation: 
  x: 0.0115444314118
  y: 0.0118909824299
  z: 0.0816176668022
  w: 0.996525908899
orientation_covariance: [0.0025, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0025]
angular_velocity: 
  x: -0.07
  y: 0.0
  z: -0.0
angular_velocity_covariance: [0.02, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.02]
linear_acceleration: 
  x: -0.210292734375
  y: 0.2711971875
  z: 9.72747539062
linear_acceleration_covariance: [0.04, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.04]

robot_localization message

header: 
  seq: 14858
  stamp: 
    secs: 1437401408
    nsecs: 104844999
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 752.873872865
      y: -960.277044656
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.0129782931114
      w: 0.999915778407
  covariance: [1235363.2798552709, 0.09966583512707781, 0.0, 0.0, 0.0, 0.00012219044159253388, 0.09966583492106752, 1235363.2253877113, 0.0, 0.0, 0.0, 8.345895540980487e-05, 0.0, 0.0, 4.997912991703062e-07, -1.6226536599895273e-14, -1.1710583642991047e-14, 0.0, 0.0, 0.0, -1.6226536599895264e-14, 4.995829440611683e-07, -9.105011207999492e-19, 0.0, 0.0, 0.0, -1.1710583642991045e-14, -9.10501120799949e-19, 4.995829440617729e-07, 0.0, 0.00012219044159253388, 8.345895540980487e-05, 0.0, 0.0, 0.0, 0.0012374751044356635]
twist: 
  twist: 
    linear: 
      x: 3.37509526949
      y: -4.67673415559
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -5.79267445233e-07
  covariance: [14.94177991668064, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 14.94177991668064, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.996870776051403e-07, -9.835971518024234e-24, -7.493838037842533e-24, 0.0, 0.0, 0.0, -9.835971518024234e-24, 4.987529858182264e-07, -2.2678308703831656e-27, 0.0, 0.0, 0.0, -7.493838037842532e-24, -2.267830867073473e-27, 4.987529858182264e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0026566693199961485]

Robot localization launch

<launch>

  <arg name="odom_topic" default="odom" />
  <arg name="imu_topic" default="imu" />

    <!-- This node will take in measurements from odometry, IMU, stamped pose, and stamped twist messages. It tracks
         the state of the robot, with the state vector being defined as X position, Y position, Z position,
         roll, pitch, yaw, their respective velocites, and linear acceleration. Units for all measurements are assumed
         to conform to the SI standard as specified in REP-103. -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <!-- ======== STANDARD PARAMETERS ======== -->

      <!-- The frequency, in Hz, at which the filter will output a position estimate. Note that
           the filter will not begin computation until it receives at least one message from
           one of the inputs. It will then run continuously at the frequency specified here,
           regardless of whether it receives more measurements. Defaults to 30 if unspecified. -->
      <param name="frequency" value="30"/>

      <!-- The period, in seconds, after which we consider a sensor to have timed out. In this event, we
           carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
           as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
           if not specified. -->
      <param name="sensor_timeout" value="0.1"/>

      <!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
           are operating in a planar environment and want to ignore the effect of small variations in the
           ground plane that might otherwise be detected by, for example, an IMU. Defaults to false if
           unspecified. -->
      <param name="two_d_mode" value="true"/>

      <!-- REP-105 (http://www.ros.org/reps/rep-0105.html) specifies three principal coordinate frames: map,
           odom, and base_link. base_link is the coordinate frame that is affixed to the robot. The robot's
           position in the odom frame will drift over time, but is accurate in the short term and should be
           continuous. The odom frame is therefore the best frame for executing local motion plans. The map
           frame, like the odom frame, is a world-fixed coordinate frame, and while it contains the most
           globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to
           the fusion of GPS data. Here is how to use the following settings:
             1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
                  * If your system does not have a map_frame, just remove it, and make sure "world_frame" is set
                    to the value of odom_frame.
             2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU dat$
                set "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state
                estimation nodes.
             3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or positi$
                updates from landmark observations) then:
                  3a. Set your "world_frame" to your map_frame value
                  3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be
                      another instance of robot_localization! However, that instance should *not* fuse the global data$
      <!-- 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="base_link"/>
      <!-- Defaults to the value of "odom_frame" if unspecified -->
      <param name="world_frame" value="odom"/>
<!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
           can be used for future dating the transform, which is required for interaction with some other
           packages. Defaults to 0.0 if unspecified. -->
      <param name="transform_time_offset" value="0.0"/>

      <!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
           TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
           e.g., odom0, odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These
           parameters obviously have no default values, and must be specified. -->
      <param name="odom0" value="$(arg odom_topic)"/>
      <!--param name="pose0" value="example/pose"/-->
      <!--param name="twist0" value="example/twist"/-->
      <param name="imu0" value="$(arg imu_topic)" />

      <!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
           which values from each measurement are fed to the filter. For example, if you have an odometry message as i$
           but only want to use its Z position value, then set the entire vector to false, except for the third entry.
           The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note tha$
           message types lack certain variables. For example, a TwistWithCovarianceStamped message has no pose informa$
           the first six values would be meaningless in that case. Each vector defaults to all false if unspecified, e$
           making this parameter required for each sensor. -->
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, true,
                                      true,  false, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <!--rosparam param="pose0_config">[true,  true,  false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam-->

      <!--rosparam param="twist0_config">[false, false, false,
                                       false, false, false,
true,  true,  true,
                                       true,  true,  true,
                                       false, false, false]</rosparam-->

      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in ve$
           measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estima$
           nodes support fusion of absolute measurements. If you have more than one sensor providing absolute measurem$
           however, you may run into problems if your covariances are not large enough, as the sensors will inevitably
           diverge from one another, causing the filter to jump back and forth rapidly. To combat this situation, you $
           either increase the covariances for the variables in question, or you can simply set the sensor's different$
           parameter to true. When differential mode is enabled, all absolute pose data is converted to velocity data $
           differentiating the absolute pose measurements. These velocities are then integrated as usual. NOTE: this o$
           applies to sensors that provide absolute measurements, so setting differential to true for twit measurement$
           no effect.

           Users should take care when setting this to true for orientation variables: if you have only one source of
           absolute orientation data, you should not set the differential parameter to true. This is due to the fact t$
           integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position va$
           this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orie$
           variables have at least one source of absolute measurement. -->
      <param name="odom0_differential" value="true"/>
      <!--param name="odom1_differential" value="false"/-->
      <!--param name="pose0_differential" value="true"/-->
      <param name="imu0_differential" value="false"/>
<!-- When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" fo$
           future measurements. While you can achieve the same effect with the differential paremeter, the key differe$
           that the relative parameter doesn't cause the measurement to be converted to a velocity before integrating $
           you simply want your measurements to start at 0 for a given sensor, set this to true. -->
      <param name="odom0_relative" value="false"/>
      <!--param name="odom1_relative" value="true"/-->
      <!--param name="pose0_relative" value="false"/-->
      <param name="imu0_relative" value="true"/>

      <!-- If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration$
           is due to gravity for each IMU. If you don't want to, then set this to false. Defaults to false if unspecif$
      <param name="imu0_remove_gravitational_acceleration" value="true"/>

      <!-- If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see
           if the node is unhappy with any settings or data. -->
      <param name="print_diagnostics" value="true"/>
 </node>

</launch>

Can anyone help me figure out what's going on here? This is the exact problem robot_localization is supposed to help solve.

Thanks for any help you can provide!

By the way, I am using the Sparkfun Razor 9Dof IMU and the associated ROS node. I went through all of the setup and calibration steps that are listed on their wiki.

---UPDATE---

I realized the problem is bigger than the IMU, robotlocalization isn't even using my odometry messages. I verified that odometry messages are publishing and that robotlocalization is subscribing to them. I can see the connection. But the ekf doesn't update for my odometry messages. Why?

Asked by Robocop87 on 2015-07-20 09:36:32 UTC

Comments

Answers

Two things:

  1. If you fuse an IMU by itself, then yes, the linear accelerations will cause the velocity and pose estimates to move rapidly. You need another velocity or pose reference.
  2. Your odometry message time stamps are 0. Time stamps matter to robot_localization, as they are needed to look up transforms in the tf tree. Your odometry messages are probably being ignored.

Asked by Tom Moore on 2015-07-24 16:39:07 UTC

Comments

I'm sorry, I just saw this! I didn't realize the time stamp on the odometry was a problem, but that makes sense. I would have expected robot_localization to complain on the diagnostic topic if it was throwing out messages. I will test this soon.

Asked by Robocop87 on 2015-08-06 07:58:11 UTC

Good idea!

https://github.com/cra-ros-pkg/robot_localization/issues/224

Asked by Tom Moore on 2015-08-06 08:29:02 UTC