Rosanswers logo


I´m currently trying to use Robot_localization to localize an UAV. I´m not able to get any sufficient Result out of the system. At this point i have no other idea what to do.

In my first steps, i only want to Fuse the Imu Data from the flight control with a Pose Msg which contains position and orientation.

My EKF param File looks like this (everything else is default):

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

imu0: /imuConverted
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: true
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

The covariance for each sensor was measured and then squareed. They are static but to my knowledge it should give a good estimate.

imu_naze_orientation_covariance:          [2.772709e-16, 0.0, 0.0, 0.0, 3.520149e-14, 0.0, 0.0, 0.0, 7.357836e-14]
imu_naze_angular_velocity_covariance:     [7.097134e-14, 0.0, 0.0, 0.0, 6.437836e-14, 0.0, 0.0, 0.0, 7.357836e-14]
imu_naze_linear_acceleration_covariance:  [1.56816e-07, 0.0, 0.0, 0.0, 1.48996e-7, 0.0, 0.0, 0.0, 8.15409e-7]

ar_tag_covariance: [2.830886e-14, 3.008685e-14, 2.368144e-18, 2.0e-14, 2.0e-14, 2.0e-14]

Could it be a problem with the coordinate systems, maybe i have got some conversions false? Most of the time, robot_localization stops and just outputs nan values.

yours, Max


The Input of my Imu Msg, standing still the Imu measure z=9,81 if rolled with the left side up its y=9,81 and ich pitched at +90 x=-9,81:

  seq: 4798
    secs: 1517413198
    nsecs: 703204352
  frame_id: "world"
  x: -0.0203049678225
  y: -0.0188459477819
  z: 0.646843813626
  w: 0.762119163457
orientation_covariance: [2.772709e-16, 0.0, 0.0, 0.0, 3.520149e-14, 0.0, 0.0, 0.0, 7.357836e-12]
  x: 0.0980235189199
  y: 0.0254552774131
  z: -0.0255530066788
angular_velocity_covariance: [7.097134e-14, 0.0, 0.0, 0.0, 6.437836e-14, 0.0, 0.0, 0.0, 7.357836e-14]
  x: 0.00307600945234
  y: -0.554165482521
  z: 9.73908805847
linear_acceleration_covariance: [1.56816e-07, 0.0, 0.0, 0.0, 1.48996e-07, 0.0, 0.0, 0.0, 8.15409e-07]

My ArTag Position:

  seq: 6743
    secs: 1517413308
    nsecs: 545397347
  frame_id: "camera"
      x: -0.00374244369306
      y: -0.0181854274021
      z: 0.641201285833
      x: 0.0357912246636
      y: -0.0256411485802
      z: 0.745954521929
      w: -0.664312705697
  covariance: [2.830886e-14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.008685e-14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.368144e-18, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2e-14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2e-14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2e-14]

The Imu Msg come in with 50 Hz and the ArTag Position with 10 Hz. I Changed the ArTag EKF config, so that it only measure the Yaw of the UAV and the IMU config to measure pitch and Roll.

I think the ekf ignores my ArTag Pose.

Edit2: I just saw that the frame ID of the Imu msg was wrong, i changed it to base_link but its still ignoring the Pose Data and the pitch and roll values of the Imu

Edit3: After some more test, i got it that the x and y position seems stable but my z is drifting like hell against -infinity. The yaw of the uav is also spinning arround.

Edit4: After reviewing the covariances i saw that i had a misunderstanding, they all have been square 2 too high. I fixed it, but it doesn´t fix the problem from Edit3.

Originally posted by Max Kirsche on ROS Answers with karma: 3 on 2018-01-30

Post score: 0

Original comments

Comment by mgruhler on 2018-01-30:
I'm closing all but the newest question...

Comment by Tom Moore on 2018-01-30:
Please post your full config and sample input messages for every sensor input. NaN values almost always are the result of bad covariances.

Comment by Tom Moore on 2018-01-30:
BTW, are you sure your IMU orientation and your IMU orientation are the same? You are fusing both of them absolutely, so they need to agree, or the state estimate will jump around.

Comment by Max Kirsche on 2018-01-31:
Hey Tom, thy for the help. I updated the question above


1 Answer 1


Rosanswers logo

I'd do the following:

  • Get rid of all rejection_thresholds. Those are advanced parameters that should only be used once you have everything else working
  • Try running with no linear accelerations (set the last three values in your IMU config to false).

Your AR tag data is in the camera frame. Are you providing a transform to the odom frame?

Originally posted by Tom Moore with karma: 13689 on 2018-02-19

This answer was ACCEPTED on the original site

Post score: 1


Your Answer

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