#include #include "AP_NavEKF2.h" #include "AP_NavEKF2_core.h" #include extern const AP_HAL::HAL& hal; // Control filter mode transitions void NavEKF2_core::controlFilterModes() { // Determine motor arm status prevMotorsArmed = motorsArmed; motorsArmed = dal.get_armed(); if (motorsArmed && !prevMotorsArmed) { // set the time at which we arm to assist with checks timeAtArming_ms = imuSampleTime_ms; } // Detect if we are in flight on or ground detectFlight(); // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations setWindMagStateLearningMode(); // Check the alignmnent status of the tilt and yaw attitude // Used during initial bootstrap alignment of the filter checkAttitudeAlignmentStatus(); // Set the type of inertial navigation aiding used setAidingMode(); } /* return effective value for _magCal for this core */ uint8_t NavEKF2_core::effective_magCal(void) const { // force use of simple magnetic heading fusion for specified cores if (frontend->_magMask & core_index) { return 2; } else { return frontend->_magCal; } } // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF2_core::setWindMagStateLearningMode() { // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE); if (!inhibitWindStates && setWindInhibit) { inhibitWindStates = true; } else if (inhibitWindStates && !setWindInhibit) { inhibitWindStates = false; // set states and variances if (yawAlignComplete && useAirspeed()) { // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // which assumes the vehicle has launched into the wind Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); // set the wind sate variances to the measurement uncertainty for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f)); } } else { // set the variances using a typical wind speed for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(5.0f); } } } // determine if the vehicle is manoeuvring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { manoeuvring = false; } // Determine if learning of magnetic field states has been requested by the user uint8_t magCal = effective_magCal(); bool magCalRequested = ((magCal == 0) && inFlight) || // when flying ((magCal == 1) && manoeuvring) || // when manoeuvring ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete (magCal == 4); // all the time // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4); // Inhibit the magnetic field calibration if not requested or denied bool setMagInhibit = !magCalRequested || magCalDenied; if (!inhibitMagStates && setMagInhibit) { inhibitMagStates = true; // variances will be reset in CovariancePrediction } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; if (magFieldLearned) { // if we have already learned the field states, then retain the learned variances P[16][16] = earthMagFieldVar.x; P[17][17] = earthMagFieldVar.y; P[18][18] = earthMagFieldVar.z; P[19][19] = bodyMagFieldVar.x; P[20][20] = bodyMagFieldVar.y; P[21][21] = bodyMagFieldVar.z; } else { // set the variances equal to the observation variances for (uint8_t index=16; index<=21; index++) { P[index][index] = sq(frontend->_magNoise); } // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances alignMagStateDeclination(); } // request a reset of the yaw and magnetic field states if not done before if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) { magYawResetRequest = true; } } // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // because we want it re-done for each takeoff if (onGround) { finalInflightYawInit = false; finalInflightMagInit = false; } // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations // if we are not using those states if (inhibitMagStates && inhibitWindStates) { stateIndexLim = 15; } else if (inhibitWindStates) { stateIndexLim = 21; } else { stateIndexLim = 23; } } // 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); bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable; bool canUseExtNav = readyToUseExtNav(); if(canUseGPS || canUseRangeBeacon || canUseExtNav) { PV_AidingMode = AID_ABSOLUTE; } else if (optFlowDataPresent() && (frontend->_flowUse == FLOW_USE_NAV) && 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()) { 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 posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if external nav is being used bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || extNavUsed; // 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 = posUsed || rngBcnUsed || extNavUsed; // 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 - lastExtNavPassTime_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 - lastExtNavPassTime_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 ((frontend->_flowUse == FLOW_USE_NAV) && optFlowDataPresent() && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { PV_AidingMode = AID_NONE; } // 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; } } // 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_SEND_TEXT(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_SEND_TEXT(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()); bool canUseRangeBeacon = readyToUseRangeBeacon(); bool canUseExtNav = readyToUseExtNav(); // We have commenced aiding and GPS usage is allowed if (canUseGPS) { GCS_SEND_TEXT(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_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); } // We have commenced aiding and external nav usage is allowed if (canUseExtNav) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); //handle yaw reset as special case only if compass is disabled if (!use_compass()) { extNavYawResetRequest = true; controlMagYawReset(); } // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); } // 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; lastExtNavPassTime_ms = imuSampleTime_ms; } break; } // Always reset the position and velocity when changing mode ResetVelocity(); ResetPosition(); } } // Check the tilt and yaw alignmnent status // Used during initial bootstrap alignment of the filter void NavEKF2_core::checkAttitudeAlignmentStatus() { // Check for tilt convergence - used during initial alignment ftype alpha = 1.0f*imuDataDelayed.delAngDT; ftype temp=tiltErrVec.length(); tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; if (tiltErrFilt < 0.005f && !tiltAlignComplete) { tiltAlignComplete = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index); } // submit yaw and magnetic field reset requests depending on whether we have compass data if (tiltAlignComplete && !yawAlignComplete) { if (use_compass()) { magYawResetRequest = true; gpsYawResetRequest = false; } else { magYawResetRequest = false; gpsYawResetRequest = true; } } } // return true if we should use the airspeed sensor bool NavEKF2_core::useAirspeed(void) const { return dal.airspeed_sensor_enabled(); } // return true if we should use the range finder sensor bool NavEKF2_core::useRngFinder(void) const { // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor return true; } // return true if optical flow data is available bool NavEKF2_core::optFlowDataPresent(void) const { return (imuSampleTime_ms - flowMeaTime_ms < 200); } // return true if the filter to be ready to use gps bool NavEKF2_core::readyToUseGPS(void) const { return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse; } // return true if the filter to be ready to use the beacon range measurements bool NavEKF2_core::readyToUseRangeBeacon(void) const { return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse; } // return true if the filter to be ready to use external nav data bool NavEKF2_core::readyToUseExtNav(void) const { return tiltAlignComplete && extNavDataToFuse; } // return true if we should use the compass bool NavEKF2_core::use_compass(void) const { return dal.get_compass() && dal.get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; } /* should we assume zero sideslip? */ bool NavEKF2_core::assume_zero_sideslip(void) const { // we don't assume zero sideslip for ground vehicles as EKF could // be quite sensitive to a rapid spin of the ground vehicle if // traction is lost return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND; } // sets the local NED origin using a LLH location (latitude, longitude, height) // returns false if absolute aiding and GPS is being used or if the origin is already set bool NavEKF2_core::setOriginLLH(const Location &loc) { if (PV_AidingMode == AID_ABSOLUTE && !extNavUsedForPos) { // reject attempts to set the origin if GPS is being used return false; } return setOrigin(loc); } // sets the local NED origin using a LLH location (latitude, longitude, height) // returns false if the origin has already been set bool NavEKF2_core::setOrigin(const Location &loc) { // if the origin is valid reject setting a new origin if (validOrigin) { return false; } EKF_origin = loc; ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt; // define Earth rotation vector in the NED navigation frame at the origin calcEarthRateNED(earthRateNED, EKF_origin.lat); validOrigin = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u origin set",(unsigned)imu_index); // put origin in frontend as well to ensure it stays in sync between lanes frontend->common_EKF_origin = EKF_origin; frontend->common_origin_valid = true; return true; } // record a yaw reset event void NavEKF2_core::recordYawReset() { yawAlignComplete = true; if (inFlight) { finalInflightYawInit = true; } } // return true and set the class variable true if the delta angle bias has been learned bool NavEKF2_core::checkGyroCalStatus(void) { // check delta angle bias variances const ftype delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); if (!use_compass()) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass // which can make this check fail Vector3F delAngBiasVarVec = Vector3F(P[9][9],P[10][10],P[11][11]); Vector3F temp = prevTnb * delAngBiasVarVec; delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) && (fabsF(temp.y) < delAngBiasVarMax); } else { delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) && (P[10][10] <= delAngBiasVarMax) && (P[11][11] <= delAngBiasVarMax); } return delAngBiasLearned; } // Update the filter status void NavEKF2_core::updateFilterStatus(void) { // init return value filterStatus.value = 0; bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && delAngBiasLearned; bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned; bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin; // set individual flags filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected filterStatus.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff and is in a ground effect mitigation mode filterStatus.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_quality_good = gpsGoodToAlign; filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); } void NavEKF2_core::runYawEstimatorPrediction() { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) { ftype trueAirspeed; if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) { if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) { trueAirspeed = tasDataDelayed.tas; } else { trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS(); } } else { trueAirspeed = 0.0f; } yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed); } } void NavEKF2_core::runYawEstimatorCorrection() { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) { if (gpsDataToFuse) { Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc); } // action an external reset request if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) { EKFGSF_resetMainFilterYaw(); } } } // request a reset the yaw to the GSF estimate // request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned void NavEKF2_core::EKFGSF_requestYawReset() { EKFGSF_yaw_reset_request_ms = imuSampleTime_ms; }