#include #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" #include #include "AP_DAL/AP_DAL.h" // Control filter mode transitions void NavEKF3_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 */ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const { // force use of simple magnetic heading fusion for specified cores if (frontend->_magMask & core_index) { return MagCal::NEVER; } // handle deprecated MagCal::EXTERNAL_YAW and MagCal::EXTERNAL_YAW_FALLBACK values const int8_t magCalParamVal = frontend->_magCal.get(); if (magCalParamVal == 5) { return MagCal::NEVER; } if (magCalParamVal == 6) { return MagCal::WHEN_FLYING; } return MagCal(magCalParamVal); } // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF3_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() && !(dragFusionEnabled && finalInflightYawInit)) || onGround || (PV_AidingMode == AID_NONE); if (!inhibitWindStates && setWindInhibit) { inhibitWindStates = true; updateStateIndexLim(); } else if (inhibitWindStates && !setWindInhibit) { inhibitWindStates = false; updateStateIndexLim(); // 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 state 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 manoeuvring = accNavMagHoriz > 0.5f; // Determine if learning of magnetic field states has been requested by the user bool magCalRequested = ((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying ((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring ((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete (effectiveMagCal == MagCal::ALWAYS); // 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() || (effectiveMagCal == MagCal::NEVER) || (onGround && effectiveMagCal != MagCal::ALWAYS); // Inhibit the magnetic field calibration if not requested or denied bool setMagInhibit = !magCalRequested || magCalDenied; if (!inhibitMagStates && setMagInhibit) { inhibitMagStates = true; updateStateIndexLim(); // variances will be reset in CovariancePrediction } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; updateStateIndexLim(); 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; } } // inhibit delta velocity bias learning if we have not yet aligned the tilt if (tiltAlignComplete && inhibitDelVelBiasStates) { // activate the states inhibitDelVelBiasStates = false; updateStateIndexLim(); // set the initial covariance values P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); P[14][14] = P[13][13]; P[15][15] = P[13][13]; } if (tiltAlignComplete && inhibitDelAngBiasStates) { // activate the states inhibitDelAngBiasStates = false; updateStateIndexLim(); // set the initial covariance values P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; } // 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; magFieldLearned = false; } updateStateIndexLim(); } // 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 void NavEKF3_core::updateStateIndexLim() { if (inhibitWindStates) { if (inhibitMagStates) { if (inhibitDelVelBiasStates) { if (inhibitDelAngBiasStates) { stateIndexLim = 9; } else { stateIndexLim = 12; } } else { stateIndexLim = 15; } } else { stateIndexLim = 21; } } else { stateIndexLim = 23; } } // Set inertial navigation aiding mode void NavEKF3_core::setAidingMode() { resetDataSource posResetSource = resetDataSource::DEFAULT; resetDataSource velResetSource = resetDataSource::DEFAULT; // Save the previous status so we can detect when it has changed PV_AidingModePrev = PV_AidingMode; // Check that the gyro bias variance has converged checkGyroCalStatus(); // Handle the special case where we are on ground and disarmed without a yaw measurement // and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS // during the previous flight. if (yaw_source_last == AP_NavEKF_Source::SourceYaw::NONE && !motorsArmed && onGround && PV_AidingMode != AID_NONE) { PV_AidingMode = AID_NONE; yawAlignComplete = false; finalInflightYawInit = false; ResetVelocity(resetDataSource::DEFAULT); ResetPosition(resetDataSource::DEFAULT); ResetHeight(); // preserve quaternion 4x4 covariances, but zero the other rows and columns for (uint8_t row=0; row<4; row++) { for (uint8_t col=4; col<24; col++) { P[row][col] = 0.0f; } } for (uint8_t col=0; col<4; col++) { for (uint8_t row=4; row<24; row++) { P[row][col] = 0.0f; } } // keep the IMU bias state variances, but zero the covariances ftype oldBiasVariance[6]; for (uint8_t row=0; row<6; row++) { oldBiasVariance[row] = P[row+10][row+10]; } zeroCols(P,10,15); zeroRows(P,10,15); for (uint8_t row=0; row<6; row++) { P[row+10][row+10] = oldBiasVariance[row]; } } // 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 // 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 if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) { PV_AidingMode = AID_ABSOLUTE; } else if (readyToUseOptFlow() || readyToUseBodyOdm()) { PV_AidingMode = AID_RELATIVE; } break; } case AID_RELATIVE: { // Check if the fusion has timed out (flow measurements have been rejected for too long) bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); // Check if the fusion has timed out (body odometry measurements have been rejected for too long) bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000); // Enable switch to absolute position mode if GPS or range beacon data is available // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) { PV_AidingMode = AID_ABSOLUTE; } else if (flowFusionTimeout && bodyOdmFusionTimeout) { 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 body odometry data is being used bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_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 or external nav is being used bool posUsed = (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 = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; // check if position drift has been constrained by a measurement source bool posAiding = posUsed || 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; 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 = !optFlowUsed && !gpsVelUsed && !bodyOdmUsed; 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, "EKF3 IMU%u 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; // reset relative aiding sensor fusion activity status flowFusionActive = false; bodyVelFusionActive = false; break; case AID_RELATIVE: // We are doing relative position navigation where velocity errors are constrained, but position drift will occur GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index); if (readyToUseOptFlow()) { // Reset time stamps flowValidMeaTime_ms = imuSampleTime_ms; prevFlowFuseTime_ms = imuSampleTime_ms; } else if (readyToUseBodyOdm()) { // Reset time stamps lastbodyVelPassTime_ms = imuSampleTime_ms; prevBodyVelFuseTime_ms = imuSampleTime_ms; } posTimeout = true; velTimeout = true; break; case AID_ABSOLUTE: if (readyToUseGPS()) { // We are commencing aiding using GPS - this is the preferred method posResetSource = resetDataSource::GPS; velResetSource = resetDataSource::GPS; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index); } else if (readyToUseRangeBeacon()) { // We are commencing aiding using range beacons posResetSource = resetDataSource::RNGBCN; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 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, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z); #if EK3_FEATURE_EXTERNAL_NAV } else if (readyToUseExtNav()) { // we are commencing aiding using external nav posResetSource = resetDataSource::EXTNAV; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 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); if (useExtNavVel) { velResetSource = resetDataSource::EXTNAV; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial vel NED = %3.1f,%3.1f,%3.1f (m/s)",(unsigned)imu_index,(double)extNavVelDelayed.vel.x,(double)extNavVelDelayed.vel.y,(double)extNavVelDelayed.vel.z); } // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); #endif // EK3_FEATURE_EXTERNAL_NAV } // clear timeout flags as a precaution to avoid triggering any additional transitions posTimeout = false; velTimeout = false; // 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; } // Always reset the position and velocity when changing mode ResetVelocity(velResetSource); ResetPosition(posResetSource); } } // Check the tilt and yaw alignmnent status // Used during initial bootstrap alignment of the filter void NavEKF3_core::checkAttitudeAlignmentStatus() { // Check for tilt convergence - used during initial alignment // Once the tilt variances have reduced, re-set the yaw and magnetic field states // and declare the tilt alignment complete if (!tiltAlignComplete) { if (tiltErrorVariance < sq(radians(5.0))) { tiltAlignComplete = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index); } } // submit yaw and magnetic field reset request if (!yawAlignComplete && tiltAlignComplete && use_compass()) { magYawResetRequest = true; } } // return true if we should use the airspeed sensor bool NavEKF3_core::useAirspeed(void) const { return dal.airspeed_sensor_enabled(); } // return true if we should use the range finder sensor bool NavEKF3_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 the filter is ready to start using optical flow measurements bool NavEKF3_core::readyToUseOptFlow(void) const { // ensure flow is used for navigation and not terrain alt estimation if (frontend->_flowUse != FLOW_USE_NAV) { return false; } if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { return false; } // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned; } // return true if the filter is ready to start using body frame odometry measurements bool NavEKF3_core::readyToUseBodyOdm(void) const { #if EK3_FEATURE_BODY_ODOM if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) && !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { // exit immediately if sources not configured to fuse external nav or wheel encoders return false; } // Check for fresh visual odometry data that meets the accuracy required for alignment bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f); // Check for fresh wheel encoder data bool wencDataGood = (imuDataDelayed.time_ms - wheelOdmDataDelayed.time_ms < 200); // We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements // because they are in a body frame of reference return (visoDataGood || wencDataGood) && tiltAlignComplete && delAngBiasLearned; #else return false; #endif // EK3_FEATURE_BODY_ODOM } // return true if the filter to be ready to use gps bool NavEKF3_core::readyToUseGPS(void) const { if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) { return false; } return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && gpsDataToFuse; } // return true if the filter to be ready to use the beacon range measurements bool NavEKF3_core::readyToUseRangeBeacon(void) const { if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) { return false; } return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; } // return true if the filter is ready to use external nav data bool NavEKF3_core::readyToUseExtNav(void) const { #if EK3_FEATURE_EXTERNAL_NAV if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) { return false; } return tiltAlignComplete && extNavDataToFuse; #else return false; #endif // EK3_FEATURE_EXTERNAL_NAV } // return true if we should use the compass bool NavEKF3_core::use_compass(void) const { const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { // not using compass as a yaw source return false; } const auto *compass = dal.get_compass(); return compass && compass->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; } // are we using a yaw source other than the magnetomer? bool NavEKF3_core::using_external_yaw(void) const { const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); #if EK3_FEATURE_EXTERNAL_NAV if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); } #endif if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; } /* should we assume zero sideslip? */ bool NavEKF3_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 NavEKF3_core::setOriginLLH(const Location &loc) { if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) { // reject attempts to set the origin if GPS is being used or if the origin is already set return false; } return setOrigin(loc); } // sets the local NED origin using a LLH location (latitude, longitude, height) // returns false is the origin has already been set bool NavEKF3_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, "EKF3 IMU%u origin set",(unsigned)imu_index); if (!frontend->common_origin_valid) { frontend->common_origin_valid = true; // put origin in frontend as well to ensure it stays in sync between lanes public_origin = EKF_origin; } return true; } // record a yaw reset event void NavEKF3_core::recordYawReset() { yawAlignComplete = true; if (inFlight) { finalInflightYawInit = true; } } // set the class variable true if the delta angle bias variances are sufficiently small void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg)); const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // which can make this check fail Vector3F delAngBiasVarVec = Vector3F(P[10][10],P[11][11],P[12][12]); Vector3F temp = prevTnb * delAngBiasVarVec; delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) && (fabsF(temp.y) < delAngBiasVarMax); } else { delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) && (P[11][11] <= delAngBiasVarMax) && (P[12][12] <= delAngBiasVarMax); } } // Update the filter status void NavEKF3_core::updateFilterStatus(void) { // init return value filterStatus.value = 0; bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000); bool doingFlowNav = (PV_AidingMode != AID_NONE) && 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 || doingBodyVelNav; bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks bool hgtNotAccurate = (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !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 || doingBodyVelNav) && 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 = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight 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 is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator 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) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_quality_good = gpsGoodToAlign; filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); } void NavEKF3_core::runYawEstimatorPrediction() { // exit immediately if no yaw estimator if (yawEstimator == nullptr) { return; } // ensure GPS is used for horizontal position and velocity if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { return; } ftype trueAirspeed; if (assume_zero_sideslip()) { trueAirspeed = MAX(tasDataDelayed.tas, 0.0f); } else { trueAirspeed = 0.0f; } yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed); } void NavEKF3_core::runYawEstimatorCorrection() { // exit immediately if no yaw estimator if (yawEstimator == nullptr) { return; } // ensure GPS is used for horizontal position and velocity if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { return; } if (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); // after velocity data has been fused the yaw variance estimate will have been refreshed and // is used maintain a history of validity ftype gsfYaw, gsfYawVariance; if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) { if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) { EKFGSF_yaw_valid_count++; } } else { EKFGSF_yaw_valid_count = 0; } } // 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(); } } else { EKFGSF_yaw_valid_count = 0; } } // 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 NavEKF3_core::EKFGSF_requestYawReset() { EKFGSF_yaw_reset_request_ms = imuSampleTime_ms; }