AP_NavEKF2: Allow tuning of non-GPS mode

Eliminate the use of horizontal position states during non-aiding operation to make it easier to tune.
Explicitly set the horizontal position associated Kalman gains to zero and the coresponding covariance entries to zero after avery fusion operation.
Make the horizontal velocity observation noise used during non-aiding operation adjustable.
Use a fixed value of velocity noise during initial alignment so that the flight peformance can be tuned without affecting the initial alignment.

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -419,6 +419,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Units: %
     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
+    // @User: Advanced
+    // @Units: m/s
+    AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizVelNoise, 10.0f),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -311,6 +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
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -385,6 +385,12 @@ 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;
 
@@ -804,6 +810,12 @@ 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

@@ -173,16 +173,13 @@ void NavEKF2_core::SelectVelPosFusion()
     // Do this to coincide with the height fusion
     if (fuseHgtData && PV_AidingMode == AID_NONE) {
         gpsDataDelayed.vel.zero();
-        gpsDataDelayed.pos.x = lastKnownPositionNE.x;
-        gpsDataDelayed.pos.y = lastKnownPositionNE.y;
         // 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) {
-            fusePosData = true;
             fuseVelData = true;
         } else {
-            fusePosData = false;
             fuseVelData = false;
         }
+        fusePosData = false;
     }
 
     // perform fusion
@@ -245,20 +242,18 @@ void NavEKF2_core::FuseVelPosNED()
         posErr = frontend->gpsPosVarAccScale * accNavMag;
 
         // estimate the GPS Velocity, GPS horiz position and height measurement variances.
-        // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
+        // Use different errors if operating without external aiding using an assumed velocity of zero
-        // otherwise we scale it using manoeuvre acceleration
+        if (PV_AidingMode == AID_NONE) {
-        // Use different errors if flying without GPS using synthetic position and velocity data
+            if (tiltAlignComplete && motorsArmed) {
-        if (PV_AidingMode == AID_NONE && motorsArmed) {
+            // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
-            // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
+                R_OBS[0] = sq(constrain_float(frontend->_noaidHorizVelNoise, 0.5f, 25.0f));
-            // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
+            } else {
-            R_OBS[0] = sq(10.0f);
+                // 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];
-            // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
+            for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
-            R_OBS[3] = sq(25.0f);
-            R_OBS[4] = R_OBS[3];
-            R_OBS[5] = posDownObsNoise;
-            for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
         } else {
             if (gpsSpdAccuracy > 0.0f) {
                 // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
@@ -270,16 +265,15 @@ void NavEKF2_core::FuseVelPosNED()
                 R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise,  0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale  * accNavMag);
             }
             R_OBS[1] = R_OBS[0];
-            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 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
             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);
-            for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
         }
-
+        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];
 
         // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
         // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
@@ -310,30 +304,27 @@ void NavEKF2_core::FuseVelPosNED()
             posHealth = ((posTestRatio < 1.0f) || badIMUdata);
             // 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, timed out, or in constant position mode
+            // use position data if healthy or timed out
-            if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) {
+            if (posHealth || posTimeout) {
                 posHealth = true;
-                // only reset the failed time and do glitch timeout checks if we are doing full aiding
+                lastPosPassTime_ms = imuSampleTime_ms;
-                if (PV_AidingMode == AID_ABSOLUTE) {
+                // if timed out or outside the specified uncertainty radius, reset to the GPS
-                    lastPosPassTime_ms = imuSampleTime_ms;
+                if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
-                    // if timed out or outside the specified uncertainty radius, reset to the GPS
+                    // reset the position to the current GPS position
-                    if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
+                    ResetPosition();
-                        // reset the position to the current GPS position
+                    // reset the velocity to the GPS velocity
-                        ResetPosition();
+                    ResetVelocity();
-                        // reset the velocity to the GPS velocity
+                    // don't fuse GPS data on this time step
-                        ResetVelocity();
+                    fusePosData = false;
-                        // don't fuse GPS data on this time step
+                    fuseVelData = false;
-                        fusePosData = false;
+                    // Reset the position variances and corresponding covariances to a value that will pass the checks
-                        fuseVelData = false;
+                    zeroRows(P,6,7);
-                        // Reset the position variances and corresponding covariances to a value that will pass the checks
+                    zeroCols(P,6,7);
-                        zeroRows(P,6,7);
+                    P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
-                        zeroCols(P,6,7);
+                    P[7][7] = P[6][6];
-                        P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
+                    // Reset the normalised innovation to avoid failing the bad fusion tests
-                        P[7][7] = P[6][6];
+                    posTestRatio = 0.0f;
-                        // Reset the normalised innovation to avoid failing the bad fusion tests
+                    velTestRatio = 0.0f;
-                        posTestRatio = 0.0f;
-                        velTestRatio = 0.0f;
-                    }
                 }
             } else {
                 posHealth = false;
@@ -498,6 +489,12 @@ 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,7 +513,12 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
     stateStruct.velocity += delVelNav;
 
     // apply a trapezoidal integration to velocities to calculate position
-    stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
+    // 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);
+    }
 
     // accumulate the bias delta angle and time since last reset by an OF measurement arrival
     delAngBodyOF += imuDataDelayed.delAng - stateStruct.gyro_bias;
@@ -1237,7 +1242,16 @@ 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
-    for (uint8_t i=6; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions
+    // 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);
+    }
+    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
     P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias