AP_NavEKF2: Improvements to non-GPS performance

Change to user adjustable fusion of constant position (as per legacy EKF) instead of constant velocity.
Enable user to specify use of 3-axis magnetometer fusion when operating without aiding.
Don't allow gyro scale factor learning without external aiding data as it can be unreliable

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -420,12 +420,12 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
 
     // @Param: NOAID_NOISE
-    // @DisplayName: Non-GPS operation velocity change (m/s)
-    // @Description: This sets the amount of velocity change that the EKF allows for when operating without external measurements (eg GPs or optical flow). Increasing this parameter makes the EKF attitude estimate less affected by changes in vehicle velocity but also makes the EKF attitude estimate more affected by IMU errors.
-    // @Range: 0.5 25.0
+    // @DisplayName: Non-GPS operation position uncertainty (m)
+    // @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
+    // @Range: 0.5 50.0
     // @User: Advanced
     // @Units: m/s
-    AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizVelNoise, 10.0f),
+    AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
 
     AP_GROUPEND
 };

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -311,7 +311,7 @@ private:
     AP_Int8 _gpsCheck;              // Bitmask controlling which preflight GPS checks are bypassed
     AP_Int8 _imuMask;               // Bitmask of IMUs to instantiate EKF2 for
     AP_Int16 _gpsCheckScaler;       // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
-    AP_Float _noaidHorizVelNoise;   // horizontal velocity measurement noise assuned when synthesised zero velocity measurements are used to constrain attitude drift : m/s
+    AP_Float _noaidHorizNoise;      // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -64,8 +64,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
 
     // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
     // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
-    // If we do nto have absolute position (eg GPS) then the earth field states cannot be learned
-    bool magCalDenied = !use_compass() || (frontend->_magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend->_magCal != 4);
+    bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
 
     // Inhibit the magnetic field calibration if not requested or denied
     inhibitMagStates = (!magCalRequested || magCalDenied);

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -385,12 +385,6 @@ void NavEKF2_core::FuseMagnetometer()
             }
         }
 
-        // inhibit position state modification if we are not aiding
-        if (PV_AidingMode == AID_NONE) {
-            Kfusion[6] = 0.0f;
-            Kfusion[7] = 0.0f;
-        }
-
         // reset the observation index to 0 (we start by fusing the X measurement)
         obsIndex = 0;
 
@@ -810,12 +804,6 @@ void NavEKF2_core::FuseDeclination()
         Kfusion[i] = t14*(P[i][17]*t2*t6-P[i][16]*magE*t6*t7);
     }
 
-    // inhibit position state modification if we are not aiding
-    if (PV_AidingMode == AID_NONE) {
-        Kfusion[6] = 0.0f;
-        Kfusion[7] = 0.0f;
-    }
-
     // get the magnetic declination
     float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -168,18 +168,21 @@ void NavEKF2_core::SelectVelPosFusion()
     // Select height data to be fused from the available baro, range finder and GPS sources
     selectHeightForFusion();
 
-    // If we are operating without any aiding, fuse in the last known position and zero velocity
+    // If we are operating without any aiding, fuse in the last known position
     // to constrain tilt drift. This assumes a non-manoeuvring vehicle
     // Do this to coincide with the height fusion
     if (fuseHgtData && PV_AidingMode == AID_NONE) {
         gpsDataDelayed.vel.zero();
-        // only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration
-        if (accNavMag < 9.8f) {
-            fuseVelData = true;
+        gpsDataDelayed.pos.x = lastKnownPositionNE.x;
+        gpsDataDelayed.pos.y = lastKnownPositionNE.y;
+        // when in flight only fuse synthetic measurements when rate of change of velocity is less than 1.1g
+        // to reduce attitude errors due to launch acceleration
+        if (accNavMag < 1.1f * GRAVITY_MSS || motorsArmed) {
+            fusePosData = true;
         } else {
-            fuseVelData = false;
+            fusePosData = false;
         }
-        fusePosData = false;
+        fuseVelData = false;
     }
 
     // perform fusion
@@ -242,17 +245,19 @@ void NavEKF2_core::FuseVelPosNED()
         posErr = frontend->gpsPosVarAccScale * accNavMag;
 
         // estimate the GPS Velocity, GPS horiz position and height measurement variances.
-        // Use different errors if operating without external aiding using an assumed velocity of zero
+        // Use different errors if operating without external aiding using an assumed position or velocity of zero
         if (PV_AidingMode == AID_NONE) {
             if (tiltAlignComplete && motorsArmed) {
             // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
-                R_OBS[0] = sq(constrain_float(frontend->_noaidHorizVelNoise, 0.5f, 25.0f));
+                R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
             } else {
                 // Use a smaller value to give faster initial alignment
                 R_OBS[0] = sq(0.5f);
             }
             R_OBS[1] = R_OBS[0];
             R_OBS[2] = R_OBS[0];
+            R_OBS[3] = R_OBS[0];
+            R_OBS[4] = R_OBS[0];
             for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
         } else {
             if (gpsSpdAccuracy > 0.0f) {
@@ -268,10 +273,10 @@ void NavEKF2_core::FuseVelPosNED()
             // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
             // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
             // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
+            R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
+            R_OBS[4] = R_OBS[3];
             for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
         }
-        R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
-        R_OBS[4] = R_OBS[3];
         R_OBS[5] = posDownObsNoise;
         for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
 
@@ -305,7 +310,10 @@ void NavEKF2_core::FuseVelPosNED()
             // declare a timeout condition if we have been too long without data or not aiding
             posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
             // use position data if healthy or timed out
-            if (posHealth || posTimeout) {
+            if (PV_AidingMode == AID_NONE) {
+                posHealth = true;
+                lastPosPassTime_ms = imuSampleTime_ms;
+            } else if (posHealth || posTimeout) {
                 posHealth = true;
                 lastPosPassTime_ms = imuSampleTime_ms;
                 // if timed out or outside the specified uncertainty radius, reset to the GPS
@@ -489,12 +497,6 @@ void NavEKF2_core::FuseVelPosNED()
                     Kfusion[23] = 0.0f;
                 }
 
-                // inhibit position state modification if we are not aiding
-                if (PV_AidingMode == AID_NONE) {
-                    Kfusion[6] = 0.0f;
-                    Kfusion[7] = 0.0f;
-                }
-
                 // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
                 stateStruct.angErr.zero();
 

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -513,12 +513,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
     stateStruct.velocity += delVelNav;
 
     // apply a trapezoidal integration to velocities to calculate position
-    // if we are not doing aiding, then the horizontal position states are not predicted
-    if (PV_AidingMode != AID_NONE) {
-        stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
-    } else {
-        stateStruct.position.z += (stateStruct.velocity.z + lastVelocity.z) * (imuDataDelayed.delVelDT*0.5f);
-    }
+    stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
 
     // accumulate the bias delta angle and time since last reset by an OF measurement arrival
     delAngBodyOF += imuDataDelayed.delAng - stateStruct.gyro_bias;
@@ -677,7 +672,6 @@ void NavEKF2_core::CovariancePrediction()
     for (uint8_t i= 0; i<=8;  i++) processNoise[i] = 0.0f;
     for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
     for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
-    processNoise[15] = dVelBiasSigma;
     if (expectGndEffectTakeoff) {
         processNoise[15] = 0.0f;
     } else {
@@ -1230,18 +1224,17 @@ void NavEKF2_core::ConstrainVariances()
 {
     for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
     for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
-    // If we are not using the horizontal position states, keep their covariances at the initialisation values
-    if (PV_AidingMode == AID_NONE) {
-        zeroRows(P,6,7);
-        zeroCols(P,6,7);
-        P[6][6]   = sq(frontend->_gpsHorizPosNoise);
-        P[7][7]   = P[6][6];
-    } else {
-        for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
-    }
+    for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
     P[8][8] = constrain_float(P[8][8],0.0f,1.0e6f); // vertical position
     for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases
-    for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
+    if (PV_AidingMode != AID_NONE) {
+        for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
+    } else {
+        // we can't reliably estimate scale factors when there is no aiding data due to transient manoeuvre induced innovations
+        // so inhibit estimation by keeping covariance elements at zero
+        zeroRows(P,12,14);
+        zeroCols(P,12,14);
+    }
     P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias
     for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
     for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field