Robotics StackExchange | Archived questions

robot_localization: visual odom + imu + gps

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 navsattransformworkflow diagram.

image description

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]

  initial_estimate_covariance: [Using default]

ekf_se_map:
  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: false

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

  odom0: /odometry/gps
  odom0_config: [true, true, true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  odom1: /zed_node/odom
  odom1_config: [true, true, true,
                 true, true, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

  # The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
  imu0: /imu/data
  # imu0: example/imu_data
  imu0_config: [false, false, false,
                true, true,  true,
                false, false, false,
                true, true, true,
                true, true, true]
  imu0_nodelay: true
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [Using default]
  initial_estimate_covariance: [Using default]

navsat_transform:
  frequency: 30
  delay: 3.0
  magnetic_declination_radians: 0.0429351  # For lat/long 55.944831, -3.186998
  yaw_offset: 1.570796327  # IMU reads 0 facing magnetic north, not east
  zero_altitude: false
  broadcast_cartesian_transform: true
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false

Warning & Errors seen:

[ WARN] [1620257072.823635719]: Transform from imu_link to base_link was unavailable for the time requested. Using latest instead.

[ WARN] [1620257074.843753539]: Transform from imu_link to base_link was unavailable for the time requested. Using latest instead.

[ERROR] [1620257076.080381169]: Critical Error, NaNs were detected in the output state of the filter. This was likely due to poorly conditioned process, noise, or sensor covariances.
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)
         at line 240 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)
         at line 255 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Error:   Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)TF_NAN_INPUT: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)

         at line          at line 240240 in  in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp/tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp

Error:   Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)

         at line          at line 255255 in  in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp/tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp

[ERROR] [1620257076.080817675]: Ignoring transform for childframeid "odom" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)

I know this is a lot, hope someone can make some sense of where the issue is.

Thank you!

Asked by ClintQ on 2021-05-05 19:06:43 UTC

Comments

First thing to do would be to check if there are any other sources that are publishing the same transformations that you are publishing with the ekf.

My guess is that the NaNs are coming from the GPS and that is were the problem must be.

Can you check how consistently /odometry/gps is being published?

Asked by turtleMaster20 on 2021-05-06 02:15:38 UTC

/odometry/gps is being published at 4Hz pretty consistently until it crashes and then I get:

average rate: 1.587
    min: 0.200s max: 2.033s std dev: 0.72066s window: 55
no new messages

I also noticed that in my tf tree, I lost the transform from map to odom and only have map to utm after the failure occurs. Is this due to the warning that I'm seeing?

[ WARN] [1620322379.574716691]: Transform from imu_link to base_link was unavailable for the time requested. Using latest instead.

Asked by ClintQ on 2021-05-06 12:35:11 UTC

Transform form map to odom should be published by ekf_se_odom, try to see if this node is still running.

The error you see can be ignored as imu to base_link should remain constant throughout.

Are you using these parameters per chance?

Check this link for proper setup instructions if you have not already.

One quick check could also be to use the ukf in the package. I believe you can use similar params (if not identical) to use the nodes.

Asked by turtleMaster20 on 2021-05-12 04:37:09 UTC

Answers

Please include one sample message from each sensor input. Even without that, though, I can make some observations.

  1. For starters, please disable all rejection_threshold parameters. Those are advanced parameters that should only be used after you have your system working more or less as you want it, and are just looking to squash the odd outlier.
  2. Is the VO data in full 3D? I assume so. If I were you, I would not fuse two absolute pose sources unless you know they will always agree. In this case, your VO data will slowly diverge from the IMU orientation, so I'd just use roll and pitch from one of those sources, and then, if the sensor provides it, fuse velocity data from the other. This is just in reference to your odom-frame EKF.
  3. In your map frame EKF, you are fusing absolute pose data from your GPS and your odometry source. Assuming your VO data is reported in the odom frame, then every time the EKF receives a measurement, it has to transform that data to the map frame (using the very transform the EKF is creating), and then fuse it absolutely. And then that pose has to agree with your GPS. VO data will drift over time, so those are going to diverge. In this EKF, I would fuse only velocity data from the VO source, if possible.
  4. All that aside, I would expect this to not behave well, but not produce NaNs. NaNs very often indicate issues with sensor data, like ill-formed covariances. This is why you need to include sample sensor messages.

Asked by Tom Moore on 2021-06-22 03:52:26 UTC

Comments