// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
    // Save the previous status so we can detect when it has changed
    PV_AidingModePrev = PV_AidingMode;

    // Determine if we should change aiding mode
    switch (PV_AidingMode) {
    case AID_NONE: {
        // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
        // and IMU gyro bias estimates have stabilised
        bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
        // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
        // GPS aiding is the preferred option unless excluded by the user
        bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
        bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
        if(canUseGPS || canUseRangeBeacon) {
            PV_AidingMode = AID_ABSOLUTE;
        } else if (optFlowDataPresent() && filterIsStable) {
            PV_AidingMode = AID_RELATIVE;
        }
        }
        break;

    case AID_RELATIVE: {
        // Check if the optical flow sensor has timed out
        bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
        // Check if the fusion has timed out (flow measurements have been rejected for too long)
        bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
        // Enable switch to absolute position mode if GPS is available
        // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
        if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
            PV_AidingMode = AID_ABSOLUTE;
        } else if (flowSensorTimeout || flowFusionTimeout) {
            PV_AidingMode = AID_NONE;
        }
        }
        break;

    case AID_ABSOLUTE: {
        // Find the minimum time without data required to trigger any check
        uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));

        // Check if optical flow data is being used
        bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);

        // Check if airspeed data is being used
        bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);

        // Check if range beacon data is being used
        bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);

        // Check if GPS is being used
        bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
        bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);

        // Check if attitude drift has been constrained by a measurement source
        bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;

        // check if velocity drift has been constrained by a measurement source
        bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;

        // check if position drift has been constrained by a measurement source
        bool posAiding = gpsPosUsed || rngBcnUsed;

        // Check if the loss of attitude aiding has become critical
        bool attAidLossCritical = false;
        if (!attAiding) {
            attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
                   (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
                   (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
                   (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
                   (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
        }

        // Check if the loss of position accuracy has become critical
        bool posAidLossCritical = false;
        if (!posAiding ) {
            uint16_t maxLossTime_ms;
            if (!velAiding) {
                maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
            } else {
                maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
            }
            posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
                   (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
        }

        if (attAidLossCritical) {
            // if the loss of attitude data is critical, then put the filter into a constant position mode
            PV_AidingMode = AID_NONE;
            posTimeout = true;
            velTimeout = true;
            rngBcnTimeout = true;
            tasTimeout = true;
            gpsNotAvailable = true;
        } else if (posAidLossCritical) {
            // if the loss of position is critical, declare all sources of position aiding as being timed out
            posTimeout = true;
            velTimeout = true;
            rngBcnTimeout = true;
            gpsNotAvailable = true;
        }
        }
        break;

    default:
        break;
    }

    // check to see if we are starting or stopping aiding and set states and modes as required
    if (PV_AidingMode != PV_AidingModePrev) {
        // set various  usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
        switch (PV_AidingMode) {
        case AID_NONE:
            // We have ceased aiding
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
            // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
            posTimeout = true;
            velTimeout = true;            
            // Reset the normalised innovation to avoid false failing bad fusion tests
            velTestRatio = 0.0f;
            posTestRatio = 0.0f;
             // store the current position to be used to keep reporting the last known position
            lastKnownPositionNE.x = stateStruct.position.x;
            lastKnownPositionNE.y = stateStruct.position.y;
            // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
            // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
            meaHgtAtTakeOff = baroDataDelayed.hgt;
            // reset the vertical position state to faster recover from baro errors experienced during touchdown
            stateStruct.position.z = -meaHgtAtTakeOff;
            break;

        case AID_RELATIVE:
            // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
            posTimeout = true;
            velTimeout = true;
            // Reset the last valid flow measurement time
            flowValidMeaTime_ms = imuSampleTime_ms;
            // Reset the last valid flow fusion time
            prevFlowFuseTime_ms = imuSampleTime_ms;
            break;

        case AID_ABSOLUTE: {
            bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
            bool canUseRangeBeacon = readyToUseRangeBeacon();
            // We have commenced aiding and GPS usage is allowed
            if (canUseGPS) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
            }
            posTimeout = false;
            velTimeout = false;
            // We have commenced aiding and range beacon usage is allowed
            if (canUseRangeBeacon) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
            }
            // reset the last fusion accepted times to prevent unwanted activation of timeout logic
            lastPosPassTime_ms = imuSampleTime_ms;
            lastVelPassTime_ms = imuSampleTime_ms;
            lastRngBcnPassTime_ms = imuSampleTime_ms;
            }
            break;

        default:
            break;
        }

        // Always reset the position and velocity when changing mode
        ResetVelocity();
        ResetPosition();
    }
}
// fuse selected position, velocity and height measurements
void NavEKF2_core::FuseVelPosNED()
{
    // start performance timer
    hal.util->perf_begin(_perf_FuseVelPosNED);

    // health is set bad until test passed
    velHealth = false;
    posHealth = false;
    hgtHealth = false;

    // declare variables used to check measurement errors
    Vector3f velInnov;

    // declare variables used to control access to arrays
    bool fuseData[6] = {false,false,false,false,false,false};
    uint8_t stateIndex;
    uint8_t obsIndex;

    // declare variables used by state and covariance update calculations
    float posErr;
    Vector6 R_OBS; // Measurement variances used for fusion
    Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
    Vector6 observation;
    float SK;

    // perform sequential fusion of GPS measurements. This assumes that the
    // errors in the different velocity and position components are
    // uncorrelated which is not true, however in the absence of covariance
    // data from the GPS receiver it is the only assumption we can make
    // so we might as well take advantage of the computational efficiencies
    // associated with sequential fusion
    if (fuseVelData || fusePosData || fuseHgtData) {
        // set the GPS data timeout depending on whether airspeed data is present
        uint32_t gpsRetryTime;
        if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
        else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms;

        // form the observation vector
        observation[0] = gpsDataDelayed.vel.x;
        observation[1] = gpsDataDelayed.vel.y;
        observation[2] = gpsDataDelayed.vel.z;
        observation[3] = gpsDataDelayed.pos.x;
        observation[4] = gpsDataDelayed.pos.y;
        observation[5] = -hgtMea;

        // calculate additional error in GPS position caused by manoeuvring
        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
        // otherwise we scale it using manoeuvre acceleration
        // Use different errors if flying without GPS using synthetic position and velocity data
        if (PV_AidingMode == AID_NONE && inFlight) {
            // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
            // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
            R_OBS[0] = sq(10.0f);
            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
            R_OBS[3] = sq(25.0f);
            R_OBS[4] = R_OBS[3];
        } else {
            if (gpsSpdAccuracy > 0.0f) {
                // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
                R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
                R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
            } else {
                // calculate additional error in GPS velocity caused by manoeuvring
                R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
                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<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
        for (uint8_t i=2; 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
        // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
        if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
            // calculate innovations for height and vertical GPS vel measurements
            float hgtErr  = stateStruct.position.z - observation[5];
            float velDErr = stateStruct.velocity.z - observation[2];
            // check if they are the same sign and both more than 3-sigma out of bounds
            if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
                badIMUdata = true;
            } else {
                badIMUdata = false;
            }
        }

        // calculate innovations and check GPS data validity using an innovation consistency check
        // test position measurements
        if (fusePosData) {
            // test horizontal position measurements
            innovVelPos[3] = stateStruct.position.x - observation[3];
            innovVelPos[4] = stateStruct.position.y - observation[4];
            varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
            varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
            // apply an innovation consistency threshold test, but don't fail if bad IMU data
            float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
            posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
            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
            if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) {
                posHealth = true;
                // only reset the failed time and do glitch timeout checks if we are doing full aiding
                if (PV_AidingMode == AID_ABSOLUTE) {
                    lastPosPassTime_ms = imuSampleTime_ms;
                    // if timed out or outside the specified uncertainty radius, reset to the GPS
                    if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
                        // reset the position to the current GPS position
                        ResetPosition();
                        // reset the velocity to the GPS velocity
                        ResetVelocity();
                        // don't fuse GPS data on this time step
                        fusePosData = false;
                        fuseVelData = false;
                        // Reset the position variances and corresponding covariances to a value that will pass the checks
                        zeroRows(P,6,7);
                        zeroCols(P,6,7);
                        P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
                        P[7][7] = P[6][6];
                        // Reset the normalised innovation to avoid failing the bad fusion tests
                        posTestRatio = 0.0f;
                        velTestRatio = 0.0f;
                    }
                }
            } else {
                posHealth = false;
            }
        }

        // test velocity measurements
        if (fuseVelData) {
            // test velocity measurements
            uint8_t imax = 2;
            // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
            if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
                imax = 1;
            }
            float innovVelSumSq = 0; // sum of squares of velocity innovations
            float varVelSum = 0; // sum of velocity innovation variances
            for (uint8_t i = 0; i<=imax; i++) {
                // velocity states start at index 3
                stateIndex   = i + 3;
                // calculate innovations using blended and single IMU predicted states
                velInnov[i]  = stateStruct.velocity[i] - observation[i]; // blended
                // calculate innovation variance
                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
                // sum the innovation and innovation variances
                innovVelSumSq += sq(velInnov[i]);
                varVelSum += varInnovVelPos[i];
            }
            // apply an innovation consistency threshold test, but don't fail if bad IMU data
            // calculate the test ratio
            velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
            // fail if the ratio is greater than 1
            velHealth = ((velTestRatio < 1.0f)  || badIMUdata);
            // declare a timeout if we have not fused velocity data for too long or not aiding
            velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
            // use velocity data if healthy, timed out, or in constant position mode
            if (velHealth || velTimeout) {
                velHealth = true;
                // restart the timeout count
                lastVelPassTime_ms = imuSampleTime_ms;
                // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
                if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
                    // reset the velocity to the GPS velocity
                    ResetVelocity();
                    // don't fuse GPS velocity data on this time step
                    fuseVelData = false;
                    // Reset the normalised innovation to avoid failing the bad fusion tests
                    velTestRatio = 0.0f;
                }
            } else {
                velHealth = false;
            }
        }

        // test height measurements
        if (fuseHgtData) {
            // calculate height innovations
            innovVelPos[5] = stateStruct.position.z - observation[5];
            varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
            // calculate the innovation consistency test ratio
            hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
            // fail if the ratio is > 1, but don't fail if bad IMU data
            hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
            // Fuse height data if healthy or timed out or in constant position mode
            if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
                // Calculate a filtered value to be used by pre-flight health checks
                // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
                if (onGround) {
                    float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
                    const float hgtInnovFiltTC = 2.0f;
                    float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
                    hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
                } else {
                    hgtInnovFiltState = 0.0f;
                }

                // if timed out, reset the height
                if (hgtTimeout) {
                    ResetHeight();
                    hgtTimeout = false;
                }

                // If we have got this far then declare the height data as healthy and reset the timeout counter
                hgtHealth = true;
                lastHgtPassTime_ms = imuSampleTime_ms;
            }
        }

        // set range for sequential fusion of velocity and position measurements depending on which data is available and its health
        if (fuseVelData && velHealth) {
            fuseData[0] = true;
            fuseData[1] = true;
            if (useGpsVertVel) {
                fuseData[2] = true;
            }
            tiltErrVec.zero();
        }
        if (fusePosData && posHealth) {
            fuseData[3] = true;
            fuseData[4] = true;
            tiltErrVec.zero();
        }
        if (fuseHgtData && hgtHealth) {
            fuseData[5] = true;
        }

        // fuse measurements sequentially
        for (obsIndex=0; obsIndex<=5; obsIndex++) {
            if (fuseData[obsIndex]) {
                stateIndex = 3 + obsIndex;
                // calculate the measurement innovation, using states from a different time coordinate if fusing height data
                // adjust scaling on GPS measurement noise variances if not enough satellites
                if (obsIndex <= 2)
                {
                    innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
                    R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                }
                else if (obsIndex == 3 || obsIndex == 4) {
                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
                    R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                } else if (obsIndex == 5) {
                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
                    const float gndMaxBaroErr = 4.0f;
                    const float gndBaroInnovFloor = -0.5f;

                    if(getTouchdownExpected()) {
                        // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
                        // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
                        // this function looks like this:
                        //         |/
                        //---------|---------
                        //    ____/|
                        //   /     |
                        //  /      |
                        innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
                    }
                }

                // calculate the Kalman gain and calculate innovation variances
                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
                SK = 1.0f/varInnovVelPos[obsIndex];
                for (uint8_t i= 0; i<=15; i++) {
                    Kfusion[i] = P[i][stateIndex]*SK;
                }

                // inhibit magnetic field state estimation by setting Kalman gains to zero
                if (!inhibitMagStates) {
                    for (uint8_t i = 16; i<=21; i++) {
                        Kfusion[i] = P[i][stateIndex]*SK;
                    }
                } else {
                    for (uint8_t i = 16; i<=21; i++) {
                        Kfusion[i] = 0.0f;
                    }
                }

                // inhibit wind state estimation by setting Kalman gains to zero
                if (!inhibitWindStates) {
                    Kfusion[22] = P[22][stateIndex]*SK;
                    Kfusion[23] = P[23][stateIndex]*SK;
                } else {
                    Kfusion[22] = 0.0f;
                    Kfusion[23] = 0.0f;
                }

                // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
                stateStruct.angErr.zero();

                // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
                for (uint8_t i = 0; i<=stateIndexLim; i++) {
                    statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
                }

                // the first 3 states represent the angular misalignment vector. This is
                // is used to correct the estimated quaternion
                stateStruct.quat.rotate(stateStruct.angErr);

                // sum the attitude error from velocity and position fusion only
                // used as a metric for convergence monitoring
                if (obsIndex != 5) {
                    tiltErrVec += stateStruct.angErr;
                }

                // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
                // this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
                for (uint8_t i= 0; i<=stateIndexLim; i++) {
                    for (uint8_t j= 0; j<=stateIndexLim; j++)
                    {
                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
                    }
                }
                for (uint8_t i= 0; i<=stateIndexLim; i++) {
                    for (uint8_t j= 0; j<=stateIndexLim; j++) {
                        P[i][j] = P[i][j] - KHP[i][j];
                    }
                }
            }
        }
    }

    // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
    ForceSymmetry();
    ConstrainVariances();

    // stop performance timer
    hal.util->perf_end(_perf_FuseVelPosNED);
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
    // Determine when to commence aiding for inertial navigation
    // Save the previous status so we can detect when it has changed
    prevIsAiding = isAiding;
    // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
    bool filterIsStable = tiltAlignComplete && yawAlignComplete;
    // If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
    bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
    // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
    // Latch to on. Aiding can be turned off by setting both
    isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;

    // check to see if we are starting or stopping aiding and set states and modes as required
    if (isAiding != prevIsAiding) {
        // We have transitioned either into or out of aiding
        // zero stored velocities used to do dead-reckoning
        heldVelNE.zero();
        // reset the flag that indicates takeoff for use by optical flow navigation
        takeOffDetected = false;
        // set various  useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
        if (!isAiding) {
            // We have ceased aiding
            // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
            PV_AidingMode = AID_NONE;
            posTimeout = true;
            velTimeout = true;
             // store the current position to be used to keep reporting the last known position
            lastKnownPositionNE.x = stateStruct.position.x;
            lastKnownPositionNE.y = stateStruct.position.y;
            // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
            // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
            meaHgtAtTakeOff = baroDataDelayed.hgt;
            // reset the vertical position state to faster recover from baro errors experienced during touchdown
            stateStruct.position.z = -meaHgtAtTakeOff;
        } else if (frontend._fusionModeGPS == 3) {
            // We have commenced aiding, but GPS useage has been prohibited so use optical flow only
            hal.console->printf("EKF is using optical flow\n");
            PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
            posTimeout = true;
            velTimeout = true;
            // Reset the last valid flow measurement time
            flowValidMeaTime_ms = imuSampleTime_ms;
            // Reset the last valid flow fusion time
            prevFlowFuseTime_ms = imuSampleTime_ms;
        } else {
            // We have commenced aiding and GPS useage is allowed
            hal.console->printf("EKF is using GPS\n");
            PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
            posTimeout = false;
            velTimeout = false;
            // we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
            // this is becasue the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
            lastTimeGpsReceived_ms = imuSampleTime_ms;
            secondLastGpsTime_ms = imuSampleTime_ms;
            // reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
            lastPosPassTime_ms = imuSampleTime_ms;
        }
        // Reset all position, velocity and covariance
        ResetVelocity();
        ResetPosition();
        CovarianceInit();

    }

    // Always turn aiding off when the vehicle is disarmed
    if (!isAiding) {
        PV_AidingMode = AID_NONE;
        posTimeout = true;
        velTimeout = true;
    }
}
// select fusion of optical flow measurements
void NavEKF2_core::SelectFlowFusion()
{
    // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
    // If so, don't fuse measurements on this time step to reduce frame over-runs
    // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
    if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
        optFlowFusionDelayed = true;
        return;
    } else {
        optFlowFusionDelayed = false;
    }

    // start performance timer
    hal.util->perf_begin(_perf_FuseOptFlow);
    // Perform Data Checks
    // Check if the optical flow data is still valid
    flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
    // Check if the optical flow sensor has timed out
    bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
    // Check if the fusion has timed out (flow measurements have been rejected for too long)
    bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
    // check is the terrain offset estimate is still valid
    gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
    // Perform tilt check
    bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
    // Constrain measurements to zero if we are using optical flow and are on the ground
    if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
        ofDataDelayed.flowRadXYcomp.zero();
        ofDataDelayed.flowRadXY.zero();
        flowDataValid = true;
    }

    // If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
    if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
        PV_AidingMode = AID_NONE;
        // reset the velocity
        ResetVelocity();
        // store the current position to be used to as a sythetic position measurement
        lastKnownPositionNE.x = stateStruct.position.x;
        lastKnownPositionNE.y = stateStruct.position.y;
        // reset the position
        ResetPosition();
    }

    // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
    // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
    if ((newDataFlow || newDataRng) && tiltOK) {
        // fuse range data into the terrain estimator if available
        fuseRngData = newDataRng;
        // fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
        fuseOptFlowData = (newDataFlow && !fuseRngData);
        // Estimate the terrain offset (runs a one state EKF)
        EstimateTerrainOffset();
        // Indicate we have used the range data
        newDataRng = false;
    }

    // Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
    if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
    {
        // Set the flow noise used by the fusion processes
        R_LOS = sq(max(frontend._flowNoise, 0.05f));
        // ensure that the covariance prediction is up to date before fusing data
        if (!covPredStep) CovariancePrediction();
        // Fuse the optical flow X and Y axis data into the main filter sequentially
        FuseOptFlow();
        // reset flag to indicate that no new flow data is available for fusion
        newDataFlow = false;
    }

    // stop the performance timer
    hal.util->perf_end(_perf_FuseOptFlow);
}
// check for new valid GPS data and update stored measurement if available
void NavEKF2_core::readGpsData()
{
    // check for new GPS data
    // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
    if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
        if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
            // report GPS fix status
            gpsCheckStatus.bad_fix = false;

            // store fix time from previous read
            secondLastGpsTime_ms = lastTimeGpsReceived_ms;

            // get current fix time
            lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();

            // estimate when the GPS fix was valid, allowing for GPS processing and other delays
            // ideally we should be using a timing signal from the GPS receiver to set this time
            gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;

            // Correct for the average intersampling delay due to the filter updaterate
            gpsDataNew.time_ms -= localFilterTimeStep_ms/2;

            // Prevent time delay exceeding age of oldest IMU data in the buffer
            gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);

            // read the NED velocity from the GPS
            gpsDataNew.vel = _ahrs->get_gps().velocity();

            // Use the speed accuracy from the GPS if available, otherwise set it to zero.
            // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
            float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
            gpsSpdAccuracy *= (1.0f - alpha);
            float gpsSpdAccRaw;
            if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
                gpsSpdAccuracy = 0.0f;
            } else {
                gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
            }

            // check if we have enough GPS satellites and increase the gps noise scaler if we don't
            if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.0f;
            } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.4f;
            } else { // <= 4 satellites or in constant position mode
                gpsNoiseScaler = 2.0f;
            }

            // Check if GPS can output vertical velocity and set GPS fusion mode accordingly
            if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
                useGpsVertVel = true;
            } else {
                useGpsVertVel = false;
            }

            // Monitor quality of the GPS velocity data before and after alignment using separate checks
            if (PV_AidingMode != AID_ABSOLUTE) {
                // Pre-alignment checks
                gpsGoodToAlign = calcGpsGoodToAlign();
            } else {
                // Post-alignment checks
                calcGpsGoodForFlight();
            }

            // Read the GPS locaton in WGS-84 lat,long,height coordinates
            const struct Location &gpsloc = _ahrs->get_gps().location();

            // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
            if (gpsGoodToAlign && !validOrigin) {
                setOrigin();
                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
                alignMagStateDeclination();
                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
                EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
            }

            // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
            if (validOrigin) {
                gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
                gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
                storedGPS.push(gpsDataNew);
                // declare GPS available for use
                gpsNotAvailable = false;
            }

            // Commence GPS aiding when able to
            if (readyToUseGPS() && PV_AidingMode != AID_ABSOLUTE) {
                PV_AidingMode = AID_ABSOLUTE;
                // Initialise EKF position and velocity states to last GPS measurement
                ResetPosition();
                ResetVelocity();
            }

        } else {
            // report GPS fix status
            gpsCheckStatus.bad_fix = true;
        }
    }

    // We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
    // If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
    // and use the last estimated position as a synthetic GPS position

    // check if we can use opticalflow as a backup
    bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);

    // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
    uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;

    // Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
    uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;

    // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
    if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {

        // Let other processes know that GPS is not available and that a timeout has occurred
        posTimeout = true;
        velTimeout = true;
        gpsNotAvailable = true;

        // If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
        // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
        // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
        if (PV_AidingMode == AID_ABSOLUTE && !useAirspeed() && !assume_zero_sideslip()) {
            if (optFlowBackupAvailable) {
                // we can do optical flow only nav
                frontend->_fusionModeGPS = 3;
                PV_AidingMode = AID_RELATIVE;
            } else {
                // store the current position
                lastKnownPositionNE.x = stateStruct.position.x;
                lastKnownPositionNE.y = stateStruct.position.y;

                // put the filter into constant position mode
                PV_AidingMode = AID_NONE;

                // Reset the velocity and position states
                ResetVelocity();
                ResetPosition();

                // Reset the normalised innovation to avoid false failing bad fusion tests
                velTestRatio = 0.0f;
                posTestRatio = 0.0f;
            }
        }
    }
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
    // Save the previous status so we can detect when it has changed
    PV_AidingModePrev = PV_AidingMode;

    // Determine if we should change aiding mode
     if (PV_AidingMode == AID_NONE) {
        // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
        // and IMU gyro bias estimates have stabilised
        bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
        // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
        // GPS aiding is the perferred option unless excluded by the user
        if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit) {
            PV_AidingMode = AID_ABSOLUTE;
        } else if (optFlowDataPresent() && filterIsStable) {
            PV_AidingMode = AID_RELATIVE;
        }
    } else if (PV_AidingMode == AID_RELATIVE) {
         // Check if the optical flow sensor has timed out
         bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
         // Check if the fusion has timed out (flow measurements have been rejected for too long)
         bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
         // Enable switch to absolute position mode if GPS is available
         // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
         if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
             PV_AidingMode = AID_ABSOLUTE;
         } else if (flowSensorTimeout || flowFusionTimeout) {
             PV_AidingMode = AID_NONE;
         }
     } else if (PV_AidingMode == AID_ABSOLUTE) {
         // check if we can use opticalflow as a backup
         bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);

         // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
         uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;

         // Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
         uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;

         // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
         if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {

             // Let other processes know that GPS is not available and that a timeout has occurred
             posTimeout = true;
             velTimeout = true;
             gpsNotAvailable = true;

             // If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
             // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
             // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
             if (!useAirspeed() && !assume_zero_sideslip()) {
                 if (optFlowBackupAvailable) {
                     // attempt optical flow navigation
                     PV_AidingMode = AID_RELATIVE;
                 } else {
                     // put the filter into constant position mode
                     PV_AidingMode = AID_NONE;
                 }
             }
         } else if (gpsInhibit) {
             // put the filter into constant position mode in response to an exernal request
             PV_AidingMode = AID_NONE;
         }
     }

    // check to see if we are starting or stopping aiding and set states and modes as required
    if (PV_AidingMode != PV_AidingModePrev) {
        // set various  usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
        if (PV_AidingMode == AID_NONE) {
            // We have ceased aiding
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
            // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
            posTimeout = true;
            velTimeout = true;            
            // Reset the normalised innovation to avoid false failing bad fusion tests
            velTestRatio = 0.0f;
            posTestRatio = 0.0f;
             // store the current position to be used to keep reporting the last known position
            lastKnownPositionNE.x = stateStruct.position.x;
            lastKnownPositionNE.y = stateStruct.position.y;
            // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
            // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
            meaHgtAtTakeOff = baroDataDelayed.hgt;
            // reset the vertical position state to faster recover from baro errors experienced during touchdown
            stateStruct.position.z = -meaHgtAtTakeOff;
        } else if (PV_AidingMode == AID_RELATIVE) {
            // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
            posTimeout = true;
            velTimeout = true;
            // Reset the last valid flow measurement time
            flowValidMeaTime_ms = imuSampleTime_ms;
            // Reset the last valid flow fusion time
            prevFlowFuseTime_ms = imuSampleTime_ms;
        } else if (PV_AidingMode == AID_ABSOLUTE) {
            // We have commenced aiding and GPS usage is allowed
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
            posTimeout = false;
            velTimeout = false;
            // we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
            // this is because the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
            lastTimeGpsReceived_ms = imuSampleTime_ms;
            secondLastGpsTime_ms = imuSampleTime_ms;
            // reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
            lastPosPassTime_ms = imuSampleTime_ms;
        }

        // Always reset the position and velocity when changing mode
        ResetVelocity();
        ResetPosition();

    }

}
// check for new valid GPS data and update stored measurement if available
void NavEKF2_core::readGpsData()
{
    // check for new GPS data
    if (_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) {
        if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
            // report GPS fix status
            gpsCheckStatus.bad_fix = false;

            // store fix time from previous read
            secondLastGpsTime_ms = lastTimeGpsReceived_ms;

            // get current fix time
            lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();

            // estimate when the GPS fix was valid, allowing for GPS processing and other delays
            // ideally we should be using a timing signal from the GPS receiver to set this time
            gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;

            // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
            // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
            gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);

            // Prevent time delay exceeding age of oldest IMU data in the buffer
            gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);

            // read the NED velocity from the GPS
            gpsDataNew.vel = _ahrs->get_gps().velocity();

            // Use the speed accuracy from the GPS if available, otherwise set it to zero.
            // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
            float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
            gpsSpdAccuracy *= (1.0f - alpha);
            float gpsSpdAccRaw;
            if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
                gpsSpdAccuracy = 0.0f;
            } else {
                gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
            }

            // check if we have enough GPS satellites and increase the gps noise scaler if we don't
            if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.0f;
            } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.4f;
            } else { // <= 4 satellites or in constant position mode
                gpsNoiseScaler = 2.0f;
            }

            // Check if GPS can output vertical velocity and set GPS fusion mode accordingly
            if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
                useGpsVertVel = true;
            } else {
                useGpsVertVel = false;
            }

            // Monitor quality of the GPS velocity data before and after alignment using separate checks
            if (PV_AidingMode != AID_ABSOLUTE) {
                // Pre-alignment checks
                gpsQualGood = calcGpsGoodToAlign();
            } else {
                // Post-alignment checks
                calcGpsGoodForFlight();
            }

            // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
            // If we don't have an origin, then set it to the current GPS coordinates
            const struct Location &gpsloc = _ahrs->get_gps().location();
            if (validOrigin) {
                gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
            } else if (gpsQualGood) {
                // Set the NE origin to the current GPS position
                setOrigin();
                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
                alignMagStateDeclination();
                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
                EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
                // We are by definition at the origin at the instant of alignment so set NE position to zero
                gpsDataNew.pos.zero();
                // If GPS useage isn't explicitly prohibited, we switch to absolute position mode
                if (isAiding && frontend._fusionModeGPS != 3) {
                    PV_AidingMode = AID_ABSOLUTE;
                    // Initialise EKF position and velocity states
                    ResetPosition();
                    ResetVelocity();
                }
            }

            // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
            decayGpsOffset();

            // save measurement to buffer to be fused later
            StoreGPS();

            // declare GPS available for use
            gpsNotAvailable = false;
        } else {
            // report GPS fix status
            gpsCheckStatus.bad_fix = true;
        }
    }

    // We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
    // If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
    // and use the last estimated position as a synthetic GPS position

    // check if we can use opticalflow as a backup
    bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);

    // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
    uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend.gpsRetryTimeUseTAS_ms : frontend.gpsRetryTimeNoTAS_ms;

    // Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
    uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend.gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;

    // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
    if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {

        // Let other processes know that GPS i snota vailable and that a timeout has occurred
        posTimeout = true;
        velTimeout = true;
        gpsNotAvailable = true;

        // If we are currently reliying on GPS for navigation, then we need to switch to a non-GPS mode of operation
        if (PV_AidingMode == AID_ABSOLUTE) {

            // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
            // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
            if (!useAirspeed() && !assume_zero_sideslip()) {
                if (optFlowBackupAvailable) {
                    // we can do optical flow only nav
                    frontend._fusionModeGPS = 3;
                    PV_AidingMode = AID_RELATIVE;
                } else {
                    // store the current position
                    lastKnownPositionNE.x = stateStruct.position.x;
                    lastKnownPositionNE.y = stateStruct.position.y;

                    // put the filter into constant position mode
                    PV_AidingMode = AID_NONE;

                    // reset all glitch states
                    gpsPosGlitchOffsetNE.zero();
                    gpsVelGlitchOffset.zero();

                    // Reset the velocity and position states
                    ResetVelocity();
                    ResetPosition();

                    // Reset the normalised innovation to avoid false failing the bad position fusion test
                    velTestRatio = 0.0f;
                    posTestRatio = 0.0f;
                }
            }
        }
    }

    // If not aiding we synthesise the GPS measurements at the last known position
    if (PV_AidingMode == AID_NONE) {
        if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
            gpsDataNew.pos.x = lastKnownPositionNE.x;
            gpsDataNew.pos.y = lastKnownPositionNE.y;
            gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
            // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
            // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
            gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
            // Prevent time delay exceeding age of oldest IMU data in the buffer
            gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
            // save measurement to buffer to be fused later
            StoreGPS();
        }
    }

}
	virtual void ProcessEvent( EFlowEvent event,SActivationInfo* pActInfo )
	{
		switch (event)
		{
		case eFE_Activate:
		{
			m_lastTime = gEnv->pTimer->GetFrameStartTime();
			m_bActive  = !GetPortBool(pActInfo, 1);
			pActInfo->pGraph->SetRegularlyUpdated( pActInfo->myID, m_bActive );

			if (!m_bActive)
			{
				ResetVelocity(pActInfo);
			}
		}

		break;
		case eFE_Initialize:
		{
			if (pActInfo->pEntity)
				m_localRot = pActInfo->pEntity->GetRotation();
			else
				m_localRot.SetIdentity();
			m_worldRot.SetIdentity();
			m_lastTime = gEnv->pTimer->GetFrameStartTime();
			m_bActive  = !GetPortBool(pActInfo, 1);
			pActInfo->pGraph->SetRegularlyUpdated( pActInfo->myID, m_bActive );
			break;
		}

		case eFE_Update:
		{
			if (m_bActive && pActInfo->pEntity)
			{
				CTimeValue time           = gEnv->pTimer->GetFrameStartTime();
				float      timeDifference = (time - m_lastTime).GetSeconds();
				m_lastTime = time;

				IEntity*   pEntity   = pActInfo->pEntity;
				const bool bUseWorld = GetPortInt(pActInfo, 2) == 0 ? true : false;
				Vec3       speed     = GetPortVec3(pActInfo, 0);
				speed *= timeDifference;
				Quat deltaRot (Ang3(DEG2RAD(speed)));
				Quat finalRot;
				if (bUseWorld == false)
				{
					finalRot  = pEntity->GetRotation();
					finalRot *= deltaRot;
				}
				else
				{
					m_worldRot *= deltaRot;
					finalRot    = m_worldRot;
					finalRot   *= m_localRot;
				}
				finalRot.NormalizeSafe();

				IPhysicalEntity* piPhysEnt = pEntity->GetPhysics();
				if (piPhysEnt && piPhysEnt->GetType() != PE_STATIC)
				{
					if (timeDifference > 0.0001f)
					{
						pe_action_set_velocity asv;
						asv.w                    = Quat::log(deltaRot) * (2.f / timeDifference);
						asv.bRotationAroundPivot = 1;
						pEntity->GetPhysics()->Action(&asv);
					}
				}
				else
				{
					pEntity->SetRotation(finalRot);
				}
				Ang3 currentAng = Ang3(finalRot);
				ActivateOutput(pActInfo, 1, Vec3(currentAng));
				currentAng = RAD2DEG(currentAng);
				ActivateOutput(pActInfo, 0, Vec3(currentAng));
			}
			break;
		}
		}
	}