Robot localization Fuse Imu and Pose data
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 ...
I'm closing all but the newest question...
Please post your full config and sample input messages for every sensor input. NaN values almost always are the result of bad covariances.
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.
Hey Tom, thy for the help. I updated the question above