robot_localization estimate (using only IMU) drifts for a stationary robot (Gazebo Model)
I am trying to localize my robot (Gazebo model) in a known map. The robot has a lidar and IMU on it. To start with I tried visualizing the robot_localization estimate in rviz. I am feeding only IMU data to the ekf estimator. I did not expect the Odometry estimate to be very accurate, however even when the robot is not moving the estimate generated by robot_localization keeps on moving. Also the estimate covariance keeps on increasing (this can be observed in rviz). What could possibly be going wrong?
I am publishing a static transform to place the map frame at the odom frame. Here is how my tf tree looks like (only relevant part shown):
This is the robot_localization configuration:
frequency: 30
sensor_timeout: 0.05
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 1.0
print_diagnostics: false
debug: false
publish_tf: true
publish_acceleration: false
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
imu0: /imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true
dynamic_process_noise_covariance: true
process_noise_covariance: [ sample values from robot_localization github repo]
initial_estimate_covariance: [ value of 0.01 for variables set to true in imu0_config]
Here are the ROS IMU plugin settings:
<gazebo reference="base_imu_link">
<gravity>true</gravity>
<sensor name="razor_9dof_imu" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<topic>/imu</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>/imu</topicName>
<bodyName>base_imu_link</bodyName>
<updateRateHZ>100</updateRateHZ>
<gaussianNoise>0.05</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>base_imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
Here is the IMU data:
header:
seq: 1791
stamp:
secs: 37
nsecs: 148000000
frame_id: "base_imu_link"
orientation:
x: 0.0246637627368
y: -0.026593164317
z: 0.0668625611945
w: 1.02289534517
orientation_covariance: [0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005]
angular_velocity:
x: -0.0326675653364
y: -0.0340936938196
z: -0.0323116318276
angular_velocity_covariance: [0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005]
linear_acceleration:
x: -0.0783367912198
y: -0.00500801404049
z: 9.79784343093
linear_acceleration_covariance: [0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005]