Ask Your Question
0

Robot localization Fuse Imu and Pose data

asked 2018-01-30 08:11:39 -0500

Max Kirsche gravatar image

updated 2018-02-01 09:42:26 -0500

Hi,

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

Edit1:

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:

header: 
  seq: 4798
  stamp: 
    secs: 1517413198
    nsecs: 703204352
  frame_id: "world"
orientation: 
  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]
angular_velocity: 
  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]
linear_acceleration: 
  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:

header: 
  seq: 6743
  stamp: 
    secs: 1517413308
    nsecs: 545397347
  frame_id: "camera"
pose: 
  pose: 
    position: 
      x: -0.00374244369306
      y: -0.0181854274021
      z: 0.641201285833
    orientation: 
      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 ...
(more)
edit retag flag offensive close merge delete

Comments

mgruhler gravatar imagemgruhler ( 2018-01-30 08:20:18 -0500 )edit

Please post your full config and sample input messages for every sensor input. NaN values almost always are the result of bad covariances.

Tom Moore gravatar imageTom Moore ( 2018-01-30 11:56:19 -0500 )edit

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.

Tom Moore gravatar imageTom Moore ( 2018-01-30 11:58:10 -0500 )edit

Hey Tom, thy for the help. I updated the question above

Max Kirsche gravatar imageMax Kirsche ( 2018-01-31 09:42:13 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2018-02-19 07:55:17 -0500

Tom Moore gravatar image

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?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-01-30 08:11:39 -0500

Seen: 353 times

Last updated: Feb 19 '18