Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: allow for double EKF build

This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00
parent 6aca0bb08a
commit 885e518741
14 changed files with 1758 additions and 1736 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1057,7 +1057,7 @@ void NavEKF3::updateCoreRelativeErrors()
// reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes
if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
coreRelativeErrors[i] += error;
coreRelativeErrors[i] = constrain_float(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
coreRelativeErrors[i] = constrain_ftype(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
}
}
}

288
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

File diff suppressed because one or more lines are too long

34
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -71,15 +71,15 @@ void NavEKF3_core::setWindMagStateLearningMode()
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;
Vector3F tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float 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);
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_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f));
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
@ -232,7 +232,7 @@ void NavEKF3_core::setAidingMode()
}
}
// keep the IMU bias state variances, but zero the covariances
float oldBiasVariance[6];
ftype oldBiasVariance[6];
for (uint8_t row=0; row<6; row++) {
oldBiasVariance[row] = P[row+10][row+10];
}
@ -407,7 +407,7 @@ void NavEKF3_core::setAidingMode()
}
// handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
ResetHeight();
#endif // EK3_FEATURE_EXTERNAL_NAV
}
@ -438,7 +438,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
// 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.0f))) {
if (tiltErrorVariance < sq(radians(5.0))) {
tiltAlignComplete = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index);
}
@ -631,16 +631,16 @@ void NavEKF3_core::recordYawReset()
void NavEKF3_core::checkGyroCalStatus(void)
{
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
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);
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) &&
@ -697,7 +697,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
return;
}
float trueAirspeed;
ftype trueAirspeed;
if (assume_zero_sideslip()) {
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
} else {
@ -720,13 +720,13 @@ void NavEKF3_core::runYawEstimatorCorrection()
if (EKFGSF_run_filterbank) {
if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
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
float gsfYaw, gsfYawVariance;
ftype gsfYaw, gsfYawVariance;
if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
EKFGSF_yaw_valid_count++;

2
libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp
View File

@ -17,7 +17,7 @@ void NavEKF3_core::resetGyroBias(void)
/*
vehicle specific initial gyro bias uncertainty in deg/sec
*/
float NavEKF3_core::InitialGyroBiasUncertainty(void) const
ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const
{
return 2.5f;
}

12
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -6,6 +6,8 @@
#include <AP_DAL/AP_DAL.h>
#pragma GCC diagnostic ignored "-Wnarrowing"
void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
{
// Write first EKF packet
@ -147,7 +149,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
nav_filter_status solutionStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
float tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus);
const struct log_NKF4 pkt4{
@ -159,7 +161,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : sqrtf(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
tiltErr : sqrtF(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
offsetNorth : offset.x,
offsetEast : offset.y,
faults : _faultStatus,
@ -191,7 +193,7 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)outputTrackError.z // output predictor position tracking error
@ -248,8 +250,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
ID : rngBcnFuseDataReportIndex,
rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)),
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)),
sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z),

332
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -25,8 +25,8 @@ void NavEKF3_core::controlMagYawReset()
}
// Quaternion and delta rotation vector that are re-used for different calculations
Vector3f deltaRotVecTemp;
Quaternion deltaQuatTemp;
Vector3F deltaRotVecTemp;
QuaternionF deltaQuatTemp;
bool flightResetAllowed = false;
bool initialResetAllowed = false;
@ -61,7 +61,7 @@ void NavEKF3_core::controlMagYawReset()
// check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset);
ftype yawInnovIncrease = fabsF(innovYaw) - fabsF(yawInnovAtLastMagReset);
// check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
@ -131,18 +131,18 @@ void NavEKF3_core::controlMagYawReset()
void NavEKF3_core::realignYawGPS()
{
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles;
Vector3F eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
// calculate course yaw angle
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x);
// calculate course yaw angle from GPS velocity
float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
ftype gpsYaw = atan2F(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
// Check the yaw angles for consistency
float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z)));
ftype yawErr = MAX(fabsF(wrap_PI(gpsYaw - velYaw)),fabsF(wrap_PI(gpsYaw - eulerAngles.z)));
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
@ -180,7 +180,7 @@ void NavEKF3_core::realignYawGPS()
void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
{
// update quaternion states and covariances
resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2f)), yawAngData.order);
resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2)), yawAngData.order);
// send yaw alignment information to console
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
@ -206,14 +206,14 @@ void NavEKF3_core::SelectMagFusion()
// 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])) {
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;
yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
yawAngDataStatic.yawAng = atan2F(prevTnb[0][1], prevTnb[0][0]);
} 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;
yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
yawAngDataStatic.yawAng = atan2F(-prevTnb[1][0], prevTnb[1][1]);
}
yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
@ -236,7 +236,7 @@ void NavEKF3_core::SelectMagFusion()
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
// 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
float gsfYaw, gsfYawVariance;
ftype gsfYaw, gsfYawVariance;
const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
// fallback methods
@ -453,8 +453,8 @@ void NavEKF3_core::FuseMagnetometer()
ftype &magXbias = mag_state.magXbias;
ftype &magYbias = mag_state.magYbias;
ftype &magZbias = mag_state.magZbias;
Matrix3f &DCM = mag_state.DCM;
Vector3f &MagPred = mag_state.MagPred;
Matrix3F &DCM = mag_state.DCM;
Vector3F &MagPred = mag_state.MagPred;
ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0];
Vector24 H_MAG;
@ -503,7 +503,7 @@ void NavEKF3_core::FuseMagnetometer()
}
// scale magnetometer observation error with total angular rate to allow for timing errors
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
@ -555,7 +555,7 @@ void NavEKF3_core::FuseMagnetometer()
// calculate the innovation test ratios
for (uint8_t i = 0; i<=2; i++) {
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
}
// check the last values from all components and set magnetometer health accordingly
@ -605,8 +605,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]);
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -619,8 +619,8 @@ void NavEKF3_core::FuseMagnetometer()
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
@ -631,8 +631,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
// zero Kalman gains to inhibit wind state estimation
@ -640,8 +640,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23 = 2
zero_range(&Kfusion[0], 22, 23);
}
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
@ -685,8 +685,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -699,8 +699,8 @@ void NavEKF3_core::FuseMagnetometer()
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
// zero Kalman gains to inhibit magnetic field state estimation
@ -712,8 +712,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
// zero Kalman gains to inhibit wind state estimation
@ -721,8 +721,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
@ -767,8 +767,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]);
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -781,8 +781,8 @@ void NavEKF3_core::FuseMagnetometer()
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
// zero Kalman gains to inhibit magnetic field state estimation
@ -794,8 +794,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
// zero Kalman gains to inhibit wind state estimation
@ -803,8 +803,8 @@ void NavEKF3_core::FuseMagnetometer()
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
@ -897,12 +897,12 @@ void NavEKF3_core::FuseMagnetometer()
*/
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];
const ftype &q0 = stateStruct.quat[0];
const ftype &q1 = stateStruct.quat[1];
const ftype &q2 = stateStruct.quat[2];
const ftype &q3 = stateStruct.quat[3];
float gsfYaw, gsfYawVariance;
ftype gsfYaw, gsfYawVariance;
if (method == yawFusionMethod::GSF) {
if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
return false;
@ -910,7 +910,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// yaw measurement error variance (rad^2)
float R_YAW;
ftype R_YAW;
switch (method) {
case yawFusionMethod::GPS:
R_YAW = sq(yawAngDataDelayed.yawAngErr);
@ -953,7 +953,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
case yawFusionMethod::PREDICTED:
default:
// determined automatically
order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
order = (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
break;
#if EK3_FEATURE_EXTERNAL_NAV
@ -964,53 +964,53 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
float yawAngPredicted;
float H_YAW[4];
Matrix3f Tbn_zeroYaw;
ftype yawAngPredicted;
ftype H_YAW[4];
Matrix3F Tbn_zeroYaw;
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;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv;
const ftype SA0 = 2*q3;
const ftype SA1 = 2*q2;
const ftype SA2 = SA0*q0 + SA1*q1;
const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
ftype SA4, SA5_inv;
if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv));
canUseA = is_positive(fabsF(SA5_inv));
}
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv;
const ftype SB0 = 2*q0;
const ftype SB1 = 2*q1;
const ftype SB2 = SB0*q3 + SB1*q2;
const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
ftype SB3, SB5_inv;
if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = is_positive(fabsf(SB5_inv));
canUseB = is_positive(fabsF(SB5_inv));
}
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
const ftype SA5 = 1.0F/SA5_inv;
const ftype SA6 = 1.0F/SA3;
const ftype SA7 = SA2*SA4;
const ftype SA8 = 2*SA7;
const ftype SA9 = 2*SA6;
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
} else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
const ftype SB5 = 1.0F/SB5_inv;
const ftype SB6 = 1.0F/SB2;
const ftype SB7 = SB3*SB4;
const ftype SB8 = 2*SB7;
const ftype SB9 = 2*SB6;
H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
@ -1021,7 +1021,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// Get the 321 euler angles
Vector3f euler321;
Vector3F euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
yawAngPredicted = euler321.z;
@ -1031,46 +1031,46 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} 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;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv;
const ftype SA0 = 2*q3;
const ftype SA1 = 2*q2;
const ftype SA2 = SA0*q0 - SA1*q1;
const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
ftype SA4, SA5_inv;
if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv));
canUseA = is_positive(fabsF(SA5_inv));
}
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv;
const ftype SB0 = 2*q0;
const ftype SB1 = 2*q1;
const ftype SB2 = -SB0*q3 + SB1*q2;
const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
ftype SB3, SB5_inv;
if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = is_positive(fabsf(SB5_inv));
canUseB = is_positive(fabsF(SB5_inv));
}
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
const ftype SA5 = 1.0F/SA5_inv;
const ftype SA6 = 1.0F/SA3;
const ftype SA7 = SA2*SA4;
const ftype SA8 = 2*SA7;
const ftype SA9 = 2*SA6;
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
} else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
const ftype SB5 = 1.0F/SB5_inv;
const ftype SB6 = 1.0F/SB2;
const ftype SB7 = SB3*SB4;
const ftype SB8 = 2*SB7;
const ftype SB9 = 2*SB6;
H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
@ -1081,7 +1081,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// Get the 312 Tait Bryan rotation angles
Vector3f euler312 = stateStruct.quat.to_vector312();
Vector3F euler312 = stateStruct.quat.to_vector312();
yawAngPredicted = euler312.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
@ -1097,8 +1097,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
{
// 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());
Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured);
break;
}
@ -1128,8 +1128,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// 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];
float varInnov = R_YAW;
ftype PH[4];
ftype varInnov = R_YAW;
for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) {
PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=3; colIndex++) {
@ -1137,7 +1137,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
varInnov += H_YAW[rowIndex]*PH[rowIndex];
}
float varInnovInv;
ftype varInnovInv;
if (varInnov >= R_YAW) {
varInnovInv = 1.0f / varInnov;
// output numerical health status
@ -1161,7 +1161,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
// calculate the innovation test ratio
yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov);
yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov);
// Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) {
@ -1184,7 +1184,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
}
for (uint8_t row = 0; row <= stateIndexLim; row++) {
for (uint8_t column = 0; column <= stateIndexLim; column++) {
float tmp = KH[row][0] * P[0][column];
ftype tmp = KH[row][0] * P[0][column];
tmp += KH[row][1] * P[1][column];
tmp += KH[row][2] * P[2][column];
tmp += KH[row][3] * P[3][column];
@ -1213,7 +1213,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
// correct the state vector
for (uint8_t i=0; i<=stateIndexLim; i++) {
statesArray[i] -= Kfusion[i] * constrain_float(innovYaw, -0.5f, 0.5f);
statesArray[i] -= Kfusion[i] * constrain_ftype(innovYaw, -0.5f, 0.5f);
}
stateStruct.quat.normalize();
@ -1234,14 +1234,14 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
* This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
* or some other absolute position or velocity reference
*/
void NavEKF3_core::FuseDeclination(float declErr)
void NavEKF3_core::FuseDeclination(ftype declErr)
{
// declination error variance (rad^2)
const float R_DECL = sq(declErr);
const ftype R_DECL = sq(declErr);
// copy required states to local variables
float magN = stateStruct.earth_magfield.x;
float magE = stateStruct.earth_magfield.y;
ftype magN = stateStruct.earth_magfield.x;
ftype magE = stateStruct.earth_magfield.y;
// prevent bad earth field states from causing numerical errors or exceptions
if (magN < 1e-3f) {
@ -1250,34 +1250,34 @@ void NavEKF3_core::FuseDeclination(float declErr)
// Calculate observation Jacobian and Kalman gains
// Calculate intermediate variables
float t2 = magE*magE;
float t3 = magN*magN;
float t4 = t2+t3;
ftype t2 = magE*magE;
ftype t3 = magN*magN;
ftype t4 = t2+t3;
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
if (t4 < 1e-4f) {
return;
}
float t5 = P[16][16]*t2;
float t6 = P[17][17]*t3;
float t7 = t2*t2;
float t8 = R_DECL*t7;
float t9 = t3*t3;
float t10 = R_DECL*t9;
float t11 = R_DECL*t2*t3*2.0f;
float t14 = P[16][17]*magE*magN;
float t15 = P[17][16]*magE*magN;
float t12 = t5+t6+t8+t10+t11-t14-t15;
float t13;
if (fabsf(t12) > 1e-6f) {
ftype t5 = P[16][16]*t2;
ftype t6 = P[17][17]*t3;
ftype t7 = t2*t2;
ftype t8 = R_DECL*t7;
ftype t9 = t3*t3;
ftype t10 = R_DECL*t9;
ftype t11 = R_DECL*t2*t3*2.0f;
ftype t14 = P[16][17]*magE*magN;
ftype t15 = P[17][16]*magE*magN;
ftype t12 = t5+t6+t8+t10+t11-t14-t15;
ftype t13;
if (fabsF(t12) > 1e-6f) {
t13 = 1.0f / t12;
} else {
return;
}
float t18 = magE*magE;
float t19 = magN*magN;
float t20 = t18+t19;
float t21;
if (fabsf(t20) > 1e-6f) {
ftype t18 = magE*magE;
ftype t19 = magN*magN;
ftype t20 = t18+t19;
ftype t21;
if (fabsF(t20) > 1e-6f) {
t21 = 1.0f/t20;
} else {
return;
@ -1285,7 +1285,7 @@ void NavEKF3_core::FuseDeclination(float declErr)
// Calculate the observation Jacobian
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
float H_DECL[24] = {};
ftype H_DECL[24] = {};
H_DECL[16] = -magE*t21;
H_DECL[17] = magN*t21;
@ -1305,8 +1305,8 @@ void NavEKF3_core::FuseDeclination(float declErr)
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -1319,8 +1319,8 @@ void NavEKF3_core::FuseDeclination(float declErr)
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
if (!inhibitMagStates) {
@ -1331,23 +1331,23 @@ void NavEKF3_core::FuseDeclination(float declErr)
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
// get the magnetic declination
float magDecAng = MagDeclination();
ftype magDecAng = MagDeclination();
// Calculate the innovation
float innovation = atan2f(magE , magN) - magDecAng;
ftype innovation = atan2F(magE , magN) - magDecAng;
// limit the innovation to protect against data errors
if (innovation > 0.5f) {
@ -1422,18 +1422,18 @@ void NavEKF3_core::alignMagStateDeclination()
}
// get the magnetic declination
float magDecAng = MagDeclination();
ftype magDecAng = MagDeclination();
// rotate the NE values so that the declination matches the published value
Vector3f initMagNED = stateStruct.earth_magfield;
float magLengthNE = norm(initMagNED.x,initMagNED.y);
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
Vector3F initMagNED = stateStruct.earth_magfield;
ftype magLengthNE = norm(initMagNED.x,initMagNED.y);
stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
if (!inhibitMagStates) {
// zero the corresponding state covariances if magnetic field state learning is active
float var_16 = P[16][16];
float var_17 = P[17][17];
ftype var_16 = P[16][16];
ftype var_17 = P[17][17];
zeroRows(P,16,17);
zeroCols(P,16,17);
P[16][16] = var_16;
@ -1484,13 +1484,13 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
}
// combine yaw with current quaternion to get yaw corrected quaternion
Quaternion quat = stateStruct.quat;
QuaternionF quat = stateStruct.quat;
if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) {
Vector3f euler321;
Vector3F euler321;
quat.to_euler(euler321.x, euler321.y, euler321.z);
quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng);
} else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) {
Vector3f euler312 = quat.to_vector312();
Vector3F euler312 = quat.to_vector312();
quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng);
} else {
// rotation order not supported
@ -1498,19 +1498,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
}
// build the expected body field from orientation and table earth field
Matrix3f dcm;
Matrix3F dcm;
quat.rotation_matrix(dcm);
Vector3f expected_body_field = dcm.transposed() * table_earth_field_ga;
Vector3F expected_body_field = dcm.transposed() * table_earth_field_ga;
// calculate error in field
Vector3f err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield;
Vector3F err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield;
// learn body frame mag biases
stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE;
// check if error is below threshold. If it is then we can
// fallback to magnetometer on failure of external yaw
float err_length = err.length();
ftype err_length = err.length();
// we allow for yaw backback to compass if we have had 50 samples
// in a row below the threshold. This corresponds to 10 seconds
@ -1540,7 +1540,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
return false;
};
float yawEKFGSF, yawVarianceEKFGSF;
ftype yawEKFGSF, yawVarianceEKFGSF;
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
// keep roll and pitch and reset yaw
rotationOrder order;
@ -1582,14 +1582,14 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
}
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const
bool NavEKF3_core::EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const
{
// return immediately if no yaw estimator
if (yawEstimator == nullptr) {
return false;
}
float velInnovLength;
ftype velInnovLength;
if (yawEstimator->getYawData(yaw, yawVariance) &&
is_positive(yawVariance) &&
yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
@ -1600,14 +1600,14 @@ bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const
return false;
}
void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order)
void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order)
{
Quaternion quatBeforeReset = stateStruct.quat;
QuaternionF quatBeforeReset = stateStruct.quat;
// check if we should use a 321 or 312 Rotation order and update the quaternion
// states using the preferred yaw definition
stateStruct.quat.inverse().rotation_matrix(prevTnb);
Vector3f eulerAngles;
Vector3F eulerAngles;
if (order == rotationOrder::TAIT_BRYAN_321) {
// rolled more than pitched so use 321 rotation order
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
@ -1624,14 +1624,14 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationO
// Update the rotation matrix
stateStruct.quat.inverse().rotation_matrix(prevTnb);
float deltaYaw = wrap_PI(yaw - eulerAngles.z);
ftype deltaYaw = wrap_PI(yaw - eulerAngles.z);
// calculate the change in the quaternion state and apply it to the output history buffer
Quaternion quat_delta = stateStruct.quat / quatBeforeReset;
QuaternionF quat_delta = stateStruct.quat / quatBeforeReset;
StoreQuatRotate(quat_delta);
// assume tilt uncertainty split equally between roll and pitch
Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, yawVariance);
Vector3F angleErrVarVec = Vector3F(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, yawVariance);
CovariancePrediction(&angleErrVarVec);
// record the yaw reset event

168
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -128,10 +128,10 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
// subtract delay from timestamp
timeStamp_ms -= delay_ms;
bodyOdmDataNew.body_offset = posOffset;
bodyOdmDataNew.vel = delPos * (1.0f/delTime);
bodyOdmDataNew.body_offset = posOffset.toftype();
bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms;
bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
bodyOdmMeasTime_ms = timeStamp_ms;
// simple model of accuracy
@ -157,7 +157,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
}
wheel_odm_elements wheelOdmDataNew = {};
wheelOdmDataNew.hub_offset = posOffset;
wheelOdmDataNew.hub_offset = posOffset.toftype();
wheelOdmDataNew.delAng = delAng;
wheelOdmDataNew.radius = radius;
wheelOdmDataNew.delTime = delTime;
@ -190,8 +190,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
@ -219,9 +219,9 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
}
// write uncorrected flow rate measurements
// note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame
ofDataNew.body_offset = posOffset;
ofDataNew.body_offset = posOffset.toftype();
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
@ -328,7 +328,7 @@ void NavEKF3_core::readMagData()
((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) {
// zero the learned magnetometer bias states
@ -357,7 +357,7 @@ void NavEKF3_core::readMagData()
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = compass.get_field(magSelectIndex) * 0.001f;
magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype();
// check for consistent data between magnetometers
consistentMagData = compass.consistent();
@ -386,7 +386,7 @@ void NavEKF3_core::readIMUData()
const auto &ins = dal.ins();
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
@ -427,7 +427,7 @@ void NavEKF3_core::readIMUData()
updateMovementCheck();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(accel_index_active);
accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
imuDataNew.accel_index = accel_index_active;
// Get delta angle data from primary gyro or primary if not available
@ -454,7 +454,7 @@ void NavEKF3_core::readIMUData()
imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into body frame at the start of accumulation
Matrix3f deltaRotMat;
Matrix3F deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator
@ -482,7 +482,7 @@ void NavEKF3_core::readIMUData()
storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF using a combination spike and LPF
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// do an addtional down sampling for data used to sample XY body frame drag specific forces
@ -506,7 +506,7 @@ void NavEKF3_core::readIMUData()
imuDataDelayed = storedIMU.get_oldest_element();
// protect against delta time going to zero
float minDT = 0.1f * dtEkfAvg;
ftype minDT = 0.1f * dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
@ -526,12 +526,16 @@ void NavEKF3_core::readIMUData()
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
const auto &ins = dal.ins();
if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel,dVel_dt);
dVel_dt = MAX(dVel_dt,1.0e-4f);
Vector3f dVelF;
float dVel_dtF;
ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
dVel = dVelF.toftype();
dVel_dt = dVel_dtF;
dVel_dt = MAX(dVel_dt,1.0e-4);
return true;
}
return false;
@ -585,7 +589,7 @@ void NavEKF3_core::readGpsData()
gpsDataNew.sensor_idx = selected_gps;
// read the NED velocity from the GPS
gpsDataNew.vel = gps.velocity(selected_gps);
gpsDataNew.vel = gps.velocity(selected_gps).toftype();
gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
// position and velocity are not yet corrected for sensor position
@ -593,7 +597,7 @@ void NavEKF3_core::readGpsData()
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
@ -686,7 +690,7 @@ void NavEKF3_core::readGpsData()
if (gpsGoodToAlign && !have_table_earth_field) {
const auto *compass = dal.get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7));
have_table_earth_field = true;
@ -699,9 +703,9 @@ void NavEKF3_core::readGpsData()
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {
gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc);
gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
if ((frontend->_originHgtMode & (1<<2)) == 0) {
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else {
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
}
@ -717,7 +721,7 @@ void NavEKF3_core::readGpsData()
// accuracy. Set to min of 5 degrees here to prevent
// the user constantly receiving warnings about high
// normalised yaw innovations
const float min_yaw_accuracy_deg = 5.0f;
const ftype min_yaw_accuracy_deg = 5.0f;
yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
}
@ -725,11 +729,15 @@ void NavEKF3_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAngDT) {
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) {
const auto &ins = dal.ins();
if (ins_index < ins.get_gyro_count()) {
ins.get_delta_angle(ins_index, dAng, dAngDT);
Vector3f dAngF;
float dAngDTF;
ins.get_delta_angle(ins_index, dAngF, dAngDTF);
dAng = dAngF.toftype();
dAngDT = dAngDTF;
return true;
}
return false;
@ -773,7 +781,7 @@ void NavEKF3_core::readBaroData()
void NavEKF3_core::calcFiltBaroOffset()
{
// Apply a first order LPF with spike protection
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
}
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
@ -782,14 +790,14 @@ void NavEKF3_core::correctEkfOriginHeight()
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
// calculate the variance of our a-priori estimate of the ekf origin height
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0, 1.0);
if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
// Use the baro drift rate
const float baroDriftRate = 0.05f;
const ftype baroDriftRate = 0.05;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f;
const ftype maxTerrGrad = 0.25;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else {
// by definition our height source is absolute so cannot run this filter
@ -799,16 +807,16 @@ void NavEKF3_core::correctEkfOriginHeight()
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
// when not using GPS as height source
float originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
// calculate the correction gain
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
// calculate the innovation
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
// check the innovation variance ratio
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if (ratio < 25.0f && gpsAccuracyGood) {
@ -910,7 +918,7 @@ void NavEKF3_core::readRngBcnData()
rngBcnDataNew.rng = beacon->beacon_distance(index);
// set the beacon position
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index);
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
// identify the beacon identifier
rngBcnDataNew.beacon_ID = index;
@ -927,15 +935,19 @@ void NavEKF3_core::readRngBcnData()
}
// Check if the beacon system has returned a 3D fix
if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) {
Vector3f bp;
float bperr;
if (beacon->get_vehicle_position_ned(bp, bperr)) {
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
}
beaconVehiclePosNED = bp.toftype();
beaconVehiclePosErr = bperr;
// Check if the range beacon data can be used to align the vehicle position
if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) {
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
const ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set
@ -1035,7 +1047,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
extNavDataNew.posReset = false;
}
extNavDataNew.pos = pos;
extNavDataNew.pos = pos.toftype();
extNavDataNew.posErr = posErr;
// calculate timestamp
@ -1052,7 +1064,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
// protect against attitude or angle being NaN
if (!quat.is_nan() && !isnan(angErr)) {
// extract yaw from the attitude
float roll_rad, pitch_rad, yaw_rad;
ftype roll_rad, pitch_rad, yaw_rad;
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
yaw_elements extNavYawAngDataNew;
extNavYawAngDataNew.yawAng = yaw_rad;
@ -1087,7 +1099,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
ext_nav_vel_elements extNavVelNew;
extNavVelNew.time_ms = timeStamp_ms;
extNavVelNew.vel = vel;
extNavVelNew.vel = vel.toftype();
extNavVelNew.err = err;
extNavVelNew.corrected = false;
@ -1246,15 +1258,15 @@ void NavEKF3_core::learnInactiveBiases(void)
// get filtered gyro and use the difference between the
// corrected gyro on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
Vector3f filtered_gyro_active = ins.get_gyro(gyro_index_active) - (stateStruct.gyro_bias/dtEkfAvg);
Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].gyro_bias/dtEkfAvg);
Vector3f error = filtered_gyro_active - filtered_gyro_inactive;
Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg);
Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
// prevent a single large error from contaminating bias estimate
const float bias_limit = radians(5);
error.x = constrain_float(error.x, -bias_limit, bias_limit);
error.y = constrain_float(error.y, -bias_limit, bias_limit);
error.z = constrain_float(error.z, -bias_limit, bias_limit);
const ftype bias_limit = radians(5);
error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
// slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
// gyro bias error in around 1 minute
@ -1275,15 +1287,15 @@ void NavEKF3_core::learnInactiveBiases(void)
// get filtered accel and use the difference between the
// corrected accel on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
Vector3f filtered_accel_active = ins.get_accel(accel_index_active) - (stateStruct.accel_bias/dtEkfAvg);
Vector3f filtered_accel_inactive = ins.get_accel(i) - (inactiveBias[i].accel_bias/dtEkfAvg);
Vector3f error = filtered_accel_active - filtered_accel_inactive;
Vector3F filtered_accel_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg);
Vector3F filtered_accel_inactive = ins.get_accel(i).toftype() - (inactiveBias[i].accel_bias/dtEkfAvg);
Vector3F error = filtered_accel_active - filtered_accel_inactive;
// prevent a single large error from contaminating bias estimate
const float bias_limit = 1.0; // m/s/s
error.x = constrain_float(error.x, -bias_limit, bias_limit);
error.y = constrain_float(error.y, -bias_limit, bias_limit);
error.z = constrain_float(error.z, -bias_limit, bias_limit);
const ftype bias_limit = 1.0; // m/s/s
error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
// slowly bring the inactive accel in line with the active
// accel. This corrects a 0.5 m/s/s accel bias error in
@ -1297,7 +1309,7 @@ void NavEKF3_core::learnInactiveBiases(void)
/*
return declination in radians
*/
float NavEKF3_core::MagDeclination(void) const
ftype NavEKF3_core::MagDeclination(void) const
{
// if we are using the WMM tables then use the table declination
// to ensure consistency with the table mag field. Otherwise use
@ -1327,17 +1339,17 @@ void NavEKF3_core::updateMovementCheck(void)
return;
}
const float gyro_limit = radians(3.0f);
const float gyro_diff_limit = 0.2f;
const float accel_limit = 1.0f;
const float accel_diff_limit = 5.0f;
const float hysteresis_ratio = 0.7f;
const float dtEkfAvgInv = 1.0f / dtEkfAvg;
const ftype gyro_limit = radians(3.0f);
const ftype gyro_diff_limit = 0.2f;
const ftype accel_limit = 1.0f;
const ftype accel_diff_limit = 5.0f;
const ftype hysteresis_ratio = 0.7f;
const ftype dtEkfAvgInv = 1.0f / dtEkfAvg;
// get latest bias corrected gyro and accelerometer data
const auto &ins = dal.ins();
Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv;
Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv;
Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv;
Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv;
if (!prevOnGround) {
gyro_prev = gyro;
@ -1349,7 +1361,7 @@ void NavEKF3_core::updateMovementCheck(void)
}
// calculate a gyro rate of change metric
Vector3f temp = (gyro - gyro_prev) * dtEkfAvgInv;
Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv;
gyro_prev = gyro;
gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();
@ -1358,14 +1370,14 @@ void NavEKF3_core::updateMovementCheck(void)
accel_prev = accel;
accel_diff = 0.99f * accel_diff + 0.01f * temp.length();
const float gyro_length_ratio = gyro.length() / gyro_limit;
const float accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
const float gyro_diff_ratio = gyro_diff / gyro_diff_limit;
const float accel_diff_ratio = accel_diff / accel_diff_limit;
const ftype gyro_length_ratio = gyro.length() / gyro_limit;
const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit;
const ftype accel_diff_ratio = accel_diff / accel_diff_limit;
bool logStatusChange = false;
if (onGroundNotMoving) {
if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
fabsf(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
accel_diff_ratio > frontend->_ognmTestScaleFactor)
{
@ -1373,7 +1385,7 @@ void NavEKF3_core::updateMovementCheck(void)
logStatusChange = true;
}
} else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
fabsf(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
fabsF(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio)
{
@ -1388,10 +1400,10 @@ void NavEKF3_core::updateMovementCheck(void)
time_us : dal.micros64(),
core : core_index,
ongroundnotmoving : onGroundNotMoving,
gyro_length_ratio : gyro_length_ratio,
accel_length_ratio : accel_length_ratio,
gyro_diff_ratio : gyro_diff_ratio,
accel_diff_ratio : accel_diff_ratio,
gyro_length_ratio : float(gyro_length_ratio),
accel_length_ratio : float(accel_length_ratio),
gyro_diff_ratio : float(gyro_diff_ratio),
accel_diff_ratio : float(accel_diff_ratio),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
@ -1401,9 +1413,9 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
{
#if EK3_FEATURE_DRAG_FUSION
// Average and down sample to 5Hz
const float bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_y;
const float mcoef = frontend->_momentumDragCoef.get();
const ftype bcoef_x = frontend->_ballisticCoef_x;
const ftype bcoef_y = frontend->_ballisticCoef_y;
const ftype mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f;

502
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -71,7 +71,7 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri
void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
{
// horizontal velocity squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
// don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits
@ -92,34 +92,34 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
prevPosN = stateStruct.position[0];
prevPosE = stateStruct.position[1];
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms;
// fuse range finder data
if (rangeDataToFuse) {
// predict range
float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
// Copy required states to local variable names
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
float R_RNG = frontend->_rngNoise;
ftype R_RNG = frontend->_rngNoise;
// calculate Kalman gain
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
// Calculate the innovation variance for data logging
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
@ -131,7 +131,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
innovRng = predRngMeas - rangeDataDelayed.rng;
// calculate the innovation consistency test ratio
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
// Check the innovation test ratio and don't fuse if too large
if (auxRngTestRatio < 1.0f) {
@ -152,18 +152,18 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
if (!cantFuseFlowData) {
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2f losPred; // predicted optical flow angular rate measurement
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
float K_OPT;
float H_OPT;
Vector2f auxFlowObsInnovVar;
Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2F losPred; // predicted optical flow angular rate measurement
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
ftype K_OPT;
ftype H_OPT;
Vector2F auxFlowObsInnovVar;
// predict range to centre of image
float flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
// constrain terrain height to be below the vehicle
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
@ -180,20 +180,20 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
// calculate observation jacobians
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = stateStruct.position.z - terrainState;
float t7 = 1.0f / (t6*t6);
float t8 = q0*q3*2.0f;
float t9 = t2-t3-t4+t5;
ftype t2 = q0*q0;
ftype t3 = q1*q1;
ftype t4 = q2*q2;
ftype t5 = q3*q3;
ftype t6 = stateStruct.position.z - terrainState;
ftype t7 = 1.0f / (t6*t6);
ftype t8 = q0*q3*2.0f;
ftype t9 = t2-t3-t4+t5;
// prevent the state variances from becoming badly conditioned
Popt = MAX(Popt,1E-6f);
// calculate observation noise variance from parameter
float flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse Y axis data
@ -207,7 +207,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
// calculate the innovation consistency test ratio
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
// don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.y < 1.0f) {
@ -239,7 +239,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
// calculate the innovation consistency test ratio
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
// don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.x < 1.0f) {
@ -269,23 +269,23 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
{
Vector24 H_LOS;
Vector3f relVelSensor;
Vector3F relVelSensor;
Vector14 SH_LOS;
Vector2 losPred;
// Copy required states to local variable names
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
float vn = stateStruct.velocity.x;
float ve = stateStruct.velocity.y;
float vd = stateStruct.velocity.z;
float pd = stateStruct.position.z;
ftype q0 = stateStruct.quat[0];
ftype q1 = stateStruct.quat[1];
ftype q2 = stateStruct.quat[2];
ftype q3 = stateStruct.quat[3];
ftype vn = stateStruct.velocity.x;
ftype ve = stateStruct.velocity.y;
ftype vd = stateStruct.velocity.z;
ftype pd = stateStruct.position.z;
// constrain height above ground to be above range measured on ground
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
float ptd = pd + heightAboveGndEst;
ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
ftype ptd = pd + heightAboveGndEst;
// Calculate common expressions for observation jacobians
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
@ -306,14 +306,14 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
// calculate range from ground plain to centre of sensor fov assuming flat earth
float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
// correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z;
}
@ -328,7 +328,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
memset(&H_LOS[0], 0, sizeof(H_LOS));
if (obsIndex == 0) {
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
ftype t2 = 1.0f / range;
H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
@ -338,98 +338,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
// calculate intermediate variables for the X observation innovation variance and Kalman gains
float t3 = q1*vd*2.0f;
float t4 = q0*ve*2.0f;
float t11 = q3*vn*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t29 = q1*q2*2.0f;
float t7 = t6-t29;
float t8 = q0*q1*2.0f;
float t9 = q2*q3*2.0f;
float t10 = t8+t9;
float t12 = P[0][0]*t2*t5;
float t13 = q0*vd*2.0f;
float t14 = q2*vn*2.0f;
float t28 = q1*ve*2.0f;
float t15 = t13+t14-t28;
float t16 = q3*vd*2.0f;
float t17 = q2*ve*2.0f;
float t18 = q1*vn*2.0f;
float t19 = t16+t17+t18;
float t20 = q3*ve*2.0f;
float t21 = q0*vn*2.0f;
float t30 = q2*vd*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23-t24+t25-t26;
float t31 = P[1][1]*t2*t15;
float t32 = P[6][0]*t2*t10;
float t33 = P[1][0]*t2*t15;
float t34 = P[2][0]*t2*t19;
float t35 = P[5][0]*t2*t27;
float t79 = P[4][0]*t2*t7;
float t80 = P[3][0]*t2*t22;
float t36 = t12+t32+t33+t34+t35-t79-t80;
float t37 = t2*t5*t36;
float t38 = P[6][1]*t2*t10;
float t39 = P[0][1]*t2*t5;
float t40 = P[2][1]*t2*t19;
float t41 = P[5][1]*t2*t27;
float t81 = P[4][1]*t2*t7;
float t82 = P[3][1]*t2*t22;
float t42 = t31+t38+t39+t40+t41-t81-t82;
float t43 = t2*t15*t42;
float t44 = P[6][2]*t2*t10;
float t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t15;
float t47 = P[2][2]*t2*t19;
float t48 = P[5][2]*t2*t27;
float t83 = P[4][2]*t2*t7;
float t84 = P[3][2]*t2*t22;
float t49 = t44+t45+t46+t47+t48-t83-t84;
float t50 = t2*t19*t49;
float t51 = P[6][3]*t2*t10;
float t52 = P[0][3]*t2*t5;
float t53 = P[1][3]*t2*t15;
float t54 = P[2][3]*t2*t19;
float t55 = P[5][3]*t2*t27;
float t85 = P[4][3]*t2*t7;
float t86 = P[3][3]*t2*t22;
float t56 = t51+t52+t53+t54+t55-t85-t86;
float t57 = P[6][5]*t2*t10;
float t58 = P[0][5]*t2*t5;
float t59 = P[1][5]*t2*t15;
float t60 = P[2][5]*t2*t19;
float t61 = P[5][5]*t2*t27;
float t88 = P[4][5]*t2*t7;
float t89 = P[3][5]*t2*t22;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P[6][4]*t2*t10;
float t65 = P[0][4]*t2*t5;
float t66 = P[1][4]*t2*t15;
float t67 = P[2][4]*t2*t19;
float t68 = P[5][4]*t2*t27;
float t90 = P[4][4]*t2*t7;
float t91 = P[3][4]*t2*t22;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = P[6][6]*t2*t10;
float t71 = P[0][6]*t2*t5;
float t72 = P[1][6]*t2*t15;
float t73 = P[2][6]*t2*t19;
float t74 = P[5][6]*t2*t27;
float t93 = P[4][6]*t2*t7;
float t94 = P[3][6]*t2*t22;
float t75 = t70+t71+t72+t73+t74-t93-t94;
float t76 = t2*t10*t75;
float t87 = t2*t22*t56;
float t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78;
ftype t3 = q1*vd*2.0f;
ftype t4 = q0*ve*2.0f;
ftype t11 = q3*vn*2.0f;
ftype t5 = t3+t4-t11;
ftype t6 = q0*q3*2.0f;
ftype t29 = q1*q2*2.0f;
ftype t7 = t6-t29;
ftype t8 = q0*q1*2.0f;
ftype t9 = q2*q3*2.0f;
ftype t10 = t8+t9;
ftype t12 = P[0][0]*t2*t5;
ftype t13 = q0*vd*2.0f;
ftype t14 = q2*vn*2.0f;
ftype t28 = q1*ve*2.0f;
ftype t15 = t13+t14-t28;
ftype t16 = q3*vd*2.0f;
ftype t17 = q2*ve*2.0f;
ftype t18 = q1*vn*2.0f;
ftype t19 = t16+t17+t18;
ftype t20 = q3*ve*2.0f;
ftype t21 = q0*vn*2.0f;
ftype t30 = q2*vd*2.0f;
ftype t22 = t20+t21-t30;
ftype t23 = q0*q0;
ftype t24 = q1*q1;
ftype t25 = q2*q2;
ftype t26 = q3*q3;
ftype t27 = t23-t24+t25-t26;
ftype t31 = P[1][1]*t2*t15;
ftype t32 = P[6][0]*t2*t10;
ftype t33 = P[1][0]*t2*t15;
ftype t34 = P[2][0]*t2*t19;
ftype t35 = P[5][0]*t2*t27;
ftype t79 = P[4][0]*t2*t7;
ftype t80 = P[3][0]*t2*t22;
ftype t36 = t12+t32+t33+t34+t35-t79-t80;
ftype t37 = t2*t5*t36;
ftype t38 = P[6][1]*t2*t10;
ftype t39 = P[0][1]*t2*t5;
ftype t40 = P[2][1]*t2*t19;
ftype t41 = P[5][1]*t2*t27;
ftype t81 = P[4][1]*t2*t7;
ftype t82 = P[3][1]*t2*t22;
ftype t42 = t31+t38+t39+t40+t41-t81-t82;
ftype t43 = t2*t15*t42;
ftype t44 = P[6][2]*t2*t10;
ftype t45 = P[0][2]*t2*t5;
ftype t46 = P[1][2]*t2*t15;
ftype t47 = P[2][2]*t2*t19;
ftype t48 = P[5][2]*t2*t27;
ftype t83 = P[4][2]*t2*t7;
ftype t84 = P[3][2]*t2*t22;
ftype t49 = t44+t45+t46+t47+t48-t83-t84;
ftype t50 = t2*t19*t49;
ftype t51 = P[6][3]*t2*t10;
ftype t52 = P[0][3]*t2*t5;
ftype t53 = P[1][3]*t2*t15;
ftype t54 = P[2][3]*t2*t19;
ftype t55 = P[5][3]*t2*t27;
ftype t85 = P[4][3]*t2*t7;
ftype t86 = P[3][3]*t2*t22;
ftype t56 = t51+t52+t53+t54+t55-t85-t86;
ftype t57 = P[6][5]*t2*t10;
ftype t58 = P[0][5]*t2*t5;
ftype t59 = P[1][5]*t2*t15;
ftype t60 = P[2][5]*t2*t19;
ftype t61 = P[5][5]*t2*t27;
ftype t88 = P[4][5]*t2*t7;
ftype t89 = P[3][5]*t2*t22;
ftype t62 = t57+t58+t59+t60+t61-t88-t89;
ftype t63 = t2*t27*t62;
ftype t64 = P[6][4]*t2*t10;
ftype t65 = P[0][4]*t2*t5;
ftype t66 = P[1][4]*t2*t15;
ftype t67 = P[2][4]*t2*t19;
ftype t68 = P[5][4]*t2*t27;
ftype t90 = P[4][4]*t2*t7;
ftype t91 = P[3][4]*t2*t22;
ftype t69 = t64+t65+t66+t67+t68-t90-t91;
ftype t70 = P[6][6]*t2*t10;
ftype t71 = P[0][6]*t2*t5;
ftype t72 = P[1][6]*t2*t15;
ftype t73 = P[2][6]*t2*t19;
ftype t74 = P[5][6]*t2*t27;
ftype t93 = P[4][6]*t2*t7;
ftype t94 = P[3][6]*t2*t22;
ftype t75 = t70+t71+t72+t73+t74-t93-t94;
ftype t76 = t2*t10*t75;
ftype t87 = t2*t22*t56;
ftype t92 = t2*t7*t69;
ftype t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
ftype t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 > R_LOS) {
@ -463,8 +463,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -477,8 +477,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
if (!inhibitMagStates) {
@ -489,22 +489,22 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
} else {
// calculate Y axis observation Jacobian
float t2 = 1.0f / range;
ftype t2 = 1.0f / range;
H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
@ -514,98 +514,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
// calculate intermediate variables for the Y observation innovation variance and Kalman gains
float t3 = q3*ve*2.0f;
float t4 = q0*vn*2.0f;
float t11 = q2*vd*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t7 = q1*q2*2.0f;
float t8 = t6+t7;
float t9 = q0*q2*2.0f;
float t28 = q1*q3*2.0f;
float t10 = t9-t28;
float t12 = P[0][0]*t2*t5;
float t13 = q3*vd*2.0f;
float t14 = q2*ve*2.0f;
float t15 = q1*vn*2.0f;
float t16 = t13+t14+t15;
float t17 = q0*vd*2.0f;
float t18 = q2*vn*2.0f;
float t29 = q1*ve*2.0f;
float t19 = t17+t18-t29;
float t20 = q1*vd*2.0f;
float t21 = q0*ve*2.0f;
float t30 = q3*vn*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23+t24-t25-t26;
float t31 = P[1][1]*t2*t16;
float t32 = P[5][0]*t2*t8;
float t33 = P[1][0]*t2*t16;
float t34 = P[3][0]*t2*t22;
float t35 = P[4][0]*t2*t27;
float t80 = P[6][0]*t2*t10;
float t81 = P[2][0]*t2*t19;
float t36 = t12+t32+t33+t34+t35-t80-t81;
float t37 = t2*t5*t36;
float t38 = P[5][1]*t2*t8;
float t39 = P[0][1]*t2*t5;
float t40 = P[3][1]*t2*t22;
float t41 = P[4][1]*t2*t27;
float t82 = P[6][1]*t2*t10;
float t83 = P[2][1]*t2*t19;
float t42 = t31+t38+t39+t40+t41-t82-t83;
float t43 = t2*t16*t42;
float t44 = P[5][2]*t2*t8;
float t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t16;
float t47 = P[3][2]*t2*t22;
float t48 = P[4][2]*t2*t27;
float t79 = P[2][2]*t2*t19;
float t84 = P[6][2]*t2*t10;
float t49 = t44+t45+t46+t47+t48-t79-t84;
float t50 = P[5][3]*t2*t8;
float t51 = P[0][3]*t2*t5;
float t52 = P[1][3]*t2*t16;
float t53 = P[3][3]*t2*t22;
float t54 = P[4][3]*t2*t27;
float t86 = P[6][3]*t2*t10;
float t87 = P[2][3]*t2*t19;
float t55 = t50+t51+t52+t53+t54-t86-t87;
float t56 = t2*t22*t55;
float t57 = P[5][4]*t2*t8;
float t58 = P[0][4]*t2*t5;
float t59 = P[1][4]*t2*t16;
float t60 = P[3][4]*t2*t22;
float t61 = P[4][4]*t2*t27;
float t88 = P[6][4]*t2*t10;
float t89 = P[2][4]*t2*t19;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P[5][5]*t2*t8;
float t65 = P[0][5]*t2*t5;
float t66 = P[1][5]*t2*t16;
float t67 = P[3][5]*t2*t22;
float t68 = P[4][5]*t2*t27;
float t90 = P[6][5]*t2*t10;
float t91 = P[2][5]*t2*t19;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = t2*t8*t69;
float t71 = P[5][6]*t2*t8;
float t72 = P[0][6]*t2*t5;
float t73 = P[1][6]*t2*t16;
float t74 = P[3][6]*t2*t22;
float t75 = P[4][6]*t2*t27;
float t92 = P[6][6]*t2*t10;
float t93 = P[2][6]*t2*t19;
float t76 = t71+t72+t73+t74+t75-t92-t93;
float t85 = t2*t19*t49;
float t94 = t2*t10*t76;
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78;
ftype t3 = q3*ve*2.0f;
ftype t4 = q0*vn*2.0f;
ftype t11 = q2*vd*2.0f;
ftype t5 = t3+t4-t11;
ftype t6 = q0*q3*2.0f;
ftype t7 = q1*q2*2.0f;
ftype t8 = t6+t7;
ftype t9 = q0*q2*2.0f;
ftype t28 = q1*q3*2.0f;
ftype t10 = t9-t28;
ftype t12 = P[0][0]*t2*t5;
ftype t13 = q3*vd*2.0f;
ftype t14 = q2*ve*2.0f;
ftype t15 = q1*vn*2.0f;
ftype t16 = t13+t14+t15;
ftype t17 = q0*vd*2.0f;
ftype t18 = q2*vn*2.0f;
ftype t29 = q1*ve*2.0f;
ftype t19 = t17+t18-t29;
ftype t20 = q1*vd*2.0f;
ftype t21 = q0*ve*2.0f;
ftype t30 = q3*vn*2.0f;
ftype t22 = t20+t21-t30;
ftype t23 = q0*q0;
ftype t24 = q1*q1;
ftype t25 = q2*q2;
ftype t26 = q3*q3;
ftype t27 = t23+t24-t25-t26;
ftype t31 = P[1][1]*t2*t16;
ftype t32 = P[5][0]*t2*t8;
ftype t33 = P[1][0]*t2*t16;
ftype t34 = P[3][0]*t2*t22;
ftype t35 = P[4][0]*t2*t27;
ftype t80 = P[6][0]*t2*t10;
ftype t81 = P[2][0]*t2*t19;
ftype t36 = t12+t32+t33+t34+t35-t80-t81;
ftype t37 = t2*t5*t36;
ftype t38 = P[5][1]*t2*t8;
ftype t39 = P[0][1]*t2*t5;
ftype t40 = P[3][1]*t2*t22;
ftype t41 = P[4][1]*t2*t27;
ftype t82 = P[6][1]*t2*t10;
ftype t83 = P[2][1]*t2*t19;
ftype t42 = t31+t38+t39+t40+t41-t82-t83;
ftype t43 = t2*t16*t42;
ftype t44 = P[5][2]*t2*t8;
ftype t45 = P[0][2]*t2*t5;
ftype t46 = P[1][2]*t2*t16;
ftype t47 = P[3][2]*t2*t22;
ftype t48 = P[4][2]*t2*t27;
ftype t79 = P[2][2]*t2*t19;
ftype t84 = P[6][2]*t2*t10;
ftype t49 = t44+t45+t46+t47+t48-t79-t84;
ftype t50 = P[5][3]*t2*t8;
ftype t51 = P[0][3]*t2*t5;
ftype t52 = P[1][3]*t2*t16;
ftype t53 = P[3][3]*t2*t22;
ftype t54 = P[4][3]*t2*t27;
ftype t86 = P[6][3]*t2*t10;
ftype t87 = P[2][3]*t2*t19;
ftype t55 = t50+t51+t52+t53+t54-t86-t87;
ftype t56 = t2*t22*t55;
ftype t57 = P[5][4]*t2*t8;
ftype t58 = P[0][4]*t2*t5;
ftype t59 = P[1][4]*t2*t16;
ftype t60 = P[3][4]*t2*t22;
ftype t61 = P[4][4]*t2*t27;
ftype t88 = P[6][4]*t2*t10;
ftype t89 = P[2][4]*t2*t19;
ftype t62 = t57+t58+t59+t60+t61-t88-t89;
ftype t63 = t2*t27*t62;
ftype t64 = P[5][5]*t2*t8;
ftype t65 = P[0][5]*t2*t5;
ftype t66 = P[1][5]*t2*t16;
ftype t67 = P[3][5]*t2*t22;
ftype t68 = P[4][5]*t2*t27;
ftype t90 = P[6][5]*t2*t10;
ftype t91 = P[2][5]*t2*t19;
ftype t69 = t64+t65+t66+t67+t68-t90-t91;
ftype t70 = t2*t8*t69;
ftype t71 = P[5][6]*t2*t8;
ftype t72 = P[0][6]*t2*t5;
ftype t73 = P[1][6]*t2*t16;
ftype t74 = P[3][6]*t2*t22;
ftype t75 = P[4][6]*t2*t27;
ftype t92 = P[6][6]*t2*t10;
ftype t93 = P[2][6]*t2*t19;
ftype t76 = t71+t72+t73+t74+t75-t92-t93;
ftype t85 = t2*t19*t49;
ftype t94 = t2*t10*t76;
ftype t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
ftype t78;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 > R_LOS) {
@ -639,8 +639,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -653,8 +653,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
if (!inhibitMagStates) {
@ -665,21 +665,21 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
}
// calculate the innovation consistency test ratio
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {

52
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -23,7 +23,7 @@ bool NavEKF3_core::healthy(void) const
}
// position and height innovations must be within limits when on-ground and in a static mode of operation
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
return false;
}
@ -98,7 +98,7 @@ void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero();
return;
}
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
}
// return accelerometer bias in m/s/s
@ -108,7 +108,7 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
accelBias.zero();
return;
}
accelBias = stateStruct.accel_bias / dtEkfAvg;
accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
}
// return the transformation matrix from XYZ (body) to NED axes
@ -121,7 +121,7 @@ void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF3_core::getQuaternion(Quaternion& ret) const
{
ret = outputDataNew.quat;
ret = outputDataNew.quat.tofloat();
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
@ -136,7 +136,7 @@ uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
{
pos = posResetNE;
pos = posResetNE.tofloat();
return lastPosReset_ms;
}
@ -152,7 +152,7 @@ uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
{
vel = velResetNE;
vel = velResetNE.tofloat();
return lastVelReset_ms;
}
@ -171,7 +171,7 @@ bool NavEKF3_core::getWind(Vector3f &wind) const
void NavEKF3_core::getVelNED(Vector3f &vel) const
{
// correct for the IMU position offset (EKF calculations are at the IMU)
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
}
// return estimate of true airspeed vector in body frame in m/s
@ -181,7 +181,7 @@ bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
if (inhibitWindStates || PV_AidingMode == AID_NONE) {
return false;
}
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
Matrix3f Tnb; // rotation from nav to body frame
@ -218,7 +218,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = gps.location(selected_gps);
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
return false;
@ -369,13 +369,13 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
// return earth magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagNED(Vector3f &magNED) const
{
magNED = stateStruct.earth_magfield * 1000.0f;
magNED = (stateStruct.earth_magfield * 1000.0f).tofloat();
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
{
magXYZ = stateStruct.body_magfield*1000.0f;
magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
}
// return magnetometer offsets
@ -394,7 +394,7 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
!inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
return true;
} else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
@ -440,15 +440,15 @@ void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaIn
// also return the delta in position due to the last position reset
void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtf(velTestRatio);
posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio);
velVar = sqrtF(velTestRatio);
posVar = sqrtF(posTestRatio);
hgtVar = sqrtF(hgtTestRatio);
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio);
offset = posResetNE;
magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtF(tasTestRatio);
offset = posResetNE.tofloat();
}
// get a particular source's velocity innovations
@ -461,8 +461,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
return false;
}
innovations = gpsVelInnov;
variances = gpsVelVarInnov;
innovations = gpsVelInnov.tofloat();
variances = gpsVelVarInnov.tofloat();
return true;
#if EK3_FEATURE_EXTERNAL_NAV
case AP_NavEKF_Source::SourceXY::EXTNAV:
@ -470,8 +470,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
return false;
}
innovations = extNavVelInnov;
variances = extNavVelVarInnov;
innovations = extNavVelInnov.tofloat();
variances = extNavVelVarInnov.tofloat();
return true;
#endif // EK3_FEATURE_EXTERNAL_NAV
default:
@ -565,11 +565,11 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
// range finder is fitted for other applications
float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio);
temp = sqrtF(auxRngTestRatio);
} else {
temp = 0.0f;
}
const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
// send message
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar);

776
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

File diff suppressed because it is too large Load Diff

228
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -42,14 +42,14 @@ void NavEKF3_core::SelectRngBcnFusion()
void NavEKF3_core::FuseRngBcn()
{
// declarations
float pn;
float pe;
float pd;
float bcn_pn;
float bcn_pe;
float bcn_pd;
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
float rngPred;
ftype pn;
ftype pe;
ftype pd;
ftype bcn_pn;
ftype bcn_pe;
ftype bcn_pd;
const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
ftype rngPred;
// health is set bad until test passed
rngBcnHealth = false;
@ -70,7 +70,7 @@ void NavEKF3_core::FuseRngBcn()
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
// predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
rngPred = deltaPosNED.length();
// calculate measurement innovation
@ -80,16 +80,16 @@ void NavEKF3_core::FuseRngBcn()
if (rngPred > 0.1f)
{
// calculate observation jacobians
float H_BCN[24];
ftype H_BCN[24];
memset(H_BCN, 0, sizeof(H_BCN));
float t2 = bcn_pd-pd;
float t3 = bcn_pe-pe;
float t4 = bcn_pn-pn;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9 = 1.0f/sqrtf(t8);
ftype t2 = bcn_pd-pd;
ftype t3 = bcn_pe-pe;
ftype t4 = bcn_pn-pn;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
ftype t9 = 1.0f/sqrtF(t8);
H_BCN[7] = -t4*t9;
H_BCN[8] = -t3*t9;
// If we are not using the beacons as a height reference, we pretend that the beacons
@ -102,23 +102,23 @@ void NavEKF3_core::FuseRngBcn()
H_BCN[9] = -t2*t9;
// calculate Kalman gains
float t10 = P[9][9]*t2*t9;
float t11 = P[8][9]*t3*t9;
float t12 = P[7][9]*t4*t9;
float t13 = t10+t11+t12;
float t14 = t2*t9*t13;
float t15 = P[9][8]*t2*t9;
float t16 = P[8][8]*t3*t9;
float t17 = P[7][8]*t4*t9;
float t18 = t15+t16+t17;
float t19 = t3*t9*t18;
float t20 = P[9][7]*t2*t9;
float t21 = P[8][7]*t3*t9;
float t22 = P[7][7]*t4*t9;
float t23 = t20+t21+t22;
float t24 = t4*t9*t23;
ftype t10 = P[9][9]*t2*t9;
ftype t11 = P[8][9]*t3*t9;
ftype t12 = P[7][9]*t4*t9;
ftype t13 = t10+t11+t12;
ftype t14 = t2*t9*t13;
ftype t15 = P[9][8]*t2*t9;
ftype t16 = P[8][8]*t3*t9;
ftype t17 = P[7][8]*t4*t9;
ftype t18 = t15+t16+t17;
ftype t19 = t3*t9*t18;
ftype t20 = P[9][7]*t2*t9;
ftype t21 = P[8][7]*t3*t9;
ftype t22 = P[7][7]*t4*t9;
ftype t23 = t20+t21+t22;
ftype t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24;
float t26;
ftype t26;
if (varInnovRngBcn >= R_BCN) {
t26 = 1.0f/varInnovRngBcn;
faultStatus.bad_rngbcn = false;
@ -144,8 +144,8 @@ void NavEKF3_core::FuseRngBcn()
Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9);
Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9);
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset(&Kfusion[10], 0, 12);
// zero indexes 10 to 12
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates) {
@ -158,8 +158,8 @@ void NavEKF3_core::FuseRngBcn()
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset(&Kfusion[13], 0, 12);
// zero indexes 13 to 15
zero_range(&Kfusion[0], 13, 15);
}
// only allow the range observations to modify the vertical states if we are using it as a height reference
@ -179,24 +179,24 @@ void NavEKF3_core::FuseRngBcn()
Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9);
Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9);
} else {
// zero indexes 16 to 21 = 6*4 bytes
memset(&Kfusion[16], 0, 24);
// zero indexes 16 to 21
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
} else {
// zero indexes 22 to 23 = 2*4 bytes
memset(&Kfusion[22], 0, 8);
// zero indexes 22 to 23
zero_range(&Kfusion[0], 22, 23);
}
// Calculate innovation using the selected offset value
Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
// fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
@ -283,7 +283,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
void NavEKF3_core::FuseRngBcnStatic()
{
// get the estimated range measurement variance
const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
/*
The first thing to do is to check if we have started the alignment and if not, initialise the
@ -307,7 +307,7 @@ void NavEKF3_core::FuseRngBcnStatic()
}
if (numBcnMeas >= 100) {
rngBcnAlignmentStarted = true;
float tempVar = 1.0f / (float)numBcnMeas;
ftype tempVar = 1.0f / (ftype)numBcnMeas;
// initialise the receiver position to the centre of the beacons and at zero height
receiverPos.x = rngBcnPosSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * tempVar;
@ -336,7 +336,7 @@ void NavEKF3_core::FuseRngBcnStatic()
// position offset to be applied to the beacon system that minimises the range innovations
// The position estimate should be stable after 100 iterations so we use a simple dual
// hypothesis 1-state EKF to estimate the offset
Vector3f refPosNED;
Vector3F refPosNED;
refPosNED.x = receiverPos.x;
refPosNED.y = receiverPos.y;
refPosNED.z = stateStruct.position.z;
@ -355,14 +355,14 @@ void NavEKF3_core::FuseRngBcnStatic()
// and the main EKF vertical position
// Calculate the mid vertical position of all beacons
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
// calculate the delta to the estimated receiver position
float delta = receiverPos.z - bcnMidPosD;
ftype delta = receiverPos.z - bcnMidPosD;
// calcuate the two hypothesis for our vertical position
float receverPosDownMax;
float receverPosDownMin;
ftype receverPosDownMax;
ftype receverPosDownMin;
if (delta >= 0.0f) {
receverPosDownMax = receiverPos.z;
receverPosDownMin = receiverPos.z - 2.0f * delta;
@ -385,62 +385,62 @@ void NavEKF3_core::FuseRngBcnStatic()
}
// calculate the observation jacobian
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
if (t8 < 0.1f) {
// calculation will be badly conditioned
return;
}
float t9 = 1.0f/sqrtf(t8);
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
float t15 = receiverPos.x*2.0f;
float t11 = t10-t15;
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
float t14 = receiverPos.y*2.0f;
float t13 = t12-t14;
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
float t18 = receiverPos.z*2.0f;
float t17 = t16-t18;
float H_RNG[3];
ftype t9 = 1.0f/sqrtF(t8);
ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
ftype t15 = receiverPos.x*2.0f;
ftype t11 = t10-t15;
ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
ftype t14 = receiverPos.y*2.0f;
ftype t13 = t12-t14;
ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
ftype t18 = receiverPos.z*2.0f;
ftype t17 = t16-t18;
ftype H_RNG[3];
H_RNG[0] = -t9*t11*0.5f;
H_RNG[1] = -t9*t13*0.5f;
H_RNG[2] = -t9*t17*0.5f;
// calculate the Kalman gains
float t19 = receiverPosCov[0][0]*t9*t11*0.5f;
float t20 = receiverPosCov[1][1]*t9*t13*0.5f;
float t21 = receiverPosCov[0][1]*t9*t11*0.5f;
float t22 = receiverPosCov[2][1]*t9*t17*0.5f;
float t23 = t20+t21+t22;
float t24 = t9*t13*t23*0.5f;
float t25 = receiverPosCov[1][2]*t9*t13*0.5f;
float t26 = receiverPosCov[0][2]*t9*t11*0.5f;
float t27 = receiverPosCov[2][2]*t9*t17*0.5f;
float t28 = t25+t26+t27;
float t29 = t9*t17*t28*0.5f;
float t30 = receiverPosCov[1][0]*t9*t13*0.5f;
float t31 = receiverPosCov[2][0]*t9*t17*0.5f;
float t32 = t19+t30+t31;
float t33 = t9*t11*t32*0.5f;
ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
ftype t23 = t20+t21+t22;
ftype t24 = t9*t13*t23*0.5f;
ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
ftype t28 = t25+t26+t27;
ftype t29 = t9*t17*t28*0.5f;
ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
ftype t32 = t19+t30+t31;
ftype t33 = t9*t11*t32*0.5f;
varInnovRngBcn = R_RNG+t24+t29+t33;
float t35 = 1.0f/varInnovRngBcn;
float K_RNG[3];
ftype t35 = 1.0f/varInnovRngBcn;
ftype K_RNG[3];
K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
// calculate range measurement innovation
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosOffsetNED.z;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
// fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted);
@ -489,7 +489,7 @@ void NavEKF3_core::FuseRngBcnStatic()
// ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) {
float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
receiverPosCov[i][j] = temp;
receiverPosCov[j][i] = temp;
}
@ -517,7 +517,7 @@ void NavEKF3_core::FuseRngBcnStatic()
Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
*/
void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning)
void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning)
{
// Handle height offsets between the primary height source and the range beacons by estimating
// the beacon systems global vertical position offset using a single state Kalman filter
@ -525,32 +525,32 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
// The main filter then uses the offset with the smaller innovations
float innov; // range measurement innovation (m)
float innovVar; // range measurement innovation variance (m^2)
float gain; // Kalman gain
float obsDeriv; // derivative of observation relative to state
ftype innov; // range measurement innovation (m)
ftype innovVar; // range measurement innovation variance (m^2)
ftype gain; // Kalman gain
ftype obsDeriv; // derivative of observation relative to state
const float stateNoiseVar = 0.1f; // State process noise variance
const float filtAlpha = 0.1f; // LPF constant
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
const ftype stateNoiseVar = 0.1f; // State process noise variance
const ftype filtAlpha = 0.1f; // LPF constant
const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
// estimate upper value for offset
// calculate observation derivative
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
float t5 = t2*t2;
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9;
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
ftype t5 = t2*t2;
ftype t6 = t3*t3;
ftype t7 = t4*t4;
ftype t8 = t5+t6+t7;
ftype t9;
if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8);
t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar;
@ -563,8 +563,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain;
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
ftype stateChange = innov * gain;
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
@ -585,11 +585,11 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
t5 = t2*t2;
t8 = t5+t6+t7;
if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8);
t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// covariance prediction
bcnPosOffsetMinVar += stateNoiseVar;
@ -602,8 +602,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain;
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
ftype stateChange = innov * gain;
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
@ -618,7 +618,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
}
// calculate the mid vertical position of all beacons
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
// ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);

32
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -22,7 +22,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler;
if (gpsGoodToAlign) {
/*
@ -51,9 +51,9 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
const auto &gps = dal.gps();
const struct Location &gpsloc = gps.location(preferred_gps); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
const ftype posFiltTimeConst = 10.0; // time constant used to decay position drift
// calculate time lapsed since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
ftype deltaTime = constrain_ftype(ftype(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
@ -81,8 +81,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsDataNew.have_vz && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else {
gpsVertVelFail = false;
}
@ -91,7 +91,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsVertVelFail) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
@ -102,8 +102,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
@ -240,14 +240,14 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const float tau = 10.0f; // time constant (sec) of peak hold decay
const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const ftype tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms;
}
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms;
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f);
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
@ -256,7 +256,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
}
// filter the raw speed accuracy using a LPF
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
// apply a peak hold filter to the LPF output
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight()
if (assume_zero_sideslip()) {
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
float gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
ftype gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
@ -319,13 +319,13 @@ void NavEKF3_core::detectFlight()
}
// trigger on ground speed
const float gndSpdThresholdSq = sq(5.0f);
const ftype gndSpdThresholdSq = sq(5.0f);
if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) {
highGndSpd = true;
}
// trigger if more than 10m away from initial height
if (fabsf(hgtMea) > 10.0f) {
if (fabsF(hgtMea) > 10.0f) {
largeHgtChange = true;
}

698
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

File diff suppressed because it is too large Load Diff

368
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -81,14 +81,21 @@
#define GSF_YAW_VALID_HISTORY_THRESHOLD 5
// minimum variances allowed for velocity and position states
#define VEL_STATE_MIN_VARIANCE 1E-4f
#define POS_STATE_MIN_VARIANCE 1E-4f
#define VEL_STATE_MIN_VARIANCE 1E-4
#define POS_STATE_MIN_VARIANCE 1E-4
// maximum number of times the vertical velocity variance can hit the lower limit before the
// associated states, variances and covariances are reset
#define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT)
#define EKF_TARGET_RATE_HZ uint32_t(1.0 / EKF_TARGET_DT)
#define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
// limit on horizontal position states
#if HAL_WITH_EKF_DOUBLE
#define EK3_POSXY_STATE_LIMIT 50.0e6
#else
#define EK3_POSXY_STATE_LIMIT 1.0e6
#endif
class NavEKF3_core : public NavEKF_core_common
{
public:
@ -407,7 +414,6 @@ private:
uint8_t imu_buffer_length;
uint8_t obs_buffer_length;
typedef float ftype;
#if MATH_CHECK_INDEXES
typedef VectorN<ftype,2> Vector2;
typedef VectorN<ftype,3> Vector3;
@ -461,14 +467,14 @@ private:
// broken down as individual elements. Both are equivalent (same
// memory)
struct state_elements {
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame 0..3
Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6
Vector3f position; // position of IMU in local NED earth frame (m) 7..9
Vector3f gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12
Vector3f accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15
Vector3f earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
Vector3f body_magfield; // body frame magnetic field vector (Gauss) 19..21
Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame 0..3
Vector3F velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6
Vector3F position; // position of IMU in local NED earth frame (m) 7..9
Vector3F gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12
Vector3F accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15
Vector3F earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
Vector3F body_magfield; // body frame magnetic field vector (Gauss) 19..21
Vector2F wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
};
union {
@ -477,73 +483,73 @@ private:
};
struct output_elements {
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame
Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec)
Vector3f position; // position of body frame origin in local NED earth frame (m)
QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame
Vector3F velocity; // velocity of body frame origin in local NED earth frame (m/sec)
Vector3F position; // position of body frame origin in local NED earth frame (m)
};
struct imu_elements {
Vector3f delAng; // IMU delta angle measurements in body frame (rad)
Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec)
float delAngDT; // time interval over which delAng has been measured (sec)
float delVelDT; // time interval over which delVelDT has been measured (sec)
Vector3F delAng; // IMU delta angle measurements in body frame (rad)
Vector3F delVel; // IMU delta velocity measurements in body frame (m/sec)
ftype delAngDT; // time interval over which delAng has been measured (sec)
ftype delVelDT; // time interval over which delVelDT has been measured (sec)
uint32_t time_ms; // measurement timestamp (msec)
uint8_t gyro_index;
uint8_t accel_index;
};
struct gps_elements : EKF_obs_element_t {
Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
float hgt; // height of the GPS antenna in local NED earth frame (m)
Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
Vector2F pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
ftype hgt; // height of the GPS antenna in local NED earth frame (m)
Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
uint8_t sensor_idx; // unique integer identifying the GPS sensor
bool corrected; // true when the position and velocity have been corrected for sensor position
bool have_vz; // true when vertical velocity is valid
};
struct mag_elements : EKF_obs_element_t {
Vector3f mag; // body frame magnetic field measurements (Gauss)
Vector3F mag; // body frame magnetic field measurements (Gauss)
};
struct baro_elements : EKF_obs_element_t {
float hgt; // height of the pressure sensor in local NED earth frame (m)
ftype hgt; // height of the pressure sensor in local NED earth frame (m)
};
struct range_elements : EKF_obs_element_t {
float rng; // distance measured by the range sensor (m)
ftype rng; // distance measured by the range sensor (m)
uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
};
struct rng_bcn_elements : EKF_obs_element_t {
float rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m)
ftype rng; // range measurement to each beacon (m)
Vector3F beacon_posNED; // NED position of the beacon (m)
ftype rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number
};
struct tas_elements : EKF_obs_element_t {
float tas; // true airspeed measurement (m/sec)
ftype tas; // true airspeed measurement (m/sec)
};
struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m)
Vector2F flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
Vector2F flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m)
};
struct vel_odm_elements : EKF_obs_element_t {
Vector3f vel; // XYZ velocity measured in body frame (m/s)
float velErr; // velocity measurement error 1-std (m/s)
Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m)
Vector3f angRate; // angular rate estimated from odometry (rad/sec)
Vector3F vel; // XYZ velocity measured in body frame (m/s)
ftype velErr; // velocity measurement error 1-std (m/s)
Vector3F body_offset;// XYZ position of the velocity sensor in body frame (m)
Vector3F angRate; // angular rate estimated from odometry (rad/sec)
};
struct wheel_odm_elements : EKF_obs_element_t {
float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
float radius; // wheel radius (m)
Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m)
float delTime; // time interval that the measurement was accumulated over (sec)
ftype delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
ftype radius; // wheel radius (m)
Vector3F hub_offset; // XYZ position of the wheel hub in body frame (m)
ftype 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
@ -553,21 +559,21 @@ private:
};
struct yaw_elements : EKF_obs_element_t {
float yawAng; // yaw angle measurement (rad)
float yawAngErr; // yaw angle 1SD measurement accuracy (rad)
ftype yawAng; // yaw angle measurement (rad)
ftype 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 {
Vector3f pos; // XYZ position measured in a RH navigation frame (m)
float posErr; // spherical position measurement error 1-std (m)
Vector3F pos; // XYZ position measured in a RH navigation frame (m)
ftype posErr; // spherical position measurement error 1-std (m)
bool posReset; // true when the position measurement has been reset
bool corrected; // true when the position has been corrected for sensor position
};
struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m/s)
float err; // velocity measurement error (m/s)
Vector3F vel; // velocity in NED (m/s)
ftype err; // velocity measurement error (m/s)
bool corrected; // true when the velocity has been corrected for sensor position
};
@ -578,8 +584,8 @@ private:
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
Vector3f gyro_bias;
Vector3f accel_bias;
Vector3F gyro_bias;
Vector3F accel_bias;
} inactiveBias[INS_MAX_INSTANCES];
// Specify source of data to be used for a partial state reset
@ -614,7 +620,7 @@ private:
// calculate the predicted state covariance matrix
// Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states
// used to perform a reset of the quaternion state covariances only. Set to null for normal operation.
void CovariancePrediction(Vector3f *rotVarVecPtr);
void CovariancePrediction(Vector3F *rotVarVecPtr);
// force symmetry on the state covariance matrix
void ForceSymmetry();
@ -641,7 +647,7 @@ private:
void FuseRngBcnStatic();
// calculate the offset from EKF vertical position datum to the range beacon system datum
void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning);
// fuse magnetometer measurements
void FuseMagnetometer();
@ -665,21 +671,21 @@ private:
void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate(const Quaternion &deltaQuat);
void StoreQuatRotate(const QuaternionF &deltaQuat);
// calculate the NED earth spin vector in rad/sec
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
void calcEarthRateNED(Vector3F &omega, int32_t latitude) const;
// initialise the covariance matrix
void CovarianceInit();
// helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt);
bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
// helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index);
void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
// update IMU delta angle and delta velocity measurements
void readIMUData();
@ -744,10 +750,10 @@ private:
void ResetPosition(resetDataSource posResetSource);
// reset the stateStruct's NE position to the specified position
void ResetPositionNE(float posN, float posE);
void ResetPositionNE(ftype posN, ftype posE);
// reset the stateStruct's D position
void ResetPositionD(float posD);
void ResetPositionD(ftype posD);
// reset velocity states using the last GPS measurement
void ResetVelocity(resetDataSource velResetSource);
@ -768,7 +774,7 @@ private:
void checkDivergence(void);
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(float K1, float K2);
void calcIMU_Weighting(ftype K1, ftype K2);
// return true if the filter is ready to start using optical flow measurements for position and velocity estimation
bool readyToUseOptFlow(void) const;
@ -847,10 +853,10 @@ private:
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
// Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr);
void FuseDeclination(ftype declErr);
// return magnetic declination in radians
float MagDeclination(void) const;
ftype MagDeclination(void) const;
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
@ -905,7 +911,7 @@ private:
// Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
// Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
// variances argument is updated with variances for each axis
void CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const;
void CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -921,14 +927,14 @@ private:
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle
void resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order);
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw();
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool EKFGSF_getYaw(float &yaw, float &yawVariance) const;
bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;
// Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
void FuseDragForces();
@ -948,7 +954,7 @@ private:
bool badIMUdata; // boolean true if the bad IMU data is detected
uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix
EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
@ -957,10 +963,10 @@ private:
EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
Matrix3F prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3F earthRateNED; // earths angular rate vector in NED (rad/s)
ftype dtIMUavg; // expected time between IMU measurements (sec)
ftype dtEkfAvg; // expected time between EKF updates (sec)
ftype dt; // time lapsed since the last covariance prediction (sec)
@ -976,12 +982,12 @@ private:
bool fuseVelData; // this boolean causes the velNED measurements to be fused
bool fusePosData; // this boolean causes the posNE measurements to be fused
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements
Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
float defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
ftype defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
MagCal effectiveMagCal; // the actual mag calibration being used as the default
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
@ -989,8 +995,8 @@ private:
ftype innovBeta; // synthetic sideslip innovation (rad)
uint32_t lastMagUpdate_us; // last time compass was updated in usec
uint32_t lastMagRead_ms; // last time compass data was successfully read
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
Vector3F velDotNED; // rate of change of velocity in NED frame
Vector3F velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool tasDataToFuse; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms; // time last time we received baro height data
@ -1005,13 +1011,13 @@ private:
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2f lastKnownPositionNE; // last known position
Vector2F lastKnownPositionNE; // last known position
uint32_t lastLaunchAccelTime_ms;
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states are inactive
bool lastInhibitMagStates; // previous inhibitMagStates
@ -1021,15 +1027,15 @@ private:
bool gpsNotAvailable; // bool true when valid GPS data is not available
struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
ftype yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
bool tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw alignment is complete
@ -1038,14 +1044,14 @@ private:
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current time horizon
imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
baro_elements baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon
range_elements rangeDataNew; // Range finder data at the current time horizon
range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
tas_elements tasDataNew; // TAS data at the current time horizon
tas_elements tasDataDelayed; // TAS data at the fusion time horizon
float tasErrVar; // TAS error variance (m/s)**2
ftype tasErrVar; // TAS error variance (m/s)**2
bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
gps_elements gpsDataNew; // GPS data at the current time horizon
@ -1053,10 +1059,10 @@ private:
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // output state data at the current time step
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec)
float innovYaw; // compass yaw angle innovation (rad)
Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
ftype innovYaw; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
@ -1068,94 +1074,94 @@ private:
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
Vector2F velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
ftype posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
bool runUpdates; // boolean true when the EKF updates can be run
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned; // true when the magnetic field has been learned
uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs
float ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
float tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
ftype tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
struct {
float pos;
float vel;
float acc;
ftype pos;
ftype vel;
ftype acc;
} vertCompFiltState;
// variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev; // LLH location of previous GPS measurement
uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
float gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
ftype gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
float sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
ftype sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed
bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
Vector3f gpsVelInnov; // gps velocity innovations
Vector3f gpsVelVarInnov; // gps velocity innovation variances
Vector3F gpsVelInnov; // gps velocity innovations
Vector3F gpsVelVarInnov; // gps velocity innovation variances
uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts)
// variables added for optical flow fusion
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon
bool flowDataValid; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
float Popt; // Optical flow terrain height state covariance (m^2)
float terrainState; // terrain position state (m)
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float varInnovRng; // range finder observation innovation variance (m^2)
float innovRng; // range finder observation innovation (m)
float hgtMea; // height measurement derived from either baro, gps or range finder data (m)
ftype Popt; // Optical flow terrain height state covariance (m^2)
ftype terrainState; // terrain position state (m)
ftype prevPosN; // north position at last measurement
ftype prevPosE; // east position at last measurement
ftype varInnovRng; // range finder observation innovation variance (m^2)
ftype innovRng; // range finder observation innovation (m)
ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
bool inhibitGndState; // true when the terrain position state is to remain constant
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2
ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2F flowGyroBias; // bias error of optical flow sensor gyro output
bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
@ -1167,16 +1173,16 @@ private:
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
bool gndOffsetValid; // true when the ground offset state can still be considered valid
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF; // time that delAngBodyOF is summed across
Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
ftype delTimeOF; // time that delAngBodyOF is summed across
bool flowFusionActive; // true when optical flow fusion is active
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m)
Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
// Range finder
float baroHgtOffset; // offset applied when when switching to use of Baro height
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
ftype baroHgtOffset; // offset applied when when switching to use of Baro height
ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
ftype storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors
@ -1214,49 +1220,49 @@ private:
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
float varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m)
ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
ftype innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec)
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr; // estimated position error from the beacon system (m)
Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter
float receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started
bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished
uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter
Vector3f rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
float rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum position of all beacons in the down direction (m)
ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
ftype minBcnPosD; // minimum position of all beacons in the down direction (m)
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
float maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
ftype bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
float minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
ftype bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
Vector3F bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
bool bcnOriginEstInit; // True when the beacon origin has been initialised
// Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m)
float innov; // range innovation (m)
float innovVar; // innovation variance (m^2)
float testRatio; // innovation consistency test ratio
Vector3f beaconPosNED; // beacon NED position
ftype rng; // measured range to beacon (m)
ftype innov; // range innovation (m)
ftype innovVar; // innovation variance (m^2)
ftype testRatio; // innovation consistency test ratio
Vector3F beaconPosNED; // beacon NED position
} *rngBcnFusionReport;
#if EK3_FEATURE_DRAG_FUSION
@ -1265,10 +1271,10 @@ private:
drag_elements dragSampleDelayed;
drag_elements dragDownSampled; // down sampled from filter prediction rate to observation rate
uint8_t dragSampleCount; // number of drag specific force samples accumulated at the filter prediction rate
float dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
Vector2f innovDrag; // multirotor drag measurement innovation (m/sec**2)
Vector2f innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
Vector2f dragTestRatio; // drag innovation consistency check ratio
ftype dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
Vector2F innovDrag; // multirotor drag measurement innovation (m/sec**2)
Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
Vector2F dragTestRatio; // drag innovation consistency check ratio
#endif
bool dragFusionEnabled;
@ -1278,11 +1284,11 @@ private:
// Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
float rngAtStartOfFlight; // range finder measurement at start of flight
ftype rngAtStartOfFlight; // range finder measurement at start of flight
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// baro ground effect
float meaHgtAtTakeOff; // height measured at commencement of takeoff
ftype meaHgtAtTakeOff; // height measured at commencement of takeoff
// control of post takeoff magnetic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
@ -1291,22 +1297,22 @@ private:
bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset
ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset
// Used by on ground movement check required when operating on ground without a yaw reference
float gyro_diff; // filtered gyro difference (rad/s)
float accel_diff; // filtered acceerometer difference (m/s/s)
Vector3f gyro_prev; // gyro vector from previous time step (rad/s)
Vector3f accel_prev; // accelerometer vector from previous time step (m/s/s)
ftype gyro_diff; // filtered gyro difference (rad/s)
ftype accel_diff; // filtered acceerometer difference (m/s/s)
Vector3F gyro_prev; // gyro vector from previous time step (rad/s)
Vector3F accel_prev; // accelerometer vector from previous time step (m/s/s)
bool onGroundNotMoving; // true when on the ground and not moving
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
// variables used to inhibit accel bias learning
bool inhibitDelVelBiasStates; // true when all IMU delta velocity bias states are de-activated
bool dvelBiasAxisInhibit[3] {}; // true when IMU delta velocity bias states for a specific axis is de-activated
Vector3f dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2
Vector3F dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2
#if EK3_FEATURE_EXTERNAL_NAV
// external navigation fusion
@ -1320,8 +1326,8 @@ private:
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
Vector3f extNavVelInnov; // external nav velocity innovations
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
Vector3F extNavVelInnov; // external nav velocity innovations
Vector3F extNavVelVarInnov; // external nav velocity innovation variances
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
@ -1384,8 +1390,8 @@ private:
ftype magXbias;
ftype magYbias;
ftype magZbias;
Matrix3f DCM;
Vector3f MagPred;
Matrix3F DCM;
Vector3F MagPred;
ftype R_MAG;
Vector9 SH_MAG;
} mag_state;
@ -1395,8 +1401,8 @@ private:
// earth field from WMM tables
bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga; // earth field from WMM tables
float table_declination; // declination in radians from the tables
Vector3F table_earth_field_ga; // earth field from WMM tables
ftype table_declination; // declination in radians from the tables
// timing statistics
struct ekf_timing timing;
@ -1408,7 +1414,7 @@ private:
bool assume_zero_sideslip(void) const;
// vehicle specific initial gyro bias uncertainty
float InitialGyroBiasUncertainty(void) const;
ftype InitialGyroBiasUncertainty(void) const;
/*
learn magnetometer biases from GPS yaw. Return true if the