Ask Your Question
0

robot_localization - using odometry and gps - get weird data

asked 2019-05-29 08:05:01 -0500

Bob112358 gravatar image

updated 2019-05-31 10:39:29 -0500

Hello there,

I'm having trouble with the robot_localization package. At first I had odometry, IMU and GPS, but the IMU data got disturbed by a magnetic field of our robot. So I have to use only odometry and GPS for this approach since I can not make any new measurements and have to evaluate what I can from this data. So, I want to fuse odometry and GPS. I am using the Dual-EKF-Node example. I will give all information needed here: As there is no usable IMU-Data, I have to use the wait-for-datum-mode and give a datum in the config-file.

Launch-File:

<launch>
  <rosparam command="load" file="$(find bike_localization)/config/odom_gps.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_state_estimate_odom" clear_params="true"/>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_state_estimate_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
    <remap from="gps/fix" to="gps_rep"/>
  </node>

</launch>

Config: odom_gps.yaml

ekf_state_estimate_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: /odom_can
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

ekf_state_estimate_map:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: true

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

  odom0: /odom_can
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: true

  odom1: odometry/gps
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false , false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: false
  odom1_differential: false
  odom1_relative: false

  use_control: false

navsat_transform:
  frequency: 30
  delay: 2.0
  magnetic_declination_radians: 3.55641
  #yaw_offset: 1.570796327
  zero_altitude: true
  broadcast_utm_transform: false
  publish_filtered_gps: true
  use_odometry_yaw: false       # I tried true, too
  wait_for_datum: true
  datum: [XXXXX, YYYYY, -1.5707, map, base_link]    # we start facing south, 0 is facing east, so -pi/2 is facing south, tried other values, get the same output 
# For the actual run, real gps data is used

Now, this ist, what one of my odometry-messages look like. The odometry is facing X:

header: 
  seq: 113
  stamp: 
    secs: 1559130292
    nsecs:  62068063
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.281387492407
      y: 0.0023472056171
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0118238048699
      w: 0.999930096376
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.440455263089
      y: 0.00978800793434
      z: 0.0
    angular: 
      x ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
2

answered 2019-05-30 22:07:18 -0500

kkrasnosky gravatar image

It looks like your covariance matrices are all zeros. Robot localization requires that all the data you feed into it has reasonable covariance values.

From the robot localization docs common errors section:

Missing covariances. If you have configured a given sensor to fuse a given variable into the state estimation node, then the variance for that value (i.e., the covariance matrix value at position (i,i) ( i , i ) , where i i is the index of that variable) should not be 0 0 . If a 0 0 variance value is encountered for a variable that is being fused, the state estimation nodes will add a small epsilon value (1e−6 1 e − 6 ) to that value. A better solution is for users to set covariances appropriately.

I would recommend you estimate your covariances and tack them on when you generate your message (or make a republisher that does it).

Hope that helps!

edit flag offensive delete link more

Comments

Thanks for your advice. I changed some values in the covariance matrix, just to see, if it has any influence. As I only fuse x and y velocities, I changed them to various values. See my edit 1 in the question. But that does not seem to change the overall picture

Bob112358 gravatar imageBob112358 ( 2019-05-31 02:54:06 -0500 )edit

Does your GPS have covariance too?

kkrasnosky gravatar imagekkrasnosky ( 2019-05-31 10:22:13 -0500 )edit

Indeed, it has. I added an example message in EDIT2

Bob112358 gravatar imageBob112358 ( 2019-05-31 10:40:00 -0500 )edit
2

answered 2019-06-03 04:48:30 -0500

Tom Moore gravatar image

Offhand, I note three things:

  1. You aren't fusing yaw or yaw velocity from your wheel odometry data. That's going to cause a covariance explosion in the output, which will have all kinds of ugly side effects. In general, every single pose or velocity variable needs at least one reference (measurement source). In the case of two_d_mode, you need a reference for X or X velocity, Y or Y velocity, and yaw or yaw velocity. You are missing the yaw data.
  2. I'm not sure why you're even using an EKF for the odom frame. You only have one input. If your IMU has gyros, I'd bring that back in. Only magnetometers are sensitive to magnetic fields. If you are sticking with just wheel odometry, then I suggest making your wheel odometry node publish the odom->base_link transform, and getting rid of your odom frame EKF.
  3. Even with sensitivity to magnetic distortion in general, if you at least _start_ your robot in a location that doesn't have magnetic fields, you ought to be fine with using your IMU.
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

4 followers

Stats

Asked: 2019-05-29 08:05:01 -0500

Seen: 181 times

Last updated: Jun 03