Using wheel encoders and IMU with robot_localization

edit

Hi all,

I am using wheel_encoders which is giving odometry information of the type nav_msgs/Odometry and IMU which gives sensor_msgs/IMU (IMU is giving orientation, angular velocity and linear acceleration). I am using them with robot_localization. It seems to be working but when I save a bag file and then run it and visualize the odom (from the wheel_encoders) and the odometry output of robot_localization, they seem off and they start diverging under the settings shown in the launch file below. Here is the link to the video (Red arrow represents odometry from wheel_encoders and Blue arrow represents output odometry from robot_localization).

ekf_robot_localization.launch

<launch>

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

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

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

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

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

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

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

  <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, false,
                                 false, false, false,
                                 false, false, true,
                                 false, false, false]</rosparam>

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

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

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="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.05, 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.03, 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.06, 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.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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,    0,     0,     0,    0,    0.01, 0,    0,    0,    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,    0.01, 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,    0,    0,    0,    0,    0.015]</rosparam>

   <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    1e-9, 0,    0,    0,    0,    0 ...