Localization problem: GPS drift when using visual odomery and INS

asked 2017-03-17 08:16:59 -0600

EdCherie gravatar image

updated 2017-03-24 09:48:17 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you please also include your navsat_transform_node configuration? Also, I would recommend commenting out the visual odometry in the second (map) EKF instance and seeing if things are stable without it first, then we can go from there.

Tom Moore gravatar imageTom Moore ( 2017-04-11 04:34:16 -0600 )edit

Have you tried plotting the raw GPS data? The pose estimate seems to track the /odom_navsat data, so we need to check why the navsat data is drifiting around. Is it drifting, or is it simply not moving in the direction you'd expect?

Tom Moore gravatar imageTom Moore ( 2017-07-06 05:37:10 -0600 )edit