robot_localization estimates wobbling around too much when fusing GPS
I am trying to fuse imu, gps, and wheel odometry sources using robot_localization package. To demonstrate the issue, I simplified the setup to use only imu and gps (both from dji N3 gps module) . My robot_localization setup is based on this answer and the config files provided in the robot_localization git repo here.
It launch file looks like:
<launch>
<rosparam command="load" file="$(find mario_launch)/navigation/rl_dual_ekf_navsat.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<!-- Published Topics -->
<remap from="odometry/filtered" to="/odometry/filtered"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
<rosparam param="datum">[50.7273430338, 7.08592135637, 0.0, map, base_link]</rosparam>
<!-- Subscribed Topics -->
<remap from="odometry/filtered" to="/odometry/filtered"/>
<!-- Input -->
<remap from="/imu/data" to="/dji_sdk/imu"/>
<remap from="/gps/fix" to="/dji_sdk/gps_position"/>
<!-- Output -->
<remap from="/odometry/gps" to="/odometry/gps"/>
</node>
</launch>
My config file looks like:
ekf_se_odom:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.1
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
imu0: /dji_sdk/imu
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: true
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 ...