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.
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 682 in cf7b01d

   if (gpsSpdAccuracy > 0.0f) {

   
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 697 in cf7b01d

   if (gpsPosAccuracy > 0.0f) {

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.
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 717 in cf7b01d

   obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);

   
   Mentioned this here: e80fb8b#commitcomment-117061127

3. CalculateVelInnovationsAndVariances also doesn't use _gpsVertVelNoise & gpsDVelVarAccScale for vertical velocity
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 496 in cf7b01d

   CalculateVelInnovationsAndVariances(gpsDataDelayed.vel, frontend->_gpsHorizVelNoise, frontend->gpsNEVelVarAccScale, gpsVelInnov, gpsVelVarInnov);

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
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 687 in cf7b01d

   } else if (extNavVelToFuse) {

   
   

   ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

   Line 712 in cf7b01d

   if (extNavVelToFuse) {

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.

ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

Lines 690 to 694 in 429dbef

	} else {
	// calculate additional error in GPS velocity caused by manoeuvring
	R_OBS[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
	R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
	}

and,

ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

Lines 703 to 705 in 429dbef

	} else {
	R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
	}

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.

ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

Lines 956 to 961 in 429dbef

	if (obsIndex <= 2) {
	innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
	R_OBS[obsIndex] *= sq(gpsNoiseScaler);
	} else if (obsIndex == 3 || obsIndex == 4) {
	innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
	R_OBS[obsIndex] *= sq(gpsNoiseScaler);