Robot localization Fuse Imu and Pose data

edit

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