Ask Your Question
0

problem configuring robot localization

asked 2019-04-07 19:42:03 -0500

ShehabAldeen gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-05-15 04:03:52 -0500

Tom Moore gravatar image

updated 2019-05-15 04:05:15 -0500

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

2 followers

Stats

Asked: 2019-04-07 19:42:03 -0500

Seen: 118 times

Last updated: May 15