Help initializing EKF to a set position

edit

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