AP_NavEKF: Filter accuracy and stability improvements

Improvements in PX4 firmware have reduced the computational load  and mkae the previous practicwe of splitting magnetometer and optical flow fusion across multiple time steps unnecessary and make it possible to perform a covariance prediction prior to fusing data on the same time step. This patch:

1) Ensures that a covariance prediction is always performed prior to fusion of any observation
2) Removes the splitting of magnetometer fusion so that fusion of the X,Y and Z components occurs on the same time time step
3) Removes the splitting of optical flow fusion so that fusion of X and Y components occurs on the same time step

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -725,10 +725,6 @@ void NavEKF::UpdateFilter()
     // or the time limit will be exceeded at the next IMU update
     if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))) {
         CovariancePrediction();
-        covPredStep = true;
-        summedDelAng.zero();
-        summedDelVel.zero();
-        dt = 0.0;
     } else {
         covPredStep = false;
     }
@@ -877,6 +873,8 @@ void NavEKF::SelectVelPosFusion()
 
     // perform fusion
     if (fuseVelData || fusePosData || fuseHgtData) {
+        // ensure that the covariance prediction is up to date before fusing data
+        if (!covPredStep) CovariancePrediction();
         FuseVelPosNED();
     }
 
@@ -898,6 +896,9 @@ void NavEKF::SelectVelPosFusion()
 // select fusion of magnetometer data
 void NavEKF::SelectMagFusion()
 {
+    // start performance timer
+    perf_begin(_perf_FuseMagnetometer);
+
     // check for and read new magnetometer measurements
     readMagData();
 
@@ -911,21 +912,14 @@ void NavEKF::SelectMagFusion()
 
     // determine if conditions are right to start a new fusion cycle
     bool dataReady = statesInitialised && use_compass() && newDataMag;
-    if (dataReady)
+    if (dataReady) {
-    {
-        fuseMagData = true;
         // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
         memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
         magUpdateCount = 0;
-    }
+        // ensure that the covariance prediction is up to date before fusing data
-    else
+        if (!covPredStep) CovariancePrediction();
-    {
+        // fuse the three magnetometer componenents sequentially
-        fuseMagData = false;
+        for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) FuseMagnetometer();
-    }
-
-        // delay if covariance prediction is being performed on this prediction cycle unless load levelling is inhibited
-        if (!covPredStep || inhibitLoadLeveling) {
-            FuseMagnetometer();
     }
 
     // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
@@ -935,11 +929,16 @@ void NavEKF::SelectMagFusion()
             states[i] += magIncrStateDelta[i];
         }
     }
+
+    // stop performance timer
+    perf_end(_perf_FuseMagnetometer);
 }
 
 // select fusion of optical flow measurements
 void NavEKF::SelectFlowFusion()
 {
+    // start performance timer
+    perf_begin(_perf_FuseOptFlow);
     // Perform Data Checks
     // Check if the optical flow data is still valid
     flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
@@ -980,9 +979,7 @@ void NavEKF::SelectFlowFusion()
         constVelMode = false;
         lastConstVelMode = false;
     }
-    // if we do have valid flow measurements
+    // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
-
-    // Fuse data into a 1-state EKF to estimate terrain height
     if ((newDataFlow || newDataRng) && tiltOK) {
         // fuse range data into the terrain estimator if available
         fuseRngData = newDataRng;
@@ -993,11 +990,9 @@ void NavEKF::SelectFlowFusion()
         // Indicate we have used the range data
         newDataRng = false;
         // we don't do subsequent fusion of optical flow data into the main filter if GPS is good and terrain offset data is invalid
-        // because an invalid height above ground estimate will casue the optical flow measurements to fight the GPS
+        // because an invalid height above ground estimate will cause the optical flow measurements to fight the GPS
         if (!gpsNotAvailable && !gndOffsetValid) {
-            // turn of fusion permissions
+            // turn off fusion permissions
-            // reset the measurement axis index
-            flow_state.obsIndex = 0;
             // reset the flags to indicate that no new range finder or flow data is available for fusion
             newDataFlow = false;
         }
@@ -1012,27 +1007,16 @@ void NavEKF::SelectFlowFusion()
         flowUpdateCount = 0;
         // Set the flow noise used by the fusion processes
         R_LOS = sq(max(_flowNoise, 0.05f));
-        // set the measurement axis index to fuse the X axis data
+        // ensure that the covariance prediction is up to date before fusing data
-        flow_state.obsIndex = 0;
+        if (!covPredStep) CovariancePrediction();
-        // Fuse the optical flow X axis data into the main filter
+        // Fuse the optical flow X and Y axis data into the main filter sequentially
-        FuseOptFlow();
+        for (flow_state.obsIndex = 0; flow_state.obsIndex <= 1; flow_state.obsIndex++) FuseOptFlow();
-        // increment the measurement axis index to fuse the Y axis data on the next prediction cycle
-        flow_state.obsIndex = 1;
         // reset flag to indicate that no new flow data is available for fusion
         newDataFlow = false;
         // indicate that flow fusion has been performed. This is used for load spreading.
         flowFusePerformed = true;
         // update the time stamp
         prevFlowUseTime_ms = imuSampleTime_ms;
-    } else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK) {
-        // Fuse the optical flow Y axis data into the main filter
-        FuseOptFlow();
-        // Reset the measurement axis index to prevent further fusion of this data
-        flow_state.obsIndex = 0;
-        // reset flag to indicate that no new flow data is available for fusion
-        newDataFlow = false;
-        // indicate that flow fusion has been performed. This is used for load spreading.
-        flowFusePerformed = true;
     }
 
     // Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
@@ -1042,6 +1026,8 @@ void NavEKF::SelectFlowFusion()
             states[i] += flowIncrStateDelta[i];
         }
     }
+    // stop the performance timer
+    perf_end(_perf_FuseOptFlow);
 }
 
 // select fusion of true airspeed measurements
@@ -1064,6 +1050,8 @@ void NavEKF::SelectTasFusion()
     // setting fuseMeNow to true disables this load spreading feature
     if (tasDataWaiting && (!(covPredStep || magFusePerformed || flowFusePerformed) || timeout || inhibitLoadLeveling))
     {
+        // ensure that the covariance prediction is up to date before fusing data
+        if (!covPredStep) CovariancePrediction();
         FuseAirspeed();
         TASmsecPrev = imuSampleTime_ms;
         tasDataWaiting = false;
@@ -1085,6 +1073,8 @@ void NavEKF::SelectBetaFusion()
     bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
     // use synthetic sideslip fusion if feasible, required, enough time has lapsed since the last fusion and it is not locked out
     if (f_feasible && f_required && f_timeTrigger && !f_lockedOut) {
+        // ensure that the covariance prediction is up to date before fusing data
+        if (!covPredStep) CovariancePrediction();
         FuseSideslip();
         BETAmsecPrev = imuSampleTime_ms;
     }
@@ -1850,6 +1840,12 @@ void NavEKF::CovariancePrediction()
     // constrain diagonals to prevent ill-conditioning
     ConstrainVariances();
 
+    // set the flag to indicate that covariance prediction has been performed and reset the increments used by the covariance prediction
+    covPredStep = true;
+    summedDelAng.zero();
+    summedDelVel.zero();
+    dt = 0.0;
+
     perf_end(_perf_CovariancePrediction);
 }
 
@@ -2238,9 +2234,6 @@ void NavEKF::FuseVelPosNED()
 // fuse each axis on consecutive time steps to spread computional load
 void NavEKF::FuseMagnetometer()
 {
-    // start performance timer
-    perf_begin(_perf_FuseMagnetometer);
-
     // declarations
     ftype &q0 = mag_state.q0;
     ftype &q1 = mag_state.q1;
@@ -2268,10 +2261,8 @@ void NavEKF::FuseMagnetometer()
     // data fit is the only assumption we can make
     // so we might as well take advantage of the computational efficiencies
     // associated with sequential fusion
-    if (fuseMagData || obsIndex == 1 || obsIndex == 2)
-    {
     // calculate observation jacobians and Kalman gains
-        if (fuseMagData)
+    if (obsIndex == 0)
     {
         // copy required states to local variable names
         q0       = statesAtMagMeasTime.quat[0];
@@ -2609,22 +2600,11 @@ void NavEKF::FuseMagnetometer()
             }
         }
     }
-        obsIndex = obsIndex + 1;
-    }
-    else
-    {
-    // set flags to indicate to other processes that fusion has not been performed and is not required on the next time step
-    magFusePerformed = false;
-    magFuseRequired = false;
-    }
 
     // force the covariance matrix to be symmetrical and limit the variances to prevent
     // ill-condiioning.
     ForceSymmetry();
     ConstrainVariances();
-
-    // stop performance timer
-    perf_end(_perf_FuseMagnetometer);
 }
 
 /*
@@ -2809,9 +2789,6 @@ void NavEKF::EstimateTerrainOffset()
 
 void NavEKF::FuseOptFlow()
 {
-    // start performance timer
-    perf_begin(_perf_FuseOptFlow);
-
     Vector22 H_LOS;
     Vector9 tempVar;
     Vector3f velNED_local;
@@ -3027,7 +3004,7 @@ void NavEKF::FuseOptFlow()
         // Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
         // Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
         bool highRates = ((flowUpdateCountMax * correctedDelAng.length()) > 0.1f);
-        // Calculate the number of averaging frames left to go. This is required because flow fusion is applied across two consecutive prediction cycles
+        // Calculate the number of averaging frames left to go.
         // There is no point averaging if the number of cycles left is less than 2
         float minorFramesToGo = float(flowUpdateCountMax) - float(flowUpdateCount);
         for (uint8_t i = 0; i<=21; i++) {
@@ -3085,8 +3062,6 @@ void NavEKF::FuseOptFlow()
     ForceSymmetry();
     ConstrainVariances();
 
-    // stop the performance timer
-    perf_end(_perf_FuseOptFlow);
 }
 
 // fuse true airspeed measurements

libraries/AP_NavEKF/AP_NavEKF.h

@@ -540,7 +540,6 @@ private:
     state_elements statesAtHgtTime; // States at the effective time of hgtMea measurement
     Vector3f innovMag;              // innovation output from fusion of X,Y,Z compass measurements
     Vector3f varInnovMag;           // innovation variance output from fusion of X,Y,Z compass measurements
-    bool fuseMagData;               // boolean true when magnetometer data is to be fused
     Vector3f magData;               // magnetometer flux readings in X,Y,Z body axes
     state_elements statesAtMagMeasTime;   // filter states at the effective time of compass measurements
     ftype innovVtas;                // innovation output from fusion of airspeed measurements