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 committed by Randy Mackay
parent 601eb12efb
commit 4f0f4608b9
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