AP_NavEKF2: pos not reset to ext nav init pos

[chobitsfan]

When use motion capture system for indoor flight navigation (ATT_POS_MOCAP or VISION_POSITION_ESTIMATE). If vehicle is not boot near motion capture system origin. (for example, vehicle is boot in a position 1.2m away from mocap origin), EKF2 will not reset to ext nav initial pos.

9 1980-1-1 08-00-00.zip (EKF PN graph left, VISP PX graph right) Drone is placed 1.2m away from ext nav origin and boot. After drone start receive ext nav data, EKF2 seems fuse these data. Because EKF2 do not reset to initial ext nav pos in ResetPosition(), there is diff between EKF position and VISP. It makes drone keep reporting "prearm: need 3d fix" and unable to arm

8 1980-1-1 08-00-00.zip (EKF PN graph left, VISP PX graph right) If drone is placed closer to ext nav origin and boot. EKF will reset to initial ext nav pos and everything is working well then

[chobitsfan]

The real cause is EKF2 pos did not reset to ext nav init pos because extNavDataDelayed is too old to pass this check https://github.com/ArduPilot/ardupilot/blob/1db0feea59bfc7b531fff957d7eed4ff00b1dfb7/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp#L127 This problem is fixed in EKF3