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:
<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>
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, 0.001132411300727592, -0.00016460156245969569, -4.971036727512627e-06, 0.251802630760143, -3.866513136687686e-05, -0.001132338668495088, 6.963000174227653e-07, 0.0005564970580662025, -3.8347082308792896e-05, -3.866513136687682e-05, 0.3528520627453843, 0.00015796138409329648, -0.0005195820989177445, 6.036177571201076e-08, 5.617159792226772e-07, -0.0011323386684950888, 0.00015796138409329648, 0.03157641168651724, 7.257794513246155e-10, 4.91036385931138e-07, 0.0011324113007275926, 6.963000174227649e-07, -0.0005195820989177441, 7.257794513253871e-10, 0.03157641321713827, 1.1834706590470503e-06, -0.0001646015624596956, 0.0005564970580662015, 6.036177571201076e-08, 4.910363859311376e-07, 1.1834706590470499e-06, 0.043689017073481766]
twist:
twist:
linear:
x: 0.0210921228234
y: 0.0062526978065
z: 0.0547688153658
angular:
x: -0.000258302719997
y: -0.000517417815119
z: -0.00076631415027
covariance: [0.06239698790077553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06239698790077553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09983517550002659, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2179398991361593e-07, 5.2601439590013494e-33, 3.0621846696809155e-30, 0.0, 0.0, 0.0, 5.260143958793435e-33, 1.2179398991361593e-07, 8.22777177602546e-30, 0.0, 0.0, 0.0, 3.0621846696809866e-30, 8.22777177602553e-30, 1.218204673952421e-07]
---
Debug output with IMU turned off:
tf_prefix is
map_frame is map
odom_frame is odom
base_link_frame is base_link
world_frame is odom
transform_time_offset is 0
frequency is 30
sensor_timeout is 0.1
two_d_mode is false
print_diagnostics is false
Subscribed to /vslam/pose (pose0)
pose0_differential is true
pose0_rejection_threshold is 1.79769e+308
pose0_queue_size is 1
pose0 update vector is [t t t f f f f f f f f f f f f ]
------ RosFilter::integrateMeasurements ------
Integration time is 1441162630.7626991272
0 measurements in queue.
Filter not yet initialized.
----- /RosFilter::integrateMeasurements ------
WARNING: failed to transform from usb_cam->odom for pose0 message received at 1441162630.990499904. No transform exists from source to target frame.
------ RosFilter::integrateMeasurements ------
Originally posted by K7 on ROS Answers with karma: 94 on 2015-09-01
Post score: 0
Original comments
Comment by Tom Moore on 2015-09-01:
Can you show me a sample output message (from the EKF)?
Comment by K7 on 2015-09-01:
Sure. I'll update the question now.
Comment by K7 on 2015-09-01:
@Tom Moore sample output is there now. Let me know if there is anything else that can be of use.
Comment by K7 on 2015-09-01:
BTW I'm using ethzasl_ptam. Not sure if it is compatible or not.
Comment by K7 on 2015-09-01:
@Tom Moore would it help if all the messages had the same time stamp?