Revision history [back]

It looks like you're fusing linear acceleration from your IMU. That tends to be pretty noisy and generally has non-zero bias, unless you have previously taken care of those problems or have an IMU that handles that stuff for you (in which case, tell me which one and I'll order a few :-) ).

I'd recommend fusing in the IMU's gyro data and fusing only the wheel encoders for the linear segment. However if someone else has done something different, feel free to jump in.