Z axis position estimate spike when flying with visual odometry


We’re testing our custom visual odometry implementation, which sends the aircraft position and velocity estimates using the ODOMETRY message. By design the device is quite reliable in the X and Y axes but the Z axis position is quite drifty.

To compensate for this we’ve fitted our aircraft with a TFMini rangefinder, and accordingly set EKF2_HGT_MODE to use the rangefinder as the primary altitude source. The barometer is disabled as it’s an indoor aircraft, and EKF2_AID_MASK is set to use vision position/yaw/velocity fusion.

We have been looking at the logs, and if we attempt to take off in Altitude Hold, the local Z position usually exhibits a large spike, and sometimes leads to dangerous rapid climb behaviour - please see the attached figure:

We suspect that this is due to bad initialization of EKF covariances - would appreciate the community’s thoughts on this. Here is the flight log: https://review.px4.io/plot_app?log=2fe38421-fa5c-40c7-8e85-ecc2e5ca8aa1