[robot_localization] high pose drift with only IMU data
Hello,
I am trying to make use of the robot_localization
package in order to estimate the position of my robot, unfortunately I have only IMU data of type sensor_msgs/imu
.
I tried to configure the package to my situation with imu topic and Icouldn't get reasonable values. I am getting drift in thousands after minutes of running this launch file
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find ghattas_localization)/params/ekf.yaml" />
</node>
</launch>
yaml parameters file as following
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
print_diagnostics: true #echo the /diagnostics_agg topic for details
## IMU 0
imu0: /mavros/imu/data
odom_frame: odom_fil # Defaults to "odom" if unspecified
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
this is the output of my imu sensor
header:
seq: 76883
stamp:
secs: 1553471696
nsecs: 643437952
frame_id: "base_link"
orientation:
x: -0.00401678876775
y: 0.00692985006211
z: 0.18002671006
w: -0.983629110108
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity:
x: 0.00155547540635
y: 0.000273763580481
z: 0.00184202415403
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.06864655
y: -0.08825985
z: 9.77723005
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---
and this is the output of the topic /odometry/filtered
after mintues of running the launch file mentioned above
header:
seq: 85355
stamp:
secs: 1553471740
nsecs: 522055387
frame_id: "odom_fil"
child_frame_id: "base_link"
pose:
pose:
position:
x: 5931.45757968
y: -524851.96709
z: -83022.4001185
orientation:
x: -3.09389446171e-05
y: -0.00255077454028
z: 0.0352035710502
w: 0.999376906466
covariance: [123428762.20239958, 14303.125278606882, -432994.46126881544, -0.1112066300728559, -2.6491734572265484, 16.50020719493778, 14303.125278592377, 117327753.84775318, -978791.8404554238, 2.6481353764573927, -0.19240492653648597, -0.17084136219435272, -432994.4612687591, -978791.8404553946, 193571512.2978465, -16.679576436670256, 1.385559988732174, 0.0005322252938017805, -0.11120663007285585, 2.648135376457387, -16.679576436670246, 0.03404565909294526, 7.622682310836132e-10, -7.402346050531404e-07, -2.6491734572265524, -0.1924049265364862, 1.3855599887321741, 7.622682310838457e-10, 0.03404565076858593, 1.2813554639653757e-07, 16.50020719493774, -0.1708413621943528, 0.0005322252938017805, -7.402346050531408e-07, 1.2813554639653757e-07, 0.04780378874325498]
twist:
twist:
linear:
x: -35.3374913315
y: -431.961762545
z: -68.7201530547
angular:
x: 0.000821793150427
y: 0.00030448779041
z: 0.00136453106476
covariance: [60.3615155475993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60.3615155475993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 96.5658004680656, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2180987398690317e-07, -2.4508723486153523e-30 ...