Map drifted immediately while odometry is from robot_localization
Dear all, I have been trying robot_localization on RTAB-Map generated VO and IMU(mpu6050). I moved a cart equipped with ZED+IMU around to form a loop. Though I didn't have a ground truth to verify if fusion improves the trajectory. I found that map drifted immediately in fusion mode. I wonder if it's possible for you to look into my launch file and see what might be wrong? Thank you so much. Here's the EKF part in my launch file
<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
<param name="frequency" value="50"/>
<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="$(arg frame_id)"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/vo"/>
<param name="imu0" value="$(arg imu_topic)"/>
<param name="debug" value="true"/>
<param name="debug_out_file" value="/home/acid/acid/catkin_ws/src/robot_localization/acid_debug_ekf_localization.txt"/>
<!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="odom0_config">[true, true, true,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam if="$(arg imu_ignore_acc)" param="imu0_config">[
false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false] </rosparam>
<rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true] </rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="true"/>
<param name="odom0_relative" value="true"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="50"/>
<param name="imu0_queue_size" value="50"/>
<!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="process_noise_covariance">[0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0015]</rosparam>
<!-- The values are ordered as x, y,
z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
and here are messages of odometry/filtered,imu/data and /vo
Thanks!!
messages of odemetry/filtered
header:
seq: 453
stamp:
secs: 1527820994
nsecs: 969716072
frame_id: "odom"
child_frame_id: "zed_center"
pose:
pose:
position:
x: 4.4771898166
y: -0.0604899307322
z: 0.230771803538
orientation:
x: 0.0238588792563
y: -0.0160936743631
z: 0.00539074957569
w: 0.99957125176
covariance:
[0.004340237423027139, 4.8924672168027036e-05, -9.285468997989843e-05, 1.0208139372847594e-05, 0.0002365120100251607, 0.0001310626548135436, 4.8924672168027064e-05, 0.0059594005670448435, -2.9179290458871165e-06, -0.00012935296453411477, 1.6243655598320525e-05, 0.004173402798782131, -9.285468997989841e-05, -2.9179290458871025e-06, 0.005908640374803734, -0.00018705378555409168, -0.0029378890931033587, -1.7458187356253646e-06, 1.0208139372847621e-05, -0.000129352964534115, -0.00018705378555409208, 0.027456169394026616, -0.0005650190293165442, 0.0007844900690369676, 0.00023651201002516096, 1.624365559832051e-05, -0.0029378890931033596, -0.000565019029316544, 0.008516691896290842, -5.0832936970778106e-06, 0.00013106265481354365, 0.004173402798782132, -1.7458187356253746e-06, 0.0007844900690369676, -5.083293697077799e-06, 0.012464848326201445]
twist:
twist:
linear:
x: 0.592057718697
y: -0.0278952504307
z: 0.0233008288102
angular:
x: 0.0418123248039
y: -0.0225535440933
z: 0.0518114256146
covariance:
[0.0027399081952755623, 2.762919360447718e-07, -5.257770799687194e-07, -3.663788000587037e-45, -7.702906063701453e-44, -1.279844213352637e-45, 2.762919360447722e-07, 0.002757651973526687, -1.0876729941683574e-07, 1.5323413756579002e-44, 1.8382684204342065e-45, -3.0219558283764765e-44, -5.257770799687199e-07, -1.0876729941683783e-07, 0.0034348307074018943, 1.2709086734642427e-43, 2.2715907434133832e-42, -7.114643771127561e-47, -3.663788000587047e-45, 1.5323413756578952e-44, 1.2709086734642405e-43, 4.99975023910877e-10, -6.766893224126127e-80, 1.4690937836313493e-82, -7.70290606370148e-44, 1.8382684204342043e-45, 2.27159074341338e-42, -6.76689322412613e-80, 4.99975023910877e-10, -3.305623065503715e-83, -1.2798442133526368e-45, -3.0219558283764725e-44, -7.114643771127396e-47, 1.469093783631353e-82, -3.305623065503658e-83, 4.999875113316257e-10]
and message of imu/data
header:
seq: 1744
stamp:
secs: 1527820991
nsecs: 559710677
frame_id: "imu_link"
orientation:
x: -0.00440620258451
y: 0.00163966044784
z: 0.000599395483732
w: 0.99997504428
orientation_covariance:
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0266316109008
y: -0.0532632218016
z: -0.0266316109008
angular_velocity_covariance:
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: -17.2501281738
y: 11.2206665039
z: 12.8576623535
linear_acceleration_covariance:
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
and /vo
header:
seq: 188
stamp:
secs: 1527820994
nsecs: 983163712
frame_id: "odom"
child_frame_id: "zed_center"
pose:
pose:
position:
x: 4.49915456772
y: -0.057336576283
z: 0.218076586723
orientation:
x: 0.00119549389511
y: -0.0059930772054
z: 0.0101260343409
w: 0.999930056178
covariance:
[0.10574581880383194, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.10574581880383194, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.10574581880383194, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00901110356496647, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00901110356496647, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00901110356496647]
twist:
twist:
linear:
x: 0.784875690937
y: 0.0459038279951
z: 0.0245149638504
angular:
x: -0.0218055471778
y: -0.0185214690864
z: 0.00288812420331
covariance:
[0.05287290940191597, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05287290940191597, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05287290940191597, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004505551782483235, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004505551782483235, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004505551782483235]
Asked by Yuli on 2018-08-08 06:19:14 UTC
Comments