Output of robot_localization node cannot converge until fusing absolute pose data [closed]

edit

Hi There,

I am working with a mobile robot CLEARPATH Jackal. It has two instances ekf_localization_node running inside, one for local and one for global.

For the first test, ekf_local_node was fused with wheel odometry and IMU. ekf_global_node was fused with wheel odometry, IMU and the output of RTabMap which took /odometry/filtered/local as the input. The launch files are shown below:

ekf_local_node: <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" output="screen" &gt;="" <remap="" from="/odometry/filtered" to="/odometry/filtered/local"/>

  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="/jackal_velocity_controller/odom"/> <!-- jackal odometry -->
  <param name="imu0" value="/imu/data"/> <!-- IMU -->

  <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
  <rosparam param="odom0_config">[false, false, false,                                                  
                                  false, false, false,                                                
                                  true, true, false,                                                  
                                  false, false, true,                                                 
                                  false, false, false]</rosparam>

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

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

  <param name="odom0_relative" value="true"/>
  <param name="imu0_relative" value="true"/>

</node>

ekf_global_node:

   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global_1" output="screen" >
  <remap from="/odometry/filtered" to="/odometry/filtered/global1"/>
  <param name="frequency" value="50"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="two_d_mode" value="false"/>

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

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="/jackal_velocity_controller/odom"/>
  <param name="pose0" value="/rtabmap/localization_pose"/>
  <param name="imu0" value="/imu/data"/>

  <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->

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

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

  <param name="odom0_relative" value="false"/>
  <param name="pose0_relative" value="false"/>
  <param name="imu0_relative" value="true"/>

The first test worked perfectly. I generated plots of positions x versus y for ekf_nodes (both ekf_local and ekf_global) and they have good agreements with those obtained fromvicon cameras. However, after a few days when I did the second test with the same launch file, both ekf nodes had convergence problems, i.e. position x grew boundlessly which is unphysical. This problem is weird because I used the same launch file and the experiment environment was the same as before.

I investigated the problem for a bit, and I found fusing the absolute pose of wheel odometry with ekf_local node can fix the convergence problem for 'ekf_local_node'. For ekf_global_node, if I fuse absolute poses from both wheel odometry and Rtabmap ... (more)