Question

Hello,

I am trying to use the robot localization package into my underwater robot. at the moment I am doing some dry tests on the system to make sure everything is working properly.

I have my parameter Yaml file as following:

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
print_diagnostics: true #echo the /diagnostics_agg topic for details


## IMU 0 data from the pixhawk
imu0: /mavros/imu/data
odom_frame: odom_fil

imu0_config: [false, false, false,
              true,  true,  true,
              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                 #
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: false

# odom from the visual odometry
odom0: /stereo_odometer/odometry
odom0_config: [true, true, true,
               true, true, true,
               false, false, false,
               false, false, false,
               false, false, false]
odom0_differential: false
odom0_relative: true
odom0_queue_size: 5
odom0_pose_rejection_threshold: 2
odom0_twist_rejection_threshold: 0.2
odom0_nodelay: false

and I have the launch file to load thees parameters as following:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find ghattas_localization)/params/ekf.yaml" />
  </node>
</launch>

at the moment the Imu is giving correct values on the topic /mavros/imu/data and the camera is giving correct visual odometry on the topic /stereo_odometer/odometry

the problem arise when I fuse those two topic information into the robot localization package here is a sample of the visual odometry and the imu data I have after moving the sensors together along the three axis back and forward:

IMU

---
header: 
  seq: 73756
  stamp: 
    secs: 1554683660
    nsecs:  25350080
  frame_id: "base_link"
orientation: 
  x: 0.690330453259
  y: -0.259261357207
  z: 0.203541852432
  w: -0.644048234429
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.0266555175185
  y: 0.0747747048736
  z: 0.0135522447526
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.5491724
  y: -9.8458766
  z: -0.9610517
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---

visual odometry

header: 
  seq: 1165
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "/odom"
child_frame_id: "zed_camera"
pose: 
  pose: 
    position: 
      x: -0.0270988403684
      y: 0.533030086882
      z: -0.381215711715
    orientation: 
      x: 0.0170582143098
      y: 0.178282180763
      z: 0.0473209938349
      w: 0.982692833437
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09]
---

the following is the output of the robot localization package and I am sure that axes of the camera and the imu are coinciding. Behavior of the output is strange the position is increasing sharply, also, the covariance is increasing reapidly.

---
header: 
  seq: 1618
  stamp: 
    secs: 1554683665
    nsecs: 505090714
  frame_id: "odom_fil"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -49.0400755618
      y: -115.681036761
      z: -16.2848216466
    orientation: 
      x: -0.00597687112956
      y: 0.00591374066772
      z: 0.0124888709834
      w: 0.999886659969
  covariance: [3730.3548210431636, 1.982648486441013, 193.9042156808251, -0.011060840200357896, -0.06884340922126217, 0.1832600052648087, 1.9826484864409322, 3712.5015402080717, 107.95606684536298, 0.07315854224003833, -0.0036843377094652346, -0.09984730770281959, 193.9042156808262, 107.95606684536305, 5663.512354209365, -0.17764112580316865, 0.11132446668202538, 0.001844578302749848, -0.011060840200357878, 0.07315854224003825, -0.1776411258031688, 0.0340901115557153, 7.307165617284408e-07, -0.00022168978197552352, -0.06884340922126213, -0.0036843377094652355, 0.11132446668202534, 7.307165617284428e-07, 0.03410325857788445, -3.7461195496827425e-06, 0.1832600052648088, -0.09984730770281952, 0.0018445783027498494, -0.00022168978197552357, -3.7461195496827383e-06, 0.047868342403188956]
twist: 
  twist: 
    linear: 
      x: -2.68296351981
      y: -4.77196833436
      z: -2.35617804842
    angular: 
      x: 0.0118733492116
      y: -0.0326006884615
      z: 0.00175550418899
  covariance: [2.976515103999102, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.976515103999102, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.600056940944669, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0008000707639751834, 4.048423952775111e-14, -6.752710043556004e-12, 0.0, 0.0, 0.0, 4.048423952775111e-14, 0.0008000707639790855, -8.462308583754863e-14, 0.0, 0.0, 0.0, -6.752710043556001e-12, -8.462308583754944e-14, 0.0016000192065814241]
---

Please if anyone could help I would be thankful.

Asked by ShehabAldeen on 2019-04-07 19:42:03 UTC

Answer 1

Thank you for the clear and detailed question.

My first suggestion is that you need to get rid of the rejection_threshold parameters. Get everything working roughly how you want, and then introduce those parameters to handle outlier rejection, if that's necessary.

Now, I see the following issues:

You have the EKF configured so that its output frame (world_frame) is odom_fil. However, your visual odometry data is in the frame /odom (side note: get rid of the forward slash there; tf2 won't like it). But I don't see a transform specified anywhere from odom_fil->odom. The EKF won't be able to use your VO pose data without that transform.
You are fusing acceleration data from your IMU, but you have the IMU specified as providing data in the base_link frame, but clearly it is not mounted neutrally, or your Y linear acceleration wouldn't be -9.81. A neutrally-mounted IMU should have a Z acceleration of +9.81. So you either need to fix your IMU data, or provide a transform from base_link -> imu, and change the frame ID in your IMU data.
You don't have a velocity reference, but you are fusing acceleration data. That's going to make for some ugliness in your velocity output (especially if your IMU has any biases), and your state estimate will definitely suffer. Your VO velocity data is in the frame zed_camera, so if you provide the correct transform there, you can fuse the VO velocity data.

Asked by Tom Moore on 2019-05-15 04:03:52 UTC

problem configuring robot localization

Comments

Answers

Comments