Robot_Localization: IMU doesn't update position
I am trying to do exactly the same as already asked in this forum question. I have an IMU (angular rate and linear acceleration) and want to interface it with the robot_localization package, while I am waiting for the rest of my sensors. Unfortunately, the filter output is not really affected by the IMU input: in the filtered odometry topic, the orientation slowly drifts and the position stays at zero. The linear twist is zero and the angular twist is slightly varying around zero (therefore the slow drift in orientation only). I have read the mentioned forum question carefully and looked at the possible problems, but still couldn't figure it out. I especially put the IMU on a flat surface s.t. Z acceleration +9.81 and moved it around on the surface, but no effect on the filter output. Any help is greatly appreciated.
Edit 1: Here is a link to a bag file that contains IMU data. The IMU is put on a flat surface and then moved around by hand a little bit.
Below, you can find examples of an IMU msg, odometry msg, launch file and diagnostics msg. The diagnostics msg states that "Erroneous data or settings detected for a robot_localization state estimation node", because neither position nor its velocity is being measured. Does that mean, the filter does not work at all with angular rate/ linear acceleration only?
Edit 2: I found out that the outlier rejection thresholds in the launch file were set too small. With a value 1.0 all measurements were treated as outliers. Increasing the thresholds, the measurements are incorporated in the filter output. With thresholds of 10.0
for all IMU quantities, only angular velocities are treated as inliers and the output drifts reasonably slow. The linear acceleration is not considered though. Increasing the imu0_linear_acceleration_rejection_treshold
to 100.0
incorporates also linear acceleration measurements, and the position drifts off quite quickly (as expected). So my only remaining question is whether this outlier rejection thresholds are quantitatively ok (to me they seem to be quite high in terms of mahalonobis distance?) or my IMU is extremely noisy.
This is an example of an IMU message (when the IMU is on the flat surface and not moving):
header:
seq: 98958
stamp:
secs: 1457632055
nsecs: 901920531
frame_id: base_link
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 100.0]
angular_velocity:
x: -0.00339975870097
y: 0.000720988133907
z: -0.00110921803251
angular_velocity_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
linear_acceleration:
x: -0.0364477649002
y: -0.0901292956104
z: 9.84980641486
linear_acceleration_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
---
This is an example of an odometry/filtered msg:
header:
seq: 5852
stamp:
secs: 1457632057
nsecs: 478465384
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: 0.0
y: 0.0
z ...