Localization problem: GPS drift when using visual odomery and INS [closed]

edit

So, I have been configuring around robot_localization 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 ekf_localization node, which produces visual odometry+imu as first instance and visual+imu+navsat odometry from second. As well I have navsat_transform node which takes output from second ekf instance, which is odom_gps and gps/imu data. Then i use launch file from rtabmap_ros package called sensor_fusion, where I removed the robot_localization 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 navsat_transform 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/57zaiqweul... ) 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 ...