Revision history [back]

none of your sensor data has any covariance values filled out

Each sensor now provides variance values on the diagonal of its covariance matrix. The off-diagonal entries are zero. This had no noticeable effect.

you don't have anything measuring Z or Z velocity, which is a big problem.

The wheel encoder odometry now provides the z velocity, which is always zero. This had no noticeable effect.

if it still misbehaves, try feeding some absolute orientation data to your odom instance of ekf_localization_node

This resulted in the same translation discontinuity that occurred when the absolute orientation data was provided by the localization instance of ekf_localization_node.

I fixed the problem by replacing the localization ekf_localization_node instance with a nodelet that subscribes to odometry/filtered and the pose provided by the solar compass. The nodelet accumulates the twist data from odometry/filtered, replaces the orientation component of the estimated pose when a pose message arrives, and publishes the map-to-odom transform.