Revision history [back]

Thanks for including your full configuration and sample messages. That's very helpful. Also, apologies for the delay in responding.

Anyway, I note in your EKF configuration that you (obviously) are not using two_d_mode, but yet you don't have a measurement/reference for Z. You are measuring every other pose variable in 3D, but unless you have a reference for at _least_ Z velocity, I would expect your EKF state estimate to be unstable, due to the fact that (a) Z isn't measured, and (b) Z is correlated with other state variables in the kinematic model.

If you have an altitude sensor, add that input to the EKF. If you don't, maybe consider faking a fixed altitude estimate?

Also, my experience has been that the IMU/GPS combo alone doesn't perform especially well. If you have any kind of velocity reference (camera-based optical flow data, for example), that would be helpful.