So, I have been configuring around robotlocalization package to use GPS and IMU together with visual odometry from rtabmap, but have weird behavior. To begin with, I use Orbbec Astra as source of visual odometry, which is then sent to first and second instances of ekflocalization node, which produces visual odometry+imu as first instance and visual+imu+navsat odometry from second. As well I have navsattransform node which takes output from second ekf instance, which is odomgps and gps/imu data. Then i use launch file from rtabmapros package called sensorfusion, where I removed the robotlocalization node part, because I want to use configuration described above. First instance works fine, visual odometry also seem to be reasonably stable, however second instance and navsattransform node output unstable behavior. At the start the position is around ~20cm a drift in x and y and if some small slow movement is done with the base, it starts drifting around to positive and negative directions (sometimes to one, sometimes to both). I tried to do this outside in more open space to give GPS to catch reasonable amount of satellites and the problem still persists. I have read one similar question before, but his problem was that he used wrong output - from first ekf instance. I double checked if I had that and I am sure i use output from second ekf instance. I would really appreciate if anyone had similar problem and managed to overcome it. I post the launch file of both rtab and robot localization (https://www.dropbox.com/sh/57zaiqweulrxslf/AAB3DxMIABsECWNLPr1P7H4ia?dl=0) as well image of drift. Also geared used is: Xsens MTi-G-700, Orbbec Astra.

Robot_localization.launch

<launch>

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



    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 1 base_link imu" />

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

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

      <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_link"/>
      <param name="world_frame" value="odom"/>

      <remap from="/odometry/filtered" to="/odom_ekf"/> 

      <param name="transform_time_offset" value="0.5"/>
      <param name="odom0" value="/vo"/>      
      <param name="imu0" value="/imu/data"/>

      <rosparam param="odom0_config">[true, true,  false,
                                      false, false, false,
                                      true, true, 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="odom0_relative" value="false"/>
      <param name="imu0_differential" value="false"/>
      <param name="imu0_relative" value="true"/>
      <param name="imu0_remove_gravitational_acceleration" value="true"/>

    </node>

    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" output= "screen" respawn="true">

      <param name="magnetic_declination_radians" value="0.190240888"/>
      <param name="delay" value="1.0"/>
      <param name="yaw_offset" value="0"/>
      <param name="use_odometry_yaw" value="false" />   
      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="wait_for_datum" value="false" /> 
      <param name="publish_filtered_gps" value="true" />  

      <remap from="/imu/data" to="/imu/data" />
      <remap from="/gps/fix" to="/fix" />
      <remap from="/odometry/filtered" to="/odom_gps" />
      <remap from="/odometry/gps" to="/odom_navsat" /> 

    </node>

    <node pkg="robot_localization" type="ekf_localization_node" name="gps_ekf_localization" output= "screen" clear_params="true">

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

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

      <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_link"/>
      <param name="world_frame" value="map"/>

      <remap from="/odometry/filtered" to="/odom_gps"/> 

      <param name="transform_time_offset" value="0.5"/>
      <param name="odom0" value="vo"/>
      <param name="odom1" value="odom_navsat"/>
      <param name="imu0" value="imu/data"/>

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

      <rosparam param="odom1_config">[true,  true,  false,
                                      false, false, false,
                                      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="odom0_relative" value="false"/>
      <param name="imu0_differential" value="false"/>
      <param name="imu0_relative" value="true"/>
      <param name="odom1_differential" value="false"/>
      <param name="odom1_relative" value="false"/>
      <param name="imu0_remove_gravitational_acceleration" value="true"/>


    </node>

</launch>

Sensorfusion.launch (from rtabmapros)

<launch>  

  <!-- This launch assumes that you have already 
       started you preferred RGB-D sensor and your IMU.
       TF between frame_id and the sensors should already be set too. -->

  <node pkg="tf" type="static_transform_publisher" name="bl_camera" args="0 0 0 0 0 0 /base_link /camera_link 100" /> 




  <arg name="frame_id"                default="base_link" />
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="imu_topic"               default="/imu/data" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  

  <group ns="rtabmap">
    <!-- Visual Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/vo"/>

      <param name="frame_id"               type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"             type="bool"   value="false"/>
      <param name="publish_null_when_lost" type="bool"   value="true"/>
      <param name="guess_from_tf"          type="bool"   value="true"/>


      <param name="Odom/FillInfoData"      type="string" value="true"/>
      <param name="Odom/ResetCountdown"    type="string" value="1"/>
      <param name="Vis/FeatureType"        type="string" value="6"/> 
      <param name="OdomF2M/MaxSize"        type="string" value="1000"/>

    </node>

    <!-- SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>

      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odom_gps"/>

      <param name="Kp/DetectorStrategy"    type="string" value="6"/> <!-- use same features as odom -->
      <param name="Optimizer/Iterations" type="string" value="0"/>
      <!-- localization mode -->
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>

    </node>
  </group>


</launch>