0
$\begingroup$

Rosanswers logo

Hi,

I have a standard configuration for robot_localization to give me a odom->base_footprint transform. We have been using this sensor data and transformations with robot_pose_ekf for over a year now with no issues. When migrating to robot_localization, randomly, after some inconsistent period of time, the entire /odometry/filtered message becomes filled with nans. This happens with no changes in the odom or imu messages (not giving out nans, nothing out of the ordinary here). It has occurred before adding lidar_odometry (odom0).

We get errors about TF_NAN_INPUT and TF_DENORMALIZED_QUATERNION from using the output /odometry/filtered (because their filled with nans when we populate the TF messages) to update TF outside of robot_localization.

I have included example imu and odom messages, all fields of them are being actively published without any NaNs. I also added the normal robot_localization output and what our nan message looks like, with the robot_localization_config.yaml file.

Any thoughts?

odom (odom1) message from wheel encoders

       header: 
      seq: 176323
      stamp: 
        secs: 1507078251
        nsecs: 535428047
      frame_id: odom
    child_frame_id: base_footprint
    pose: 
      pose: 
        position: 
          x: 0.0
          y: 0.0
          z: 0.0
        orientation: 
          x: 0.0
          y: 0.0
          z: 0.0
          w: 1.0
      covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
    twist: 
      twist: 
        linear: 
          x: 0.0
          y: 0.0
          z: 0.0
        angular: 
          x: 0.0
          y: 0.0
          z: 0.0
      covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]

imu message from imu

    header: 
      seq: 157520
      stamp: 
        secs: 1507078338
        nsecs: 195523977
      frame_id: imu_link
    orientation: 
      x: 0.0101925589928
      y: -0.00273427809162
      z: -0.501709473646
      w: 0.864971814291
    orientation_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]
    angular_velocity: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular_velocity_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]
    linear_acceleration: 
      x: -0.0453446374339
      y: 0.177417214661
      z: 9.64665574154
    linear_acceleration_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]

normal robot_localization output

header: 
  seq: 78377
  stamp: 
    secs: 1507078377
    nsecs: 253389359
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: -0.000973945901217
      y: 0.126273200099
      z: -1.61762382101e-17
    orientation: 
      x: -0.000105876156922
      y: -0.000161010011301
      z: 3.51162535183e-06
      w: 0.999999981427
  covariance: [9.997287159166042e-07, 5.664801003518761e-29, -2.1928212327851335e-23, 1.995211302619292e-21, 1.9952112977467756e-21, 1.9952112987625185e-21, 5.66480204376903e-29, 9.997287159166042e-07, -1.0024367487167898e-23, 4.133819569988897e-21, 4.133819559834025e-21, 4.133819561950336e-21, -2.192821232213108e-23, -1.0024367489488378e-23, 9.84239690692538e-07, -3.559546469538204e-19, -3.5595464608106723e-19, -3.559546462629691e-19, 1.995211302619292e-21, 4.133819569988894e-21, -3.5595464695382053e-19, 3.7351064403771496e-06, 8.197240597870183e-07, 8.197240588830697e-07, 1.995211297746775e-21, 4.133819559834024e-21, -3.5595464608106727e-19, 8.197240597870181e-07, 3.73510644970679e-06, 8.197240635485211e-07, 1.995211298762519e-21, 4.133819561950336e-21, -3.559546462629692e-19, 8.197240588830698e-07, 8.197240635485212e-07, 3.7351064478981465e-06]
twist: 
  twist: 
    linear: 
      x: -2.48706599015e-08
      y: -2.83532553066e-08
      z: 1.97893646378e-12
    angular: 
      x: 1.21518334371e-05
      y: 1.21518334371e-05
      z: 1.21518334371e-05
  covariance: [0.00012761360927131761, 4.276062538471263e-17, -2.0189041419998664e-13, 5.307289670456897e-25, 5.307289670456897e-25, 5.307289670456897e-25, 4.2760625384725265e-17, 0.0001276136092713545, 1.3277594998183542e-13, 1.1951157076208104e-24, 1.1951157076208104e-24, 1.1951157076208104e-24, -2.0189041419998664e-13, 1.3277594998183545e-13, 0.000127612982381363, -1.0106819716410595e-22, -1.0106819716410596e-22, -1.0106819716410596e-22, 5.307289670456896e-25, 1.1951157076208108e-24, -1.0106819716410592e-22, 3.7350920785276993e-06, 8.197096932737929e-07, 8.197096932737929e-07, 5.307289670456898e-25, 1.195115707620811e-24, -1.0106819716410595e-22, 8.19709693273793e-07, 3.7350920785276993e-06, 8.197096932737929e-07, 5.307289670456896e-25, 1.1951157076208112e-24, -1.0106819716410595e-22, 8.197096932737929e-07, 8.197096932737929e-07, 3.7350920785276993e-06]

robot_localization output when naning

  stamp:                                                              
    secs: 1506764997
    nsecs: 770193100
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: nan
      y: nan
      z: nan
    orientation: 
      x: nan
      y: nan
      z: nan
      w: nan
  covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, na
n, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
twist: 
  twist: 
    linear: 
      x: nan
      y: nan
      z: nan
    angular: 
      x: nan
      y: nan
      z: nan
  covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, na
n, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]

yaml config

# Base parameters
frequency: 40
sensor_timeout: 0.1
two_d_mode: false
publish_tf: false

use_control: false
stamped_control: false

print_diagnostics: false
debug: false
debug_out_file: "debug_robot_localization.txt"

# Frame parameters
map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom

transform_time_offset: 0.

# Sensor configuration
odom0: /odom_rf2o
odom0_config: [
    true,   true,   true,   # x, y, z
    false,  false,  false,   # roll, pitch, yaw
    false,  false,  false,  # x vel, y vel, z vel
    false,  false,  false,  # roll vel, pitch vel, yaw vel
    false,  false,  false   # x accel, y accel, z accel
]
odom0_differential: false
odom0_relative: false #maybe should be true?
odom0_queue_size: 50

odom1: /odom
odom1_config: [
    false,  false,   false,  # x, y, z
    false,  false,  false,  # roll, pitch, yaw
    true,   true,   false,   # x vel, y vel, z vel
    false,  false,  false,   # roll vel, pitch vel, yaw vel
    false,  false,  false   # x accel, y accel, z accel
]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 50

imu0: /imu
imu0_config: [
    false,  false,  false,  # x, y, z
    true,   true,   true,   # roll, pitch, yaw
    false,  false,  false,  # x vel, y vel, z vel
    true,   true,   true,   # roll vel, pitch vel, yaw vel
    false,  false,  false   # x accel, y accel, z accel (ideal want true true false)
]
imu0_differential: false
imu0_relative: false
imu0_queue_size: 50
imu0_remove_gravitational_acceleration: true
imu0_gravitational_acceleration: 9.80665



# Covariance intialization
process_noise_covariance: [
    0.06,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # x
    0,     0.06,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # y
    0,     0,     0.001, 0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # z
    0,     0,     0,     0.001, 0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # roll
    0,     0,     0,     0,     0.001, 0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # pitch
    0,     0,     0,     0,     0,     0.001, 0,     0,     0,     0,     0,     0,     0,     0,     0,    # yaw
    0,     0,     0,     0,     0,     0,     0.025, 0,     0,     0,     0,     0,     0,     0,     0,    # x_dot
    0,     0,     0,     0,     0,     0,     0,     0.025, 0,     0,     0,     0,     0,     0,     0,    # y_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0.025, 0,     0,     0,     0,     0,     0,    # z_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0.001, 0,     0,     0,     0,     0,    # roll_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0.001, 0,     0,     0,     0,    # pitch_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0.001, 0,     0,     0,    # yaw_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0.01,  0,     0,    # x_ddot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0.01,  0,    # y_ddot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0.01, # z_ddot
]

initial_estimate_covariance: [
    1e-2,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # x
    0,     1e-2,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # y
    0,     0,     1e-9,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # z
    0,     0,     0,     2e-3,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # roll
    0,     0,     0,     0,     2e-3,  0,     0,     0,     0,     0,     0,     0,     0,     0,     0,    # pitch
    0,     0,     0,     0,     0,     2e-3,  0,     0,     0,     0,     0,     0,     0,     0,     0,    # yaw
    0,     0,     0,     0,     0,     0,     3e-2,  0,     0,     0,     0,     0,     0,     0,     0,    # x_dot
    0,     0,     0,     0,     0,     0,     0,     3e-2,  0,     0,     0,     0,     0,     0,     0,    # y_dot
    0,     0,     0,     0,     0,     0,     0,     0,     1e-9,  0,     0,     0,     0,     0,     0,    # z_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-3,  0,     0,     0,     0,     0,    # roll_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-3,  0,     0,     0,     0,    # pitch_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-3,  0,     0,     0,    # yaw_dot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-9,  0,     0,    # x_ddot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-9,  0,    # y_ddot
    0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     0,     1e-9, # z_ddot
]

Originally posted by stevemacenski on ROS Answers with karma: 8272 on 2017-10-03

Post score: 1


Original comments

Comment by Humpelstilzchen on 2017-10-05:
Did you check the imu & odom inputs on the exact time when robot_localization outputs NaN values? I recommend to use bag files to analyze the messages.

Comment by Humpelstilzchen on 2017-10-05:
Also the covariances are really small (arithmetic errors?), how do you get that accurate odometry?

Comment by stevemacenski on 2017-10-05:
There are no NaNs anywhere in any of the inputs to robot_localization, I have checked with multiple bag files. Nothing on the inputs are out of the ordinary as far as I can tell in values, normalization, rates, frames, delay, etc. Which covariances are you refering to? EKF, odom, or IMU?

Comment by Humpelstilzchen on 2017-10-05:
All covariances. IMU and ODOM are both 0.000001, that is about micrometer accuracy. And the output of robot_localization is even lower. In your initial_estimate_covariance I even see multiple 1e-9.

Comment by stevemacenski on 2017-10-06:
They are accurate, those numbers could use some tuning but I just used some defaults for those to get robot_localization off the ground. These are the same values that have worked with robot pose ekf for over a year now, however. Would robot_localization create NaNs here where robot pose ekf didnt?

Comment by Tom Moore on 2017-10-12:
Can you post a bag file that includes this happening? The only time I have ever seen NaN values is through sensor data or ill-conditioned covariance matrices.

Comment by Tom Moore on 2017-10-12:
Also, what is publishing the transform to the IMU frame?

Comment by stevemacenski on 2017-10-17:
I don't have a bag file off hand but from the ill-conditioned covariances clicks well. I'm starting to run it today with updated covariances (IMU only on main diagonal, odom not 1e-6) and I'll update back if the issue resumes

Comment by Tom Moore on 2017-10-17:
I think your odom covariance matrix was invertible, though it had a large condition number. Regardless, the EKF will ignore covariance values for variables you aren't fusing.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I’d have to see a bag to be sure, but your IMU covariance matrices are definitely ill-conditioned. Try to invert a 3x3 matrix where all the values are the same:

https://m.matrix.reshish.com/inverse.php

You need to put zeros in the off-diagonal entries.


Originally posted by Tom Moore with karma: 13689 on 2017-10-12

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by stevemacenski on 2017-10-17:
Thanks! I'm recording a bag file right now with the new rational covariances. If it comes back, I'll post the bag file, but I do expect this to resolve the problem since I have noted the error in the inverse - section of the code. If it turns out to be the case, I'll PR a ROS_WARN for it.

Comment by Tom Moore on 2017-10-17:
Excellent, thanks very much for contributing. If you find the issue is solved, would you mind marking this answer as accepted? Thanks!

Comment by stevemacenski on 2018-02-12:
Sorry to get back to you so late, but it did solve my problem. I am PRing now some log statements to detect this issue since it appears to be very common

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.