Robot_localization unexpected behavior for filtered navsat output

edit

Getting unexpected output from navsat_transform for /gps/filtered topic after being processed through the ekf filter. Coordinates diverge quickly from /gps/fix input topic very fast (likely due to imu drift) but divergence is mostly in negative z direction.

Project background info: Using robot_localization to fuse gps and imu referencing this Tom Moore answer: http://answers.ros.org/question/20007.. . My setup is the same with a full-size SUV vehicle, Beaglebone Black board, Phidgets imu w/mag, ublox gps, with no odometry sensors. I am running Ubuntu 14.04.4 LTS, deb 4.1.15-ti-rt-r40 armv7l, ros indigo, and robot_localization with the ekf filter. I've calibrated the magnetometer for hard and soft iron on the vehicle and incorporate the factors in the driver. I use an overall launch file that separately calls the devices and robot_localization. REP's 103 and 105 were adhered to with the imu (as far as I can tell), since the raw mag points (X) east at zero and the other checks (Y and Z) work out. Declination is accounted for in the navsat_transform launch file. Navsat and Ekf launch files:

<!-- Launch file for navsat_transform_node -->

<launch>

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

  <!-- Frequency of the main run loop. -->
  <param name="frequency" value="32"/>
  <param name="delay" value="0"/>
  <param name="magnetic_declination_radians" value="-0.206996"/>
  <param name="yaw_offset" value="0.0"/>
  <param name="zero_altitude" value="false"/>
  <param name="broadcast_utm_transform" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="use_odometry_yaw" value="false"/>
  <param name="wait_for_datum" value="false"/>

  <!-- Placeholders for input remapping. Set your topic names as the "to" values.  -->
         <remap from="/imu/data" to="/imu/data"/>
         <remap from="/odometry/filtered" to="/odometry/filtered"/>
         <remap from="/gps/fix" to="/gps/fix"/>

  <!-- Placeholders for output remapping.        -->

         <remap from="/odometry/gps" to="/odometry/gps"/>
         <remap from="/gps/filtered" to="/gps/filtered"/>
</node>

</launch>`

ekf_localization launch file:

<!-- Launch file for ekf_localization_node -->

<launch>

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

  <!-- ======== STANDARD PARAMETERS ======== -->

  <param name="frequency" value="32"/>
  <param name="sensor_timeout" value="0.05"/>
  <param name="two_d_mode" value="false"/>
  <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="transform_time_offset" value="0.0"/>
  <param name="odom0" value="/odometry/gps"/>
  <param name="imu0" value="/imu/data"/>

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

  <rosparam param="imu0_config">[false, false, false,
                                 true,  true,  true,
                                 false, false, false,
                                 true,  true,  true,
                                 true,  false,  true]</rosparam>
  <param name="odom0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>
  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="true"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>
  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <!-- If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to control
       how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
       numeric_limits<double>::max() if ...