robot_localization: yaw and position scaled down

edit

We have a setup on a Kobuki where we use the robot_localization package to fuse sensor data consisting of wheel odometry and landmark recognition, which is complemented by amcl to correct the odometry data while no landmarks are visible.

For now, the landmark system is not providing input, but the wheel odometry is fed to an ekf_localization_node which provides the odom -> base_footprint (analogous to base_link) transform. amcl receives the filtered odometry output produced by the ekf_localization_node and provides the map -> odom transform.

The ekf_localization_node however seems to "filter" the odometry in a weird way: The x and y position moves in the right direction, but only by half the distance of what it should do, whereas the yaw movement is scaled to maybe a 100th of it's original amount. What I would like to have happen is that the odometry input leads to a somewhat similar output on the /filtered/odometry topic.

Another issue which might be related is that the output on the /filtered/odometry topic seems to very slowly drift in both the xy-position as well as the yaw.

robot_localization launch file

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

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

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

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

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_footprint"/>
  <param name="world_frame" value="odom"/>

  <param name="odom0" value="/odom"/>


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



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


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

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="/home/***/debug_ekf_localization.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.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.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.0 ...