robot_localization: yaw angle jumping erratically

edit

Hello Tom/All,

I am currently using robot_pose_ekf to fuse wheel odometry and imu data on a custom robot with two tracks. This is wokring pretty well (works perfectly with gmapping and the navigation stack). I am now in the process of adding GPS into the mix, which would allow me to do some outdoor navigation experiments.

As adding GPS as sensor source is not officially supported by robot_pose_ekf, so I was on the lookout for an alternative solution. On paper, I found that robot_localization is exactly what is was looking for. Unfortunately, I am facing issues getting it to work.

My Setup

1. odom0 is coming from by custom base_controller on topic /wheel_odom as nav_msgs/Odometry. It provides the following data: x, y, yaw, x velocity, yaw velocity. All remaining fields included in nav_msgs/Odometry are 0's.
2. odom1 is coming from a gps_common/utm_dodmetry_node as nav_msgs/Odometry. It provides the following data: x, y and z. All remaining fields included in nav_msgs/Odometry are 0's.
3. imu0 is coming from a XSens IMU (lse_xses_mti driver) on topic /imu_data as sensor_msgs/Imu. It provides the following data: roll, pitch and yaw angles. All remaining fields of sensor_msgs/Imu are 0's.

For now, the robot is meant to operate on the plane only. Hence, we can deduce that z is always 0, and so are the roll and pitch angles. My launch file below takes that into account.

My robot_localization launch file

<launch>  
  <!-- Launch robot_localization node -->
  <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization" >
    <remap from="set_pose" to="/robot_localization/set_pose"/>           
    <remap from="odometry/filtered" to="/robot_localization/odom_combined"/>

    <param name="odom_frame" value="odom_combined"/>
    <param name="base_link_frame" value="base_footprint"/>    

    <!-- =============================================================================== -->    
    <!-- Configure odom0 (WHEEL_ODOM) -->
    <param name="odom0" value="/wheel_odom" />
    <rosparam param="odom0_config">[true,  true,  false,    <!-- x, y, z position -->
                                    false, false, true,    <!-- roll, pitch, yaw angles-->
                                    true,  true,  false,   <!-- x/y/z velocity -->
                                    false, false, true]    <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom0_differential">[false,  false,  false,   <!-- x, y, z position -->
                                          false,  false,  false]   <!-- roll, pitch, yaw angles-->
    </rosparam>    
    <!-- =============================================================================== -->    
    <!-- Configure odom1 (GPS_ODOM) -->
    <param name="odom1" value="/utm_odometry_node/gps_odom"/>
    <rosparam param="odom1_config">[true,  true,  false,      <!-- x, y, z position -->
                                    false, false, false,   <!-- roll, pitch, yaw angles-->
                                    false, false, false,   <!-- x/y/z velocity -->
                                    false, false, false]   <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom1_differential">[false, false, false,  <!-- x, y, z position -->
                                          false, false, false]  <!-- roll, pitch, yaw angles-->
    </rosparam>
    <!-- =============================================================================== -->    
    <!-- Configure imu0 (XSENS)-->
    <param name="imu0" value="/imu_data"/>               
    <rosparam param="imu0_config">[false, false, false,  <!-- x, y, z position -->
                                   false, false, true,   <!-- roll, pitch, yaw angles-->
                                   false, false, false,  <!-- x/y/z velocity -->
                                   false, false, false]  <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="imu0_differential">[false, false, false,   <!-- x, y, z position -->
                                         false, false, false]   <!-- roll, pitch, yaw angles -->
    </rosparam>
    <!-- =============================================================================== -->    
  </node>

</launch>

The Issue

The problem is that the yaw angle of /robot_localization/odom_combined is jumping erratically as long as data is coming in on the /imu_data topic. (yaw on /imu_data is rock solid however).

When I disable the yaw-angle-component of odom0 (/wheel_odom), the problem disappears. I have also ... (more)