ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

robot_localization odom->base_link generation from encoder, particle filter, imu

asked 2022-01-11 05:41:54 -0500

kekpirat gravatar image

updated 2022-01-25 01:04:38 -0500


I'm confused by how to use robot_localization to generate the odom->base_link transformation correctly.

My setup is a 4 wheeled robot, and I have 3D pose (with orientation) from my global localisation particle filter (PF), which generates the map->odom transformation, though with some delay and at a low rate, which is why I want to use robot_localization to generate a continuous/smooth pose using my wheel encoders + imu + maybe my PF pose using the "smooth_lagged_data" setting.

I've tried reading previous discussions and came up with the following robot_localization yaml config which can be found at the bottom of the post [I increased the process noise covariance of the velocity terms as I found my wheel encoder odoms quite accurate]

The issue I'm facing is that the odom->base_link transform that robot_localization is generating is now based off the initial location of the odom, but my odom frame is also moving because it is being updated by my PF. Hence, my map -> base_link frame is moving at twice the actual velocity, as both the map->odom (from PF) and odom->base_link (from robot_localization) transforms move. I would have thought that the odom->base_link transform ought to "reset" to 0 whenever the map->odom tf is updated (along with the pose covariances, which in my current case grow unbounded as I don't take the pose directly but the differential). What am I doing wrong here?

Thank you!


frequency: 50

silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false
permit_corrected_publication: false

smooth_lagged_data: true
# History length in seconds that must be kept to smooth lagged data
history_length: 3

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
## Generates the transform between odom_frame and base_link_frame in /tf
world_frame: odom           # Defaults to the value of odom_frame if unspecified

# Wheel encoder - only gives vx and yaw rate, but I notice vy/vz go crazy sometimes, and they're normally 0 anyway (unless theres a gentle slope/ramp, which I rely on the orientation from pose0 to correct)
odom0: odom
odom0_config: [false,  false,  false,
               false, false, false,
               true, true, true,
               true, true, true,
               false, false, false]

odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: true
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

# Particle filter pose
pose0: robot/pose
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: true
pose0_relative: true
pose0_queue_size: 5
pose0_rejection_threshold: 2  # Note the difference in parameter name
pose0_nodelay: false

imu0: imu
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: 20
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: true

use_control: false

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


Please do not link to external sites where the data expires. Instead, edit your description and copy/paste the text yaml file into it, then format the lines with the 101010 button. It's OK if it is long.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-01-11 08:22:02 -0500 )edit

Oops sorry, fixed!

kekpirat gravatar image kekpirat  ( 2022-01-11 10:16:27 -0500 )edit

What sensor(s) are feeding the global localization particle filter?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-01-11 16:06:26 -0500 )edit

Lidar + wheel encoders

kekpirat gravatar image kekpirat  ( 2022-01-11 19:31:07 -0500 )edit

I think I know what's happening, but please provide a sample message from every EKF input.

Tom Moore gravatar image Tom Moore  ( 2022-01-14 04:14:33 -0500 )edit

@Tom Moore I've added sample messages in the main question

kekpirat gravatar image kekpirat  ( 2022-01-25 01:05:34 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2022-01-25 04:24:30 -0500

Tom Moore gravatar image

I would have thought that the odom->base_link transform ought to "reset" to 0 whenever the map->odom tf is update

I'm not clear on what this would achieve.

The first thing I want to suggest is that you simplify your config. You have a whole bunch of advanced parameters in there that should only be added once you have your setup working as you want.

That aside, there are a bunch of issues with what you're trying to do. First, I'd recommend that you read REP-105 if you haven't yet. The thing to bear in mind is that tf does not allow a given frame to have two parents. Your particle filter is technically calculating the map->base_link transform, and your EKF instance is calculating odom->base_link. But tf doesn't allow that, so your particle filter is (or should be) taking in the odom->base_link transform and using that to generate a map->odom transform. This provides a consistent tf tree.

In a normal "two tier" setup like this, you would have your particle filter (or a second EKF instance) publish the pose data and its transform, and then the EKF would just fuse wheel odometry and IMU data (continuous input sources only).

What I think you're trying to do is "fill in the gaps" between your particle filter updates. If so, this is the wrong way to go about it. You are fusing your map frame pose into the odom frame EKF, which is then using that information to generate the odom->base_link transform, which the particle filter is then using to generate the map->odom transform again. Without going through the math, I think you're creating a kind of positive feedback loop that will cause the state estimate to change more rapidly than it is.

If you are just trying to create a smooth state estimate, you have three options:

  1. Change the particle filter so it can do predictions and provide higher-rate updates to the map->odom transfrom, and then stop fusing the particle filter pose in the EKF.
  2. Nodes like move_base will require you to have both a map and an odom frame for global and local planning, respectively, but if you aren't using move_base or just generally don't require both frames, then change the output frame of the particle filter to odom, and turn off the transform publication for it. Then the EKF will use the particle filter data (again, whose frame_id should now be odom) whenever it's available, and fill in the gaps with fused odom/IMU data.
  3. Similar to (2), you can introduce another EKF. You need to once again tell your particle filter to not publish any transforms, but you can make your map frame EKF fuse the particle filter, IMU, and wheel encoder data, and then make the odom frame EKF only fuse the wheel encoder and IMU data.

(3) is a common practice for mobile ... (more)

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2022-01-11 05:41:54 -0500

Seen: 376 times

Last updated: Jan 25 '22