Revision history [back]

Looking at your configuration, the problem is that you have no input source measuring Z position. In addition, you are fuzing Z acceleration from the IMU. The double-integration required to turn linear acceleration into position leads to large amounts of drift. One of the many things on my list is to incorporate IMU bias calculations in the filter, but as it stands, we don't have that feature. Even if we did, using acceleration alone for a given pose variable is probably not going to ever work well. Telling navsat_transform_node to zero its altitude measurement just changes the output of navsat_transform_node; it doesn't affect the EKF at all. You still have to fuse that Z measurement in the EKF if you want to "clamp" Z to 0. You can also use two_d_mode, at least until I get my act together and put out a 2D version of the filter.

In any case, either fuse the 0 Z measurement from any of your input sources, or turn on two_d_mode. If your robot operates outside and you want your pose to be in 3D, go with the former option. If you want to assume a planar environment, go with the latter.