From 0bf991eef977861d5643c11438667821e4db5ae6 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 5 Nov 2014 20:05:07 +1100 Subject: [PATCH] AP_NavEKF: Fix bug in optical flow fusion smoothing --- libraries/AP_NavEKF/AP_NavEKF.cpp | 16 ++++++++++++---- libraries/AP_NavEKF/AP_NavEKF.h | 7 ++++--- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 807d702ceb..85b6bd2959 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -397,6 +397,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _msecMagAvg = 100; // average number of msec between magnetometer measurements _msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements _msecBetaMax = 200; // maximum number of msec between synthetic sideslip measurements + _msecFlowAvg = 100; // average number of msec between optical flow measurements dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. // Misc initial conditions @@ -545,7 +546,7 @@ void NavEKF::InitialiseFilterDynamic(void) dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); dtIMUinv = 1.0f / dtIMU; - // If we have a high rate update step of >100 Hz, then there may not enough time to all the fusion steps at once, so load levelling is used + // If we have a high rate update step of >100 Hz, then there may not be enough time to do all the fusion steps at once, so load levelling is used if (dtIMU < 0.009f) { inhibitLoadLeveling = false; } else { @@ -558,6 +559,8 @@ void NavEKF::InitialiseFilterDynamic(void) hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); + flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecFlowAvg); + flowUpdateCountMax = uint8_t(1.0f/flowUpdateCountMaxInv); // calculate initial orientation and earth magnetic field states state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); @@ -931,6 +934,9 @@ void NavEKF::SelectFlowFusion() // if the filter is initialised, we have data to fuse and the vehicle is not excessively tilted, then perform optical flow fusion if (useOptFlow() && newDataFlow && tiltOK && !delayFusion && !staticMode) { + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); + flowUpdateCount = 0; // Set the flow noise used by the fusion processes R_LOS = sq(max(_flowNoise, 0.05f)); // Fuse the optical flow X axis data into the main filter @@ -944,6 +950,7 @@ void NavEKF::SelectFlowFusion() flowFusePerformed = true; // update the time stamp prevFlowFusionTime_ms = imuSampleTime_ms; + } else if (flow_state.obsIndex == 1 && !delayFusion){ // Fuse the optical flow Y axis data into the main filter FuseOptFlow(); @@ -3020,10 +3027,10 @@ 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 = ((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 + 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 // There is no point averaging if the number of cycles left is less than 2 - float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount); + float minorFramesToGo = float(flowUpdateCountMax) - float(flowUpdateCount); 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]; @@ -4292,6 +4299,7 @@ void NavEKF::ZeroVariables() memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta)); memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); + memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); gpsPosGlitchOffsetNE.zero(); // optical flow variables newDataFlow = false; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 3abeeb0cce..40d6d3e9d7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -545,7 +545,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 flowIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer 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 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 @@ -595,14 +595,15 @@ 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 + float flowIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement bool newDataRng; // true when new valid range finder data has arrived. - bool velHoldMode; // true when holding velocity in optical flow mode when no flow measurements are available + bool velHoldMode; // true when holding velocity in optical flow mode when no flow measurements are available bool lastHoldVelocity; // last value of holdVelocity Vector2f heldVelNE; // velocity held when no aiding is available + uint16_t _msecFlowAvg; // average number of msec between synthetic sideslip measurements // states held by optical flow fusion across time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps