robot_localization estimates wobbling around too much when fusing GPS

asked 2020-01-31 09:47:32 -0600

Arul Selvam gravatar image

updated 2020-02-02 08:15:09 -0600

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 ...
(more)
edit retag flag offensive close merge delete