Localizing using AR tags and wheel odometry

edit

Hello everyone,

My team and I are working on a project where we need to localize our robots using AR tags (ar_track_alvar). Our robots will not always see tags, so we are using wheel odometry to track motion during these periods. We are trying to accomplish this through sensor fusion using the robot_pose_ekf or robot_localization package. We have tried both, but neither have given us the desired effect.

We have multiple tags in our environment which all have static transforms with respect to the map frame. The robot looks for tags and finds a transform from the tag to the robot. By extension, this yields a transform from the map to the robot. This map->robot transform is converted to an odometry message using only pose measurements. The covariance for the pose is set to:

|0.1 100 100 100 100 100 |
|100 0.1 100 100 100 100 |
|100 100 100 100 100 100 |
|100 100 100 100 100 100 |
|100 100 100 100 100 100 |
|100 100 100 100 100 0.1 |

The twist covariances are all set to 10000. The frame_id for this tag based odom message is map.

The wheel odometry is very standard, it is based off of the turtlebot odometry. One note is that when the first tag estimate is received, a transform is broadcasted which places the wheel odom frame at the initial location of the robot with respect to the map.

Using both of the aforementioned packages and setups, and keeping the robot motionless, it is observed that the filter estimates become increasingly erratic and tend to infinity in no particular direction. Removing the wheel odometry and keeping only the ar tag odometry yields the same effect. The ar tag odometry messages remain completely constant within three significant figures.

Thus we conclude that somehow our consistent tag odometry measurements are causing the kalman filters to behave erratically and tend to infinity. This happens even with the wheel odometry measurements being fused as well. Can anyone explain why this might be, and offer any suggestions to fix this? I would be happy to provide any extra information which I haven't provided.

Side note: As a naive test, we also set all of the covariances to large values and observed that no matter how we moved the robot the differences in the filter outputs were tiny (+/- 0.01), more likely drift than an actual reading.

EDIT:

Thank you for your advice Tom. We have already combed through all of the wiki, and we did see the part about inflating covariances being unhelpful. We were confused though, because we didn't know what to set them to if not an arbitrarily large number. We did use the config to turn them off. You can see this in the posted config file.

We do not have an IMU. We are using a very ragtag group of robots (its what our school had lying around) and we are lucky we even have wheel odometry!

Our AR ...