robot_localization drift
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 ...
Can you show me a sample output message (from the EKF)?
Sure. I'll update the question now.
@Tom Moore sample output is there now. Let me know if there is anything else that can be of use.
BTW I'm using ethzasl_ptam. Not sure if it is compatible or not.
@Tom Moore would it help if all the messages had the same time stamp?