External navigation measurement noise is overwritten by GPS reported accuracy in EKF

[pavloblindnology]

1. If external navigation is used for position and/or speed together with GPS, then external navigation measurement errors are overwritten by GPS reported errors. Is it ok? AFAIU, the external navigation if-s should come 1st. https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L682 https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L697

2. Also, when compiting GPS velocity variances used for data checks, vertical velocity is treated identically to horizontal velocity. I.e., _gpsHorizVelNoise & gpsNEVelVarAccScale are used instead of _gpsVertVelNoise & gpsDVelVarAccScale for vertical velocity, as it's done for variances used for fusion. Also, description mentions only horizontal velocity. https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L717 Mentioned this here: https://github.com/ArduPilot/ardupilot/commit/e80fb8b3fa301fb3857d3306099da7a763be99ee#commitcomment-117061127

3. CalculateVelInnovationsAndVariances also doesn't use _gpsVertVelNoise & gpsDVelVarAccScale for vertical velocity https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L496

4. If there are ext.nav. messages to fuse but they are not configured to be used as velocity source, variances for ext.nav. are still used, though GPS velocities are actually fused https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L687 https://github.com/ArduPilot/ardupilot/blob/cf7b01d73a08aa78899c2031371cc8bb467e5e3f/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L712

The issues concern both EKF2 & 3.

[rmackay9]

FYI @priseborough I think this probably is a real bug.

[SrijaneeBiswas]

Hi @rmackay9,

Thanks for taking a look into this.

As discussed on call on 22/9/23 with you and @shubham-shahh, here are our additional observations pertinent to the same topic:

1. the else case mentioned in Point 1 above may need to be refactored as well. That is, if the source is neither GPS nor EXTNAV, the R_OBS values for both position and velocity are still populated using GPS Vel Noise and Gps Vel Var Acc Scale.

https://github.com/ArduPilot/ardupilot/blob/429dbefadee9a1bb3f6705c7ea4a594343a5f3c6/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L690-L694

and,

https://github.com/ArduPilot/ardupilot/blob/429dbefadee9a1bb3f6705c7ea4a594343a5f3c6/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L703-L705

2. When we sequentially fuse the position and velocity data, we multiply a sq(gpsNoiseScaler) irrespective of the current active source and while this may be correct if we were only using EXTNAV or only using OPT_FLOW (completely GPS denied) where (gpsNoiseScaler=1.0f), it can potentially introduce problems in cases where we switch EK3 source from, say, GPS to EXTNAV since the gpsNoiseScaler is not reset to 1.

https://github.com/ArduPilot/ardupilot/blob/429dbefadee9a1bb3f6705c7ea4a594343a5f3c6/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L956-L961