EKF3: switching sources between ExtNav and OptFlow leads to position jump if GPS enabled

[rmackay9]

If GPS/Non-GPS transitions are setup using External Nav (e.g. Visual Odometry) and Optical flow then transitions from optical flow to External Nav may result in a large position jump to the GPS location even though the GPS is not defined in any of the source sets (e.g. the GPS is not defined in any EK3SRCx parameters)

Below is an image from SITL testing where GCS_SEND_TEXT() debug messages were added to NavEKF3_core::ResetPosition() to determine the position source used for the position reset.

The issue can be reproduced in SITL by doing the following the SITL instructions for Vicon and optical flow. To be more specific follow these instructions:

• start SITL
• param load ../Tools/autotest/default_params/copter-vicon.parm
• param fetch (and repeat above again)
• param load ../Tools/autotest/default_params/copter-optflow.parm
• param fetch (and repeat above again)
• param set SIM_VICON_YAW 90
• param set EK3_SRC1_POSXY 6
• param set EK3_SRC1_POSZ 6
• param set EK3_SRC1_VELXY 6
• param set EK3_SRC1_VELZ 6
• param set EK3_SRC1_YAW 6
• param set EK3_SRC2_POSXY 0
• param set EK3_SRC2_POSZ 1
• param set EK3_SRC2_VELXY 5
• param set EK3_SRC2_VELZ 0
• param set EK3_SRC2_YAW 0
• param set RC9_OPTION 90
• param set GPS1_TYPE 1
• exit SITL
• ../Tools/autotest/sim_vehicle.py --map --console -A "--serial5=sim:vicon:"

Fly the vehicle in Guided mode first using External nav

• rc 9 1000 (to use external nav)
• GUIDED
• arm throttle
• takeoff 15
• click on the map to the west of the vehicle
• rc 9 1500 (to use optical flow)
• click on the map North of the vehicle
• rc 9 1000 (to use external nav)

At this last command the vehicle will momentarily appear where the GPS reports the vehicle to be and then move violently back to where it was.

[peterbarker]

Could be related to this: https://github.com/ArduPilot/ardupilot/issues/16046