EKF3 altitude divergence when using External Navigation after setting EKF origin

ArduCopter Copter 4.5
, , , ,
1

We’re encountering an issue with EKF3 (Copter 4.5.7) when using external navigation to fly without GPS. Specifically, the Z-position estimate becomes inconsistent after setting the EKF origin.

newplot (71)
newplot (71)1536×919 54 KB

You can see this in the attached plot:

  • At t=40 (ekf_yaw_reset) we start publishing VISION_POSITION_ESTIMATE
  • A few seconds later, we send SET_GPS_GLOBAL_ORIGIN to set the EKF origin. From that point onward, the EKF3’s Z estimate (XKF1[0].PD) diverges and stabilizes around 2× the value of VISP.PZ.

We did not experience this issue in previous versions of ArduPilot using EK2
Could this be a configuration issue, or is it potentially a bug?

Parameters:

AHRS_EKF_TYPE,3
GPS_TYPE,0

EK3_SRC1_POSXY, 6
EK3_SRC1_POSZ, 6
EK3_SRC1_VELXY, 0
EK3_SRC1_VELZ, 0
EK3_SRC1_YAW, 6

COMPASS_USE,0
COMPASS_USE2,0
COMPASS_USE3,0

VISO_TYPE,1