robot_localization: visual odom + imu + gps

edit

I'm running into some challenges with my robot_localization setup. I have visual odometry from a ZED2, IMU and GPS from an xsens MTi-710.

Goal: Configure my system as outlined in the navsat_transform_workflow diagram.

Problem: When just running the visual odometry and imu in a single ekf instance things work as expected. When adding in the second ekf instance and the navsat transform things go sideways. My map to odom transform goes haywire and I get warnings and errors. This is a benchtop implementation, so minimal movement from visual odom or imu. There is some noise on the GPS signal, but not as much as is showing up in the transform between odom and map.

Launch file:

<launch>
  <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
  <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

  <arg name="zed_node_name"        default="zed_node" />
  <arg name="camera_model"         default="zed2" />
  <arg name="publish_urdf"         default="true" />

  <arg name="camera_name"          default="zed" />

  <arg name="base_frame"           default="base_link" />
  <arg name="rviz_file"            default="zed-rtabmap.rviz" />

  <include file="$(find zed_wrapper)/launch/include/zed_no_tf.launch.xml">
    <arg name="camera_name"         value="$(arg camera_name)" />
    <arg name="svo_file"            value="$(arg svo_file)" />
    <arg name="stream"              value="$(arg stream)" />
    <arg name="node_name"           value="$(arg zed_node_name)" />
    <arg name="camera_model"        value="$(arg camera_model)" />
    <arg name="base_frame"          value="$(arg base_frame)" />
    <arg name="publish_urdf"        value="$(arg publish_urdf)" />
  </include>

  <include file="$(find xsens_mti_driver)/launch/display.launch" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
    <rosparam command="load" file="$(find robot_localization)/params/dual_ekf_navsat_qurrent.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
    <remap from="gps/fix" to="/gnss"/>
  </node>

</launch>

Parameters:

ekf_se_odom:
  frequency: 30
  silent_tf_failure: false
  sensor_timeout: 0.1
  two_d_mode: false
  transform_time_offset: 0.2
  transform_timeout: 0.2
  print_diagnostics: true
  debug: false

  publish_tf: true
  publish_acceleration: true
  permit_corrected_publichation: false
  predict_to_current_time: true

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: /zed_node/odom
  odom0_config: [true, true, true,
                 true, true, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 2
  odom0_nodelay: false
  odom0_differential: false
  odom0_relative: false

  odom0_pose_rejection_threshold: 5
  odom0_twist_rejection_threshold: 1

  pose0: /example/pose
  pose0_config: [true,  true,  true,
                true,  true,  true,
                false, false, false,
                false, false, false,
                false, false, false]
  pose0_differential: true
  pose0_relative: false
  pose0_queue_size: 5
  pose0_rejection_threshold: 2  # Note the difference in parameter name
  pose0_nodelay: false


  imu0: /imu/data
  # imu0: /example/imu_data

# The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                false, false, false,
                true,  true,  true]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 5
  imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
  imu0_twist_rejection_threshold: 0.8                #
  imu0_linear_acceleration_rejection_threshold: 0.8  #
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [Using default ...