Ask Your Question
0

How to fuse odometry and AR slam using robot_localization?

asked 2019-03-24 10:38:48 -0600

rohbotics gravatar image

updated 2019-03-26 00:36:20 -0600

I am trying to use robot_localization to fuse wheel odometry and fiducial_slam and I am a little confused by the transforms.

In the current system, we have the wheel odometry publishing odom->base_link, and fiducials publishing map->odom.

When I run the EKF in this system (with publish_tf: false) and look at the /odom/filtered topic in rviz, it looks great, the kalman filter pose seems to track the robot pose in the real world very well.

The problem comes when I turn off the map->odom publishing from fiducials, and turn on publish_tf for robot_localization. This causes the position of the robot to fly off wildly every time we move.

I think this is because the wheel odometry data is published relative to the /odom frame, but there is no way to transform this into the /map frame without the kalman filter using it's own output, and this cyclical flow is causing wild instability.

How should I go about trying to solve this issue? How do you fuse map relative and odom relative pose measurements.

Edit: localization config

frequency: 10
two_d_mode: true

map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: map

publish_tf: true

#  x, y, z,
#  roll, pitch, yaw,
#  vx, vy, vz,
#  vroll, vpitch, vyaw,
#  ax, ay, az

odom0: /odom
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_relative: true

#imu0: /imu
#imu0_config: [false, false, false,
#              false, false, true,
#              false, false, false,
#              false, false, true,
#              false, false, false]
#imu0_differential: true
#imu0_relative: true

pose0: /fiducial_pose
pose0_config: [true,   true,   false,
               false,  false,  true,
               false,  false,  false,
               false,  false,  false,
               false,  false,  false]

pose0_rejection_threshold: 3

#  x,    y,    z,    roll, pitch, yaw, vx,  vy,    vz,  vroll, vpitch, vyaw, ax,   ay,  az
process_noise_covariance: [0.05, 0,    0,    0,    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,
                           0,    0,    0.00, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.00, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.00, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.01, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.000, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.000, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.00, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.00, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.00, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.01, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.00, 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 03:43:19 -0600

Tom Moore gravatar image

Please include sample messages for all sensor inputs, as well as the config for your _odom_ frame EKF. Without those, I can't comment fully.

Without that information, I have other questions and comments.

  • When you turn on publish_tf for the second EKF instance, are you making sure to disable tf publication in whatever node is doing the fiducials? You can't have two nodes trying to publish the same transform.
  • You are correct in your statement: first, you are fusing two absolute source of pose data, which is not advised, unless both are in agreement (think of, e.g., two GPS sensors), and moreover, you are using the EKF's own transform to transform data that it is trying to fuse. That's not really a huge deal, since the EKF uses its own state internally to do some transforms, but it will make your life more complicated. Any reason not to just fuse the velocity data from the wheel odometry? That's a common practice.
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

3 followers

Stats

Asked: 2019-03-24 10:38:48 -0600

Seen: 90 times

Last updated: May 15