robot_localization estimate drifts for a stationary robot in rviz
I am using robot_localization to fuse odometry, imu and gps measurements. However once my robot is spawn in rviz, it starts drifting. The /imu/data output keeps changing and the estimated pose is inaccurate.
My robot_localization configuration is:
<rosparam command="load" file="$(find fav)/config/dual_ekf_navsat_transform.yaml"/>
<node name="ekf_se_odom" pkg="robot_localization" type="ekf_localization_node" output="screen" clear_params="true" />
<node name="ekf_se_map" pkg="robot_localization" type="ekf_localization_node" output="screen" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" output="screen" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
<remap from="gps/fix" to="/navsat/fix" />
<remap from="imu/data" to="/imu/data" />
</node>
and my dual_ekf_navsat_transform.yaml is
ekf_se_odom:
frequency: 100
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.5
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
odom0: fav_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
imu0: /imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
use_control: false
process_noise_covariance: [1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0 ...
A couple of things, how are you handling your IMU data, is it being passed through a filter such as
imu_filter_madgwicka node such as this will give you the flexibility to adjust the gain values and alleviate some of that drift. If you are already doing this and cannot get your imu drift any better, the next thing to check would be the hardware layer, is the imu faulty or simply bad? Imu and wheel odometry drift is unavoidable and an ekf will not solve it. I run a dual ekf setup and in my second gps node, i don't fuse imu and wheel odometry, luckily i have a gnss provided heading so this outputs a highly accurate (although still discontinuous) map frame with effectively zero drift.@agurman Thanks for your response. Can the imu_filter_node be used with libhector_gazebo_ros_gps.so plugin?