AP_NavEKF : Add smoothing to optical flow fusion

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -929,7 +929,13 @@ void NavEKF::SelectFlowFusion()
         // cheap and can be perfomred on the same prediction cycle with subsequent fusion steps
         flowFusePerformed = false;
     }
-
+    // Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
+    if (flowUpdateCount < flowUpdateCountMax) {
+        flowUpdateCount ++;
+        for (uint8_t i = 0; i <= 9; i++) {
+            states[i] += flowIncrStateDelta[i];
+        }
+    }
 }
 
 // select fusion of true airspeed measurements
@@ -2940,6 +2946,19 @@ void NavEKF::FuseOptFlow()
         {
             states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
         }
+        // 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 = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f);
+        // Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles
+        // There is no point averaging if the number of cycles left is less than 2
+        float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount);
+        for (uint8_t i = 0; i<=21; i++) {
+            if ((i <= 3 && highRates) || i >= 10 || minorFramesToGo < 1.5f) {
+                states[i] = states[i] - Kfusion[i] * innovOptFlow[obsIndex];
+            } else {
+                    flowIncrStateDelta[i] -= Kfusion[i] * innovOptFlow[obsIndex] * (flowUpdateCountMaxInv * float(flowUpdateCountMax) / minorFramesToGo);
+            }
+        }
         // normalise the quaternion states
         state.quat.normalize();
         // correct the covariance P = (I - K*H)*P

libraries/AP_NavEKF/AP_NavEKF.h

@@ -532,7 +532,7 @@ private:
     // Used by smoothing of state corrections
     float gpsIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement
     float hgtIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next height measurement
-    float magIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
+    float flowIncrStateDelta[10];   // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
     uint8_t gpsUpdateCount;         // count of the number of minor state corrections using GPS data
     uint8_t gpsUpdateCountMax;      // limit on the number of minor state corrections using GPS data
     float gpsUpdateCountMaxInv;     // floating point inverse of gpsFilterCountMax
@@ -581,6 +581,10 @@ private:
     float R_LOS;                    // variance of optical flow rate measurements (rad/sec)^2
     float auxRngTestRatio;          // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
     Vector2f flowGyroBias;          // bias error of optical flow sensor gyro output
+    float magIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
+    uint8_t flowUpdateCount;        // count of the number of minor state corrections using optical flow data
+    uint8_t flowUpdateCountMax;     // limit on the number of minor state corrections using optical flow data
+    float flowUpdateCountMaxInv;    // floating point inverse of flowUpdateCountMax
 
     // states held by optical flow fusion across time steps
     // optical flow X,Y motion compensated rate measurements are fused across two time steps