External nav+gps fix

[eufrizz]

Resolves #30076

Basically, the case where FusePosVelNED function is called for only a baro update, caused GPS data to be inadvertently fused with the external nav estimate. This created two conflicting sources of position, and an unstable filter with resetting behaviour as external nav and GPS diverged (e.g. if external nav is VIO).
This PR allows running/testing of external nav with GPS as a backup. Works cleanly in the case of:

EK3_SRC1_POSXY=6
EK3_SRC1_VELXY=6
EK3_SRC1_POSZ=1
EK3_SRC1_VELZ=0
EK3_SRC1_YAW=1

and

EK3_SRC1_POSXY=3
EK3_SRC1_VELXY=3
..(rest same)

Have not tested other permutations of sources too much. Runs with POSZ=3 but I don't have any data where baro and GPS alt are differentiated.

Restructuring the if statements to deal with GPS, external nav and other data separately, e.g.

if (extNavUsedForVel) {
   ...
} else {
   ...
}

becomes

if (extNavUsedForVel) {
   ...
} else if (gpsDataToFuse) {
   ...
}   
else {
  // TODO: deal with vel/pos sources that aren't external nav or GPS
  ...
}

As you can see, I'm not entirely sure what to add in the else case as I don't have experience with other sensor modalities - these are:
Position:

• Beacon
  Velocity:
• Beacon
• OpticalFlow
• WheelEncoder

So would appreciate some guidance/help there. We can just set them to be the same as the GPS case but it seems like they should be treated separately?

I also broke up

if (fuseVelData || fusePosData || fuseHgtData) {

into separate if statements for each of the three conditions, as the code is clearer that way.

The other important change was:

if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign))

to

if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || (gpsGoodToAlign && gpsDataToFuse))) {

To prevent this clause falsely triggering when using external nav. Similar things are done with gpsDataToFuse && frontend->_gpsGlitchRadiusMax <= 0 that are less critical but prevent the glitch radius parameter interfering with external nav operation.

The result is that external nav can be used as a primary source and GPS as a secondary, without any interference.

[rmackay9]

Hi @eufrizz,

thanks very much for this. It looks like this is failing some autotests for valid reasons. Perhaps the issue is it fails when some defines are changed.

error: 'extNavVelToFuse' was not declared in this scope

By the way, could you modify the commits so that they start with "AP_NavEKF3:"? We do this on all our commits to allow for easier understanding of what the commits affect and it helps with backporting. To be clear, I mean the commit titles, not the PR titles

[eufrizz]

Hi @eufrizz,

thanks very much for this. It looks like this is failing some autotests for valid reasons. Perhaps the issue is it fails when some defines are changed.

error: 'extNavVelToFuse' was not declared in this scope

By the way, could you modify the commits so that they start with "AP_NavEKF3:"? We do this on all our commits to allow for easier understanding of what the commits affect and it helps with backporting. To be clear, I mean the commit titles, not the PR titles

@rmackay9 Fixed!

Also took it for a test flight on an X500 quadcopter yesterday and it performed admirably, was able to switch between GPS and external nav no issues. Here is the log - it starts of with some low flying, switching between the external nav and GPS (shown below), then an auto mission where the external nav fails, innovation goes high, EKF failsafe occurs, and eventually we are able to switch it back to RTL using GPS.

Also, would it be possible to get this into the 4.6.0 release? Happy to join the Monday dev call or help out in some other way. Will hopefully be testing on a longer plane flight soon.

[rishabsingh3003]

Hi! Thanks for this PR. I’m still a bit unclear on this part:

"The case where FuseVelPosNED() is called for only a baro update caused GPS data to be inadvertently fused with the ExternalNav estimate."

I’ve been going over the logic and I don’t see how that would happen. From what I can tell, the actual fusion doesn’t occur until here:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L983

At that point, fusion is strictly guarded by flags like fusePosData and fuseVelData, which are reset each timestep and only set under specific source conditions. So unless I am overlooking something, I don’t see a clear path for GPS data to be fused while EXTNAV is selected.

That said, I do agree the surrounding code, particularly where the measurement variances are set up (which is the top part of the function that you are making changes to), can be tricky. I’ve fixed a few issues there myself recently. However, with the volume of changes in the PR, it’s difficult to distinguish formatting updates from actual logic fixes.

As a general suggestion: this part of the EKF is extremely sensitive and quite complex. A small change can cause extreme behaviour. I’d strongly recommend breaking code changes into smaller PRs, each focused on a specific bug fix. If you would like to do formatting changes, I would suggest opening a PR that ONLY does that with no logical changes. That would make the review process much more manageable. Also, for anything touching this fusion logic, a REPLAY log should be included so that the fix is backed by a clear before/after proof of behavior.

Thanks again!

[rishabsingh3003]

I think this is wrong because now the data won't pop from the buffer till the active source set is extnav. I think we should only be fusing newest data which woudn't happen in this case if the active source switches from another source.

[rishabsingh3003]

Same issue as above

[rishabsingh3003]

This should not be here

[rishabsingh3003]

This is empty, there are several other places that has something like this. In general we don't allow this throughout our code.

[rmackay9]

Hi @eufrizz,

We have a virtual beacon system in SITL which can be setup as described here.