Revision history [back]

A couple things:

• Are the time stamps on your inputs correct? Is one very old?
• Even if the filter was respecting both, you are fusing two sources of absolute pose data. One is coming from odometry, so over time, they won't agree with each other, so the filter will start jumping back and forth between them.

You need to fuse X and Y position from your XY positioning data sensor, and X, Y, and yaw _velocity_ from your odometry. Even then, you'll eventually run into trouble, because your yaw will drift, and your true heading (as determined by using atan on your XY positioning data) won't match the robot's odometry heading. Can you compute a heading for the odom1 data source?