diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 037d65fba5..2b59c927d3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -179,10 +179,7 @@ void NavEKF3_core::realignYawGPS() void NavEKF3_core::alignYawAngle() { // update quaternion states and covariances - const rotationOrder order = (yawAngDataDelayed.type == 1) ? rotationOrder::TAIT_BRYAN_312 : rotationOrder::TAIT_BRYAN_321; - - // update quaternion states and covariances - resetQuatStateYawOnly(yawAngDataDelayed.yawAng, sq(MAX(yawAngDataDelayed.yawAngErr, 1.0e-2f)), order); + resetQuatStateYawOnly(yawAngDataDelayed.yawAng, sq(MAX(yawAngDataDelayed.yawAngErr, 1.0e-2f)), yawAngDataDelayed.order); // send yaw alignment information to console GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index); @@ -206,6 +203,24 @@ void NavEKF3_core::SelectMagFusion() yaw_source_reset = true; } + // Store yaw angle when moving for use as a static reference when not moving + if (!onGroundNotMoving) { + if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis + yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321; + } else { + // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis + yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312; + } + if (yawAngDataStatic.order == rotationOrder::TAIT_BRYAN_321) { + yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); + } else if (yawAngDataStatic.order == rotationOrder::TAIT_BRYAN_312) { + yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]); + } + yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f); + yawAngDataStatic.time_ms = imuDataDelayed.time_ms; + } + // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in // flight using the output from the GSF yaw estimator. if (!use_compass() && @@ -219,44 +234,26 @@ void NavEKF3_core::SelectMagFusion() } if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { - if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { - // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis - yawAngDataDelayed.type = 2; - } else { - // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis - yawAngDataDelayed.type = 1; - } - float yawEKFGSF, yawVarianceEKFGSF, velInnovLength; bool canUseEKFGSF = yawEstimator != nullptr && yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) && (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov)); - if (yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip()) { - // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement - // for non fixed wing platform types - yawAngDataDelayed.yawAngErr = MAX(sqrtf(yawVarianceEKFGSF), 0.05f); - yawAngDataDelayed.yawAng = yawEKFGSF; - fuseEulerYaw(false, true); - } else { - // fuse the last dead-reckoned yaw when static to stop yaw drift and estimate yaw gyro bias estimate - yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); - if (!onGroundNotMoving) { - if (yawAngDataDelayed.type == 2) { - yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); - } else if (yawAngDataDelayed.type == 1) { - yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]); - } - } + // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement + // for non fixed wing platform types + const bool didUseEKFGSF = yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF); + + // fallback methods + if (!didUseEKFGSF) { if (onGroundNotMoving) { // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight - fuseEulerYaw(false, true); + fuseEulerYaw(yawFusionMethod::STATIC); } else if (onGround || (sq(P[0][0])+sq(P[1][1])+sq(P[2][2])+sq(P[3][3]) > 0.01f)) { // prevent uncontrolled yaw variance growth by fusing a zero innovation // when not on ground allow more variance growth so yaw can be corrected // by manoeuvring - fuseEulerYaw(true, true); + fuseEulerYaw(yawFusionMethod::PREDICTED); } } magTestRatio.zero(); @@ -274,34 +271,25 @@ void NavEKF3_core::SelectMagFusion() alignYawAngle(); yaw_source_reset = false; } else if (tiltAlignComplete && yawAlignComplete) { - fuseEulerYaw(false, true); + fuseEulerYaw(yawFusionMethod::EXTERNAL); } have_fused_gps_yaw = true; last_gps_yaw_fusion_ms = imuSampleTime_ms; - } else if (tiltAlignComplete && !yawAlignComplete && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) { - yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); - // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops - if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { - // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis + } else if (tiltAlignComplete && !yawAlignComplete) { + // External yaw sources can take singificant time to start providing yaw data so + // wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable + if(imuSampleTime_ms - lastSynthYawTime_ms > 140) { + yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); + // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops if (!onGroundNotMoving) { - yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); + // prevent uncontrolled yaw variance growth by fusing a zero innovation + fuseEulerYaw(yawFusionMethod::PREDICTED); + } else { + // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight + fuseEulerYaw(yawFusionMethod::STATIC); } - yawAngDataDelayed.type = 2; - } else { - // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis - if (!onGroundNotMoving) { - yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]); - } - yawAngDataDelayed.type = 1; + lastSynthYawTime_ms = imuSampleTime_ms; } - if (onGroundNotMoving) { - // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight - fuseEulerYaw(false, true); - } else { - // prevent uncontrolled yaw variance growth by fusing a zero innovation - fuseEulerYaw(true, true); - } - lastSynthYawTime_ms = imuSampleTime_ms; } if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL) { // no fallback @@ -377,7 +365,7 @@ void NavEKF3_core::SelectMagFusion() if (dataReady) { // use the simple method of declination to maintain heading if we cannot use the magnetic field states if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) { - fuseEulerYaw(false, false); + fuseEulerYaw(yawFusionMethod::MAGNETOMER); // zero the test ratio output from the inactive 3-axis magnetometer fusion magTestRatio.zero(); @@ -855,62 +843,74 @@ void NavEKF3_core::FuseMagnetometer() } /* - * Fuse yaw measurements using explicit algebraic equations auto-generated from + * Fuse direct yaw measurements using explicit algebraic equations auto-generated from * /AP_NavEKF3/derivation/main.py with output recorded in /AP_NavEKF3/derivation/generated/yaw_generated.cpp - * This fusion method only modifies the orientation, does not require use of the magnetic field states and is computationally cheaper. - * It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground). - * It is not as robust to magnetometer failures. - * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degrees latitude of the magnetic poles) - * - * The following booleans are passed to the function to control the fusion process: - * - * usePredictedYaw - Set this to true if no valid yaw measurement will be available for an extended periods. - * This uses an innovation set to zero which prevents uncontrolled quaternion covariance - * growth or if available, a yaw estimate from the Gaussian Sum Filter. - * UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor (GPS or external nav) is being used instead of the magnetometer. + * Returns true if the fusion was successful */ -void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) +bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) { const float &q0 = stateStruct.quat[0]; const float &q1 = stateStruct.quat[1]; const float &q2 = stateStruct.quat[2]; const float &q3 = stateStruct.quat[3]; - // external yaw available check - bool canUseGsfYaw = false; - float gsfYaw, gsfYawVariance, velInnovLength; - if (usePredictedYaw && yawEstimator != nullptr) { - canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) && - is_positive(gsfYawVariance) && - gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) && - (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov)); + float gsfYaw, gsfYawVariance; + if (method == yawFusionMethod::GSF) { + bool canUseGsfYaw; + if (yawEstimator != nullptr) { + float velInnovLength; + canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) && + is_positive(gsfYawVariance) && + gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) && + (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov)); + } else { + canUseGsfYaw = false; + } + if (!canUseGsfYaw) { + return false; + } } // yaw measurement error variance (rad^2) float R_YAW; - if (canUseGsfYaw) { - R_YAW = gsfYawVariance; - } else if (!useExternalYawSensor) { - R_YAW = sq(frontend->_yawNoise); - } else { + switch (method) { + case yawFusionMethod::EXTERNAL: R_YAW = sq(yawAngDataDelayed.yawAngErr); + break; + + case yawFusionMethod::GSF: + R_YAW = gsfYawVariance; + break; + + case yawFusionMethod::STATIC: + R_YAW = sq(yawAngDataStatic.yawAngErr); + break; + + case yawFusionMethod::MAGNETOMER: + case yawFusionMethod::PREDICTED: + default: + R_YAW = sq(frontend->_yawNoise); + } // determine if a 321 or 312 Euler sequence is best - bool useEuler321 = true; - if (useExternalYawSensor) { - // If using an external sensor, the definition of yaw is specified through the sensor interface - if (yawAngDataDelayed.type == 2) { - useEuler321 = true; - } else if (yawAngDataDelayed.type == 1) { - useEuler321 = false; - } else { - // invalid selection - return; - } - } else { - // if using the magnetometer, it is determined automatically - useEuler321 = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])); + rotationOrder order; + switch (method) { + case yawFusionMethod::EXTERNAL: + order = yawAngDataDelayed.order; + break; + + case yawFusionMethod::STATIC: + order = yawAngDataStatic.order; + break; + + case yawFusionMethod::MAGNETOMER: + case yawFusionMethod::GSF: + case yawFusionMethod::PREDICTED: + default: + // determined automatically + order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312; + } // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix @@ -918,7 +918,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) float H_YAW[4]; Matrix3f Tbn_zeroYaw; - if (useEuler321) { + if (order == rotationOrder::TAIT_BRYAN_321) { // calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw bool canUseA = false; const float SA0 = 2*q3; @@ -967,7 +967,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2); H_YAW[3] = -SB5*(-SB0*SB7 - SB9*q3); } else { - return; + return false; } // Get the 321 euler angles @@ -978,7 +978,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // set the yaw to zero and calculate the zero yaw rotation from body to earth frame Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); - } else { + } else if (order == rotationOrder::TAIT_BRYAN_312) { // calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw bool canUseA = false; const float SA0 = 2*q3; @@ -1027,7 +1027,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2); H_YAW[3] = -SB5*(SB0*SB7 + SB9*q3); } else { - return; + return false; } // Get the 312 Tait Bryan rotation angles @@ -1036,33 +1036,43 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // set the yaw to zero and calculate the zero yaw rotation from body to earth frame Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); + } else { + // order not supported + return false; } // Calculate the innovation - float innovation; - if (!usePredictedYaw) { - if (!useExternalYawSensor) { - // Use the difference between the horizontal projection and declination to give the measured yaw - // rotate measured mag components into earth frame - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); - innovation = wrap_PI(yawAngPredicted - yawAngMeasured); - } else { - // use the external yaw sensor data - innovation = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng); - } - } else if (canUseGsfYaw) { - // The GSF yaw esitimator can provide a better estimate than the main nav filter can when no yaw - // sensor is available - innovation = wrap_PI(yawAngPredicted - gsfYaw); - } else { - // setting the innovation to zero enables quaternion covariance growth to be constrained when there - // is no method of observing yaw - innovation = 0.0f; + switch (method) { + case yawFusionMethod::MAGNETOMER: + { + // Use the difference between the horizontal projection and declination to give the measured yaw + // rotate measured mag components into earth frame + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); + innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured); + break; } - // Copy raw value to output variable used for data logging - innovYaw = innovation; + case yawFusionMethod::EXTERNAL: + // both external sensor yaw and last yaw when static are stored in yawAngDataDelayed.yawAng + innovYaw = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng); + break; + + case yawFusionMethod::STATIC: + // both external sensor yaw and last yaw when static are stored in yawAngDataDelayed.yawAng + innovYaw = wrap_PI(yawAngPredicted - yawAngDataStatic.yawAng); + break; + + case yawFusionMethod::GSF: + innovYaw = wrap_PI(yawAngPredicted - gsfYaw); + break; + + case yawFusionMethod::PREDICTED: + default: + innovYaw = 0.0f; + break; + + } // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero float PH[4]; @@ -1085,7 +1095,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) CovarianceInit(); // output numerical health status faultStatus.bad_yaw = true; - return; + return false; } // calculate Kalman gain @@ -1098,7 +1108,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) } // calculate the innovation test ratio - yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov); + yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov); // Declare the magnetometer unhealthy if the innovation test fails if (yawTestRatio > 1.0f) { @@ -1106,19 +1116,12 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects // If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data if (inFlight) { - return; + return false; } } else { magHealth = true; } - // limit the innovation so that initial corrections are not too large - if (innovation > 0.5f) { - innovation = 0.5f; - } else if (innovation < -0.5f) { - innovation = -0.5f; - } - // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero // calculate K*H*P for (uint8_t row = 0; row <= stateIndexLim; row++) { @@ -1157,7 +1160,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // correct the state vector for (uint8_t i=0; i<=stateIndexLim; i++) { - statesArray[i] -= Kfusion[i] * innovation; + statesArray[i] -= Kfusion[i] * constrain_float(innovYaw, -0.5f, 0.5f); } stateStruct.quat.normalize(); @@ -1168,6 +1171,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // record fusion numerical health status faultStatus.bad_yaw = true; } + return true; } /* @@ -1423,13 +1427,16 @@ bool NavEKF3_core::learnMagBiasFromGPS(void) // combine yaw with current quaternion to get yaw corrected quaternion Quaternion quat = stateStruct.quat; - if (yawAngDataDelayed.type == 2) { + if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) { Vector3f euler321; quat.to_euler(euler321.x, euler321.y, euler321.z); quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng); - } else if (yawAngDataDelayed.type == 1) { + } else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) { Vector3f euler312 = quat.to_vector312(); quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng); + } else { + // rotation order not supported + return false; } // build the expected body field from orientation and table earth field diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index fe5dc0d1be..fb6d6e4772 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -961,7 +961,13 @@ void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_ yawAngDataNew.yawAng = yawAngle; yawAngDataNew.yawAngErr = yawAngleErr; - yawAngDataNew.type = type; + if (type == 2) { + yawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; + } else if (type == 1) { + yawAngDataNew.order = rotationOrder::TAIT_BRYAN_312; + } else { + return; + } yawAngDataNew.time_ms = timeStamp_ms; storedYawAng.push(yawAngDataNew); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index baa5334959..e04f8fa9f9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -723,7 +723,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED() delVelNav = prevTnb.mul_transpose(delVelCorrected); delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT; - // calculate the body to nav cosine matrix + // calculate the nav to body cosine matrix stateStruct.quat.inverse().rotation_matrix(prevTnb); // calculate the rate of change of velocity (used for launch detect and other functions) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index fe13f7d809..4df805e4fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -569,10 +569,16 @@ private: float delTime; // time interval that the measurement was accumulated over (sec) }; + // Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available + enum class rotationOrder { + TAIT_BRYAN_321=0, + TAIT_BRYAN_312=1 + }; + struct yaw_elements : EKF_obs_element_t { - float yawAng; // yaw angle measurement (rad) - float yawAngErr; // yaw angle 1SD measurement accuracy (rad) - uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX) + float yawAng; // yaw angle measurement (rad) + float yawAngErr; // yaw angle 1SD measurement accuracy (rad) + rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY) }; struct ext_nav_elements : EKF_obs_element_t { @@ -612,10 +618,13 @@ private: EXTNAV=7 // Use external nav data }; - // Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available - enum class rotationOrder { - TAIT_BRYAN_321=0, - TAIT_BRYAN_312=1 + // specifies the method to be used when fusing yaw observations + enum class yawFusionMethod { + MAGNETOMER=0, + EXTERNAL=1, + GSF=2, + STATIC=3, + PREDICTED=4, }; // update the navigation filter status @@ -858,8 +867,9 @@ private: // align the NE earth magnetic field states with the published declination void alignMagStateDeclination(); - // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states) - void fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor); + // Fuse compass measurements using a direct yaw angle observation (doesn't require magnetic field states) + // Returns true if the fusion was successful + bool fuseEulerYaw(yawFusionMethod method); // return the best Tait-Bryan rotation order to use void bestRotationOrder(rotationOrder &order); @@ -1215,6 +1225,8 @@ private: EKF_obs_buffer_t storedYawAng; yaw_elements yawAngDataNew; yaw_elements yawAngDataDelayed; + yaw_elements yawAngDataStatic; + // Range Beacon Sensor Fusion EKF_obs_buffer_t storedRangeBeacon; // Beacon range buffer