From 988de2a898989b6be9ee2914f0fb3d406a553625 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 2 Sep 2014 20:01:44 +1000 Subject: [PATCH] AP_NavEKF : Add smoothing to optical flow fusion --- libraries/AP_NavEKF/AP_NavEKF.cpp | 21 ++++++++++++++++++++- libraries/AP_NavEKF/AP_NavEKF.h | 6 +++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index ff157f3bb7..5fc1703617 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/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 diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 4170beb15c..ab721c369c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/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