robot_localization drift

edit

I am trying to use robot_localization to fuse IMU with PTAM but I am getting some major drift. The error seems to be compounding.

At first I thought that the axes were not aligned correctly (which still might be the case) but now I am wondering if the PTAM data is being ignored or not factored in.

IMU message:

---
header: 
  seq: 20112
  stamp: 
    secs: 1441090194
    nsecs: 749087915
  frame_id: usb_cam
pose: 
  pose: 
    position: 
      x: 31.8292968996
      y: 15.1699469361
      z: -236.728859078
    orientation: 
      x: 0.726365842025
      y: 0.0331459598161
      z: -0.0598464211576
      w: 0.683895031977
  covariance: [0.0039018500775288606, 0.0027630803033894093, 0.007831431075466348, 0.00037953225698867527, 0.0012029410262677585, 0.0008225141417525416, 0.0027630803033894093, 0.0022154257522649786, 0.005748820203067354, 0.0004981530611692311, 0.0008370959001320362, 0.00021214889413358968, 0.007831431075466348, 0.005748820203067352, 0.016284114101305945, 0.0008166119036448137, 0.002352189342863207, 0.0015354683751600184, 0.0003795322569886803, 0.0004981530611692337, 0.0008166119036448272, 0.00044415715572847697, 0.00014435743999326682, 0.0004755561972274128, 0.0012029410262677585, 0.0008370959001320371, 0.0023521893428632076, 0.00014435743999326628, 0.0005786599694969616, 8.747369640226204e-05, 0.0008225141417525225, 0.00021214889413362665, 0.0015354683751599768, 0.00047555619722741335, 8.747369640224455e-05, 0.0014638212613628672]
---

IMU message (time delay between last message):

---
header: 
  seq: 415116
  stamp: 
    secs: 1441090283
    nsecs: 586467584
  frame_id: fcu
orientation: 
  x: 0.0348078489314
  y: 0.0336684433503
  z: 0.620517126138
  w: 0.782696202715
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.000315623357892
  y: -0.000284243375063
  z: 0.000455724075437
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.084431245923
  y: 0.915327310562
  z: 9.66289806366
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---

tf launch file:

<launch>
    <node pkg="tf" type="static_transform_publisher" name="odom" args="-0.5 0 0 1.5707 0 1.5707 odom usb_cam 100" />
    <node pkg="tf" type="static_transform_publisher" name="base_link" args="0 0 0 0 0 0 base_link fcu 100" />
</launch>

And the ekf launch file:

<launch>

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

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

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

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

  <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="pose0" value="/vslam/pose"/>
  <param name="imu0" value="/mavros/imu/data"/>

  <rosparam param="pose0_config">[true,  true,  true,
                                  true, true, true,
                                  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,  true,  true]</rosparam>


  <param name="pose0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>

  <param name="pose0_relative" value="false"/>
  <param name="imu0_relative" value="true"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

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

</node>

</launch>

The robot_localization message:

    ---
header: 
  seq: 75
  stamp: 
    secs: 1441104536
    nsecs: 404455893
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.0327968532627
      y: 0.0107415590331
      z: 0.0672586918467
    orientation: 
      x: 0.000269673528914
      y: -9.62085270466e-05
      z: 0.000620642648884
      w: 0.999999766411
  covariance: [0.25178859948463683, -4.971036727512653e-06, -3.834708230879292e-05, 5.617159792226774e-07 ...