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 EK3_SRCx_ 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
• to the West of the vehicle, right-mouse-button-click on the map, select "Fly To Here", "15m"
• rc 9 1500 (to use optical flow)
• to the North of the vehicle, right-mouse-button-click on the map, select "Fly To Here", "15m"
• 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: #16046

[rmackay9]

This is largely resolved now. As discussed on the dev call there are potentially other places where the EKF resets to a position which is not using the active sensor. In particular if a lane switch happens while the EKF source is set to OpticalFlow (e.g EK3_SRCx_POSXY = 0, EK3_SRCx_VELXY = 5/OpticalFlow) it likely jumps to the GPS or external nav position instead of simply not resetting the position. This is just a guess though so I'll close this issue and hopefully do some more investigation in the future.

[sabrina2200]

Hi,

I’m currently seeing behaviour that looks very similar to what was described in #27729, and wanted to check if this is expected or if there is a recommended way to handle it.

In our setup we have both GPS and Visual Odometry available:

• GPS is coming from the autopilot as usual
• Visual odometry is generated on a companion computer and sent via MAVLink

What we observe is:

• During a GPS denial event, the system runs well using visual odometry (stable position, low variance)
• When GPS comes back online, the GPS position can be significantly wrong (we’ve seen jumps on the order of ~300 m)
• Despite this being clearly inconsistent with the current EKF state (which is stable from visodom + IMU), the EKF appears to accept or reset toward the GPS position
• This results in a large position jump and loss of stability / failsafe

Our expectation was that the EKF would reject this first GPS inputs after it comes back (based on innovation / consistency checks) and continue using the visual odometry until the GPS becomes healthy again.

At the moment we are sending visual odometry as a GPS-like input (fused with GPS), but we are considering switching to MAVLink ODOMETRY (ExternalNav). However, that change is non-trivial on our side, so before doing that we wanted to check:

• Is this behaviour expected when mixing GPS and ExternalNav sources?
• Is there a recommended configuration to prevent EKF resets to bad GPS when it comes back online?
• Are there parameters (e.g. innovation gates, source selection, lane switching behaviour) that can be tuned so that EKF continues to trust visodom over GPS in these cases?

I noticed #27729 mentions resets to GPS even when it is not part of the active source set, so I’m wondering if what we’re seeing could be related to similar reset / lane-switch logic.

Any guidance on best practice for handling GPS recovery after denial (especially when external nav is available and reliable) would be really helpful.

Thanks!