I've been trying to fuse gps data with a odometry and an imu for a few days now and for some reason cannot get the navsattransformnode to work. I plot the odometry/gps topic right next to the raw converted gps data from the receiver and the odometry/gps will randomly start slowly shooting off to hundreds of meters while the raw data stays in a nice small area.

Am I doing anything obviously wrong? I just can't seem to figure it out.

EDIT: i forced the imu and odometry to do nothing and only looked at the gps data, and here is what happens. The raw gps data is on the bottom and stays in a nice area, and the odometry/gps topic is on top, and randomly drifts off into the distance.

EDIT 2: Still not entirely sure if I've solved the issue, but one problem I am having is that if I use the example typical navsat transform and two ekf nodes approach I get the random drift off into the distance. I then change the world frame of the second ekf that combines (odometry/gps,odom,and imu/data) to odom and it starts to work... I still have plenty of issues, but the random drift is gone. I'm not sure I entirely understand why the second ekf has a different world frame then the first anyway...

My launch file is here:

<launch>

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

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value=".138172"/>
  <param name="zero_altitude" value="true"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="wait_for_datum" value="false"/>
  <remap from="/odometry/filtered" to="/odometry/filtered_local"/>
</node>

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

  <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"/>

  <param name="odom0" value="odom"/>
  <param name="imu0" value="/imu/data"/> 


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

  <param name="imu0_remove_gravitational_acceleration" value="false"/>
  <remap from="/odometry/filtered" to="/odometry/filtered_local"/>

</node>

This is my gps/fix topic:

header: 
  seq: 1018
  stamp: 
    secs: 1492713144
    nsecs: 431811094
  frame_id: gps
status: 
  status: 0
  service: 1
latitude: 40.7624
longitude: 73.9738
altitude: 0.0
position_covariance: [2.1315999999999997, 0.0, 0.0, 0.0, 2.1315999999999997, 0.0, 0.0, 0.0, 8.526399999999999]
position_covariance_type: 1

My imu/data topic:

header: 
  seq: 134445
  stamp: 
    secs: 1492713512
    nsecs: 637841939
  frame_id: imu
orientation: 
  x: -0.0355834960938
  y: 0.0366821289062
  z: -0.767761230469
  w: 0.638732910156
orientation_covariance: [0.0025, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0025]
angular_velocity: 
  x: 0.00327249234749
  y: 0.00327249234749
  z: -0.00872664625997
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: -0.09
  y: -0.07
  z: -0.08
linear_acceleration_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]

and my odom topic:

header: 
  seq: 12578
  stamp: 
    secs: 1492713416
    nsecs: 487432956
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: -0.00563927695514
      y: 0.000251395002658
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0130539033403
      w: 0.999914794174
  covariance: [0.01, 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, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist: 
  twist: 
    linear: 
      x: -0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01, 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, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]