1. You aren't fusing yaw or yaw velocity from your wheel odometry data. That's going to cause a covariance explosion in the output, which will have all kinds of ugly side effects. In general, every single pose or velocity variable needs at least one reference (measurement source). In the case of two_d_mode, you need a reference for X or X velocity, Y or Y velocity, and yaw or yaw velocity. You are missing the yaw data.