Revision history [back]

The EKF follows a predict/update cycle. If no measurement is received within the sensor_timeout period, the filter will carry out a prediction step. If no sensor is available to correct it, the error will grow rapidly, and I wouldn't be surprised to see your pose drift. Is there a reason why your wheel odometry can't publish messages when standing still? I would make it publish messages with decreased covariance values, as you can be certain the robot isn't moving. Also, I'd switch your odometry source so that it is fusing velocity and not pose data.

Response to Update 2

If you just fuse velocities, there will always be unbounded error growth in your pose. You have nothing correcting the covariance for your pose variables. At every time step, we do a prediction, which causes the errors to grow for every variable. Then we immediately correct with a measurement, and any variable that is measured sees its covariance (usually) decrease in the state estimate. If your variable is never measured, it will never be corrected. If you watch the covariance on your twist data, you'll see that it's bounded.

I'm not sure I understand why adding pose input would result in drift. If anything, it should stick very close to your pose estimate, with some slight corrections as you move around.