ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

Would you mind posting your entire launch file and a sample message from each input that you have? Feel free to remove comments from the launch file to make it less cluttered. It sounds like there may be some issues with frame IDs.

Would you mind posting your entire launch file and a sample message from each input that you have? Feel free to remove comments from the launch file to make it less cluttered. It sounds like there may be some issues with frame IDs.

EDIT: OK, I've got this sorted. First, I'll give you an updated launch file:

<launch>

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>  

      <param name="sensor_timeout" value="0.1"/>  

      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="odom_imu"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_projected"/>
      <param name="world_frame" value="odom_imu"/>

      <param name="odom0" value="/odom"/>
      <param name="imu0" value="/imu/data"/> 

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

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

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

      <param name="imu0_remove_gravitational_acceleration" value="true"/>

      <param name="debug"           value="true"/>
      <param name="debug_out_file"  value="debug_ekf.txt"/>

      <rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.00, 0.06, 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.00, 0.0, 0.025, 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.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 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, 0.00, 0.0, 0.0, 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.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]</rosparam>


    </node>

</launch>

Now some notes:

  • First, you helped me uncover a bug, so thanks! I've fixed it now, and it's checked into the repo. You'll have to pull down the source until I can push a release through. The bug would have only affected users who are trying to use ekf_localization_node or ukf_localization_node for generating the map->odom transform AND were using differential integration of absolute measurements. I'm surprised you were getting position estimates that jumped around (perhaps that's not what you meant), but they were inaccurate.
  • However, I did see a couple things worth tweaking in your launch file. First, in your odom0_config, I enabled fusing Y and Z velocity, as your robot appears to be ground-based an nonholonomic. See this page.
  • Your odometry covariances are all 0. The values on the diagonals for X, Y, and yaw should be non-zero. Give the covariances for Y velocity and Z velocity very small values.
  • Strangely, in one of your bag files, the IMU data has angular velocity and linear acceleration, and they both have reasonable covariances. In the other, those values aren't filled out, and the covariance matrices are set to -1 for those values (even the off-diagonal ones). Not sure what is responsible for the switch, but be careful not to accidentally fuse a measurement with a bunch of -1 covariance values. The filter will explode.
  • Did you start on or move up/down an incline during the latest bag file run?
  • For your odometry data, I noticed its refresh rate is low, and towards the end of the bag file, it stops altogether for quite a long time.
  • You're likely aware of this, but for the benefit of future readers, keep in mind that if you visualize both the raw odometry (odom) and the filtered output (odometry/filtered), they will appear right on top of one another no matter what. This indicates that the odom_imu->odom transform is being computed correctly, and rviz is just carrying out the transform before drawing the odometry.

Would you mind posting your entire launch file and a sample message from each input that you have? Feel free to remove comments from the launch file to make it less cluttered. It sounds like there may be some issues with frame IDs.

EDIT: OK, I've got this sorted. First, I'll give you an updated launch file:

<launch>

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>  

      <param name="sensor_timeout" value="0.1"/>  

      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="odom_imu"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_projected"/>
      <param name="world_frame" value="odom_imu"/>

      <param name="odom0" value="/odom"/>
      <param name="imu0" value="/imu/data"/> 

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

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

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

      <param name="imu0_remove_gravitational_acceleration" value="true"/>

      <param name="debug"           value="true"/>
      <param name="debug_out_file"  value="debug_ekf.txt"/>

      <rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.00, 0.06, 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.00, 0.0, 0.025, 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.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 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, 0.00, 0.0, 0.0, 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.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]</rosparam>


    </node>

</launch>

Now some notes:

  • First, you helped me uncover a bug, so thanks! I've fixed it now, and it's checked into the repo. You'll have to pull down the source until I can push a release through. The bug would have only affected users who are trying to use ekf_localization_node or ukf_localization_node for generating the map->odom transform AND were using differential integration of absolute measurements. I'm surprised you were getting position estimates that jumped around (perhaps that's not what you meant), but they were inaccurate.
  • However, I did see a couple things worth tweaking in your launch file. First, in your odom0_config, I enabled fusing Y and Z velocity, as your robot appears to be ground-based an nonholonomic. See this page.
  • Your odometry covariances are all 0. The values on the diagonals for X, Y, and yaw should be non-zero. Give the covariances for Y velocity and Z velocity very small values.
  • Strangely, in one of your bag files, the IMU data has angular velocity and linear acceleration, and they both have reasonable covariances. In the other, those values aren't filled out, and the covariance matrices are set to -1 for those values (even the off-diagonal ones). Not sure what is responsible for the switch, but be careful not to accidentally fuse a measurement with a bunch of -1 covariance values. The filter will explode.
  • Did you start on or move up/down an incline during the latest bag file run? EDIT: OK, it looks like you started on a downward incline, or at least your IMU says you did. You then leveled out. Important note: if you use differential integration, this will appear to the filter as if you started on level ground and then started going up an incline. If it's an issue, you can turn off the differential setting for your IMU data.
  • For your odometry data, I noticed its refresh rate is low, and towards the end of the bag file, it stops altogether for quite a long time.
  • You're likely aware of this, but for the benefit of future readers, keep in mind that if you visualize both the raw odometry (odom) and the filtered output (odometry/filtered), they will appear right on top of one another no matter what. This indicates that the odom_imu->odom transform is being computed correctly, and rviz is just carrying out the transform before drawing the odometry.