Ask Your Question
0

Help initializing EKF to a set position

asked 2017-04-03 13:50:57 -0500

bluehash gravatar image

updated 2017-04-04 07:43:02 -0500

I had asked this question a while back, but received no response. Hopefully the revised subject line and info helps me come to a solution.

I'm running the latest robot_localization package on Kinetic/Ubuntu 16.

For testing certain functions of the vehicle, we would like to place the vehicle at a certain point in the map(which is already generated and published). To initialize the EKF to a location, I use the /set_pose rosservice call, which works IF odom0_differential=true. /set_pose does not work if odom0_differential=false. There is a tiny blip on the EKF output to the set location, but then the EKF starts at 0 again.

There are two sensors currently providing Pose data. The odometry(/odom) from wheel encoders and imu yaw(imu/data).

The EKF is setup as follows. Everything else is set to default.

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

odom0: /odom
imu0:  /imu/data

odom0_config: [true,  true,  false,          
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]

imu0_config:  [false, false,  false,
               false, false,  true,
               false, false,  false,
               false, false,  true,
               false, false,  false]

odom0_differential: false
imu0_differential:  false

odom0_relative: true
imu0_relative:  true

Rossservice call, initializing EKF to (1,1):

rosservice call --wait /set_pose "pose:
  header:
    seq: 0
    stamp:
      secs: 0
      nsecs: 0
    frame_id: odom
  pose:
    pose:
      position: {x: 1.0, y: 1.0, z: 0.0}
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
    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]"

Edit(April 4th, 2017, 8:15AM): I've stripped my vehicle bag file to a new bag file called ekfTest.bag, with just odom+imu+tf data - Download. This bag contains /tf, /odom and /imu/data topics. Please note that I've turned off "publish_tf" in the EKF parameters as the bag file already has this /tf data.

I've also attached my ekf parameter file(ekf_template.yaml) and launch file.

  1. Start the robot_localization node with the parameter file. 2.Open a window to view EKF data: rostopic echo /odometry/filtered/pose/pose/position 3.Run the bag file: rosbag play ekfTest.bag
  2. Set pose: rosservice call /set_pose "pose: header: seq: 0 stamp: now frame_id: odom pose: pose: position: {x: 1.0, y: 1.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ...
(more)
edit retag flag offensive close merge delete

Comments

Please post sample input messages from all sensors. Also, you are passing in an invalid quaternion (set w to 1.0).

Tom Moore gravatar imageTom Moore ( 2017-04-04 04:32:48 -0500 )edit

I've added data to the bottom of my pose. I've made w=1. I usually press tab after /set_pose and was not paying attention to orientation. The default is 0. This is a controlled test with sensor data(odom+imu) streaming into the EKF. I see the EKF latching to the /set_pose value and not change.

bluehash gravatar imagebluehash ( 2017-04-04 07:41:57 -0500 )edit

Would you mind accepting the answer (circular checkbox)? Thanks!

Tom Moore gravatar imageTom Moore ( 2017-05-03 02:14:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-04-11 04:17:01 -0500

Tom Moore gravatar image

updated 2017-04-11 04:17:59 -0500

Ah, sorry, didn't look at your config closely enough. You are fusing X and Y in your odom0 config, which is pose data, so the first measurement you get after you call set_pose will snap it right back. In other words, this is happening:

  • Time t0: filter gets measurement from odom0. X and Y position in the filter output will be very near the odom0 measurement.
  • Time t1: you call set_pose with some specific pose. EKF pose jumps to that position.
  • Time t2: filter gets measurement from odom0. X and Y position in the filter output will be very near the odom0 measurement.

This is why differential mode works: it converts the odom0 pose to a velocity, which won't interfere with your set_pose call. All set_pose is doing is sending your robot to the location you send it. If you then give it measurements that tell it that it is somewhere else, it's going to jump back.

edit flag offensive delete link more

Comments

Thanks Tom!

bluehash gravatar imagebluehash ( 2017-05-02 07:48:51 -0500 )edit

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: 2017-04-03 13:50:57 -0500

Seen: 659 times

Last updated: Apr 11 '17