Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Clean up yaw fusion logic

Browse Source
This commit is contained in:
Paul Riseborough 2020-12-15 08:43:46 +11:00 committed by Peter Barker
parent ccfd89240b
commit a9e76d44af
4 changed files with 168 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

271
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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

8
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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)

30
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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<yaw_elements> storedYawAng;
yaw_elements yawAngDataNew;
yaw_elements yawAngDataDelayed;
yaw_elements yawAngDataStatic;
// Range Beacon Sensor Fusion
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer