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

AP_NavEKF3: allow for double EKF build

This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00
parent 380b9b39ed
commit a6c5b65f4a
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 // 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)) { if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
coreRelativeErrors[i] += error; 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 (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // 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 // which assumes the vehicle has launched into the wind
Vector3f tempEuler; Vector3F tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
// set the wind state variances to the measurement uncertainty // set the wind state variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) { 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 { } else {
// set the variances using a typical wind speed // 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 // keep the IMU bias state variances, but zero the covariances
float oldBiasVariance[6]; ftype oldBiasVariance[6];
for (uint8_t row=0; row<6; row++) { for (uint8_t row=0; row<6; row++) {
oldBiasVariance[row] = P[row+10][row+10]; oldBiasVariance[row] = P[row+10][row+10];
} }
@ -407,7 +407,7 @@ void NavEKF3_core::setAidingMode()
} }
// handle height reset as special case // handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z; 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(); ResetHeight();
#endif // EK3_FEATURE_EXTERNAL_NAV #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 // Once the tilt variances have reduced, re-set the yaw and magnetic field states
// and declare the tilt alignment complete // and declare the tilt alignment complete
if (!tiltAlignComplete) { if (!tiltAlignComplete) {
if (tiltErrorVariance < sq(radians(5.0f))) { if (tiltErrorVariance < sq(radians(5.0))) {
tiltAlignComplete = true; tiltAlignComplete = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index); 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) void NavEKF3_core::checkGyroCalStatus(void)
{ {
// check delta angle bias variances // 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(); 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) && 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)) { (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 // 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 // which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]); Vector3F delAngBiasVarVec = Vector3F(P[10][10],P[11][11],P[12][12]);
Vector3f temp = prevTnb * delAngBiasVarVec; Vector3F temp = prevTnb * delAngBiasVarVec;
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) && delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
(fabsf(temp.y) < delAngBiasVarMax); (fabsF(temp.y) < delAngBiasVarMax);
} else { } else {
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) && delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
(P[11][11] <= delAngBiasVarMax) && (P[11][11] <= delAngBiasVarMax) &&
@ -697,7 +697,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
return; return;
} }
float trueAirspeed; ftype trueAirspeed;
if (assume_zero_sideslip()) { if (assume_zero_sideslip()) {
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f); trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
} else { } else {
@ -720,13 +720,13 @@ void NavEKF3_core::runYawEstimatorCorrection()
if (EKFGSF_run_filterbank) { if (EKFGSF_run_filterbank) {
if (gpsDataToFuse) { if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y); Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise); ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc); yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
// after velocity data has been fused the yaw variance estimate will have been refreshed and // after velocity data has been fused the yaw variance estimate will have been refreshed and
// is used maintain a history of validity // is used maintain a history of validity
float gsfYaw, gsfYawVariance; ftype gsfYaw, gsfYawVariance;
if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) { if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) { if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
EKFGSF_yaw_valid_count++; 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 vehicle specific initial gyro bias uncertainty in deg/sec
*/ */
float NavEKF3_core::InitialGyroBiasUncertainty(void) const ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const
{ {
return 2.5f; return 2.5f;
} }

12
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -6,6 +6,8 @@
#include <AP_DAL/AP_DAL.h> #include <AP_DAL/AP_DAL.h>
#pragma GCC diagnostic ignored "-Wnarrowing"
void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
{ {
// Write first EKF packet // Write first EKF packet
@ -147,7 +149,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); 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); getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus); getFilterStatus(solutionStatus);
const struct log_NKF4 pkt4{ 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), sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar), sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar), 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, offsetNorth : offset.x,
offsetEast : offset.y, offsetEast : offset.y,
faults : _faultStatus, 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 offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*innovRng), // range finder innovations RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range 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 angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)outputTrackError.y, // output predictor velocity error velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)outputTrackError.z // output predictor position tracking 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, ID : rngBcnFuseDataReportIndex,
rng : (int16_t)(100*report.rng), rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov), innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)), sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)), testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*report.beaconPosNED.x), beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y), beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z), 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 // Quaternion and delta rotation vector that are re-used for different calculations
Vector3f deltaRotVecTemp; Vector3F deltaRotVecTemp;
Quaternion deltaQuatTemp; QuaternionF deltaQuatTemp;
bool flightResetAllowed = false; bool flightResetAllowed = false;
bool initialResetAllowed = false; bool initialResetAllowed = false;
@ -61,7 +61,7 @@ void NavEKF3_core::controlMagYawReset()
// check for increasing height // check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; 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 // check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f; bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
@ -131,18 +131,18 @@ void NavEKF3_core::controlMagYawReset()
void NavEKF3_core::realignYawGPS() void NavEKF3_core::realignYawGPS()
{ {
// get quaternion from existing filter states and calculate roll, pitch and yaw angles // 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); stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) { if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
// calculate course yaw angle // 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 // 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 // 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 // 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; 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) void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
{ {
// update quaternion states and covariances // 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 // send yaw alignment information to console
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index); 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 // Store yaw angle when moving for use as a static reference when not moving
if (!onGroundNotMoving) { 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 // 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.order = rotationOrder::TAIT_BRYAN_321;
yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]); yawAngDataStatic.yawAng = atan2F(prevTnb[0][1], prevTnb[0][0]);
} else { } else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis // 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.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.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
yawAngDataStatic.time_ms = imuDataDelayed.time_ms; yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
@ -236,7 +236,7 @@ void NavEKF3_core::SelectMagFusion()
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) { 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 // 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 // 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); const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
// fallback methods // fallback methods
@ -453,8 +453,8 @@ void NavEKF3_core::FuseMagnetometer()
ftype &magXbias = mag_state.magXbias; ftype &magXbias = mag_state.magXbias;
ftype &magYbias = mag_state.magYbias; ftype &magYbias = mag_state.magYbias;
ftype &magZbias = mag_state.magZbias; ftype &magZbias = mag_state.magZbias;
Matrix3f &DCM = mag_state.DCM; Matrix3F &DCM = mag_state.DCM;
Vector3f &MagPred = mag_state.MagPred; Vector3F &MagPred = mag_state.MagPred;
ftype &R_MAG = mag_state.R_MAG; ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0]; ftype *SH_MAG = &mag_state.SH_MAG[0];
Vector24 H_MAG; Vector24 H_MAG;
@ -503,7 +503,7 @@ void NavEKF3_core::FuseMagnetometer()
} }
// scale magnetometer observation error with total angular rate to allow for timing errors // 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 // 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; 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 // calculate the innovation test ratios
for (uint8_t i = 0; i<=2; i++) { 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 // 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[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]); 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 { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -619,8 +619,8 @@ void NavEKF3_core::FuseMagnetometer()
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
// zero Kalman gains to inhibit magnetic field state estimation // zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) { 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[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]); 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 { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
// zero Kalman gains to inhibit wind state estimation // 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[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]); 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 { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23 = 2
memset(&Kfusion[22], 0, 8); 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 // 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[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]); 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 { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -699,8 +699,8 @@ void NavEKF3_core::FuseMagnetometer()
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
// zero Kalman gains to inhibit magnetic field state estimation // 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[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]); 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 { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
// zero Kalman gains to inhibit wind state estimation // 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[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]); 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 { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); 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 // 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[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]); 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 { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -781,8 +781,8 @@ void NavEKF3_core::FuseMagnetometer()
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
// zero Kalman gains to inhibit magnetic field state estimation // 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[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]); 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 { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
// zero Kalman gains to inhibit wind state estimation // 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[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]); 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 { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); 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 // 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) bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
{ {
const float &q0 = stateStruct.quat[0]; const ftype &q0 = stateStruct.quat[0];
const float &q1 = stateStruct.quat[1]; const ftype &q1 = stateStruct.quat[1];
const float &q2 = stateStruct.quat[2]; const ftype &q2 = stateStruct.quat[2];
const float &q3 = stateStruct.quat[3]; const ftype &q3 = stateStruct.quat[3];
float gsfYaw, gsfYawVariance; ftype gsfYaw, gsfYawVariance;
if (method == yawFusionMethod::GSF) { if (method == yawFusionMethod::GSF) {
if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) { if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
return false; return false;
@ -910,7 +910,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} }
// yaw measurement error variance (rad^2) // yaw measurement error variance (rad^2)
float R_YAW; ftype R_YAW;
switch (method) { switch (method) {
case yawFusionMethod::GPS: case yawFusionMethod::GPS:
R_YAW = sq(yawAngDataDelayed.yawAngErr); R_YAW = sq(yawAngDataDelayed.yawAngErr);
@ -953,7 +953,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
case yawFusionMethod::PREDICTED: case yawFusionMethod::PREDICTED:
default: default:
// determined automatically // 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; break;
#if EK3_FEATURE_EXTERNAL_NAV #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 // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
float yawAngPredicted; ftype yawAngPredicted;
float H_YAW[4]; ftype H_YAW[4];
Matrix3f Tbn_zeroYaw; Matrix3F Tbn_zeroYaw;
if (order == rotationOrder::TAIT_BRYAN_321) { if (order == rotationOrder::TAIT_BRYAN_321) {
// calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw // calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false; bool canUseA = false;
const float SA0 = 2*q3; const ftype SA0 = 2*q3;
const float SA1 = 2*q2; const ftype SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1; const ftype SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv; ftype SA4, SA5_inv;
if (is_positive(sq(SA3))) { if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3); SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1; SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv)); canUseA = is_positive(fabsF(SA5_inv));
} }
bool canUseB = false; bool canUseB = false;
const float SB0 = 2*q0; const ftype SB0 = 2*q0;
const float SB1 = 2*q1; const ftype SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2; const ftype SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv; ftype SB3, SB5_inv;
if (is_positive(sq(SB2))) { if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2); SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1; 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))) { if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv; const ftype SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3; const ftype SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4; const ftype SA7 = SA2*SA4;
const float SA8 = 2*SA7; const ftype SA8 = 2*SA7;
const float SA9 = 2*SA6; const ftype SA9 = 2*SA6;
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
H_YAW[1] = SA5*(SA1*SA6 - SA8*q1); H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
H_YAW[2] = SA5*(SA1*SA7 + SA9*q1); H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0); H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv; const ftype SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2; const ftype SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4; const ftype SB7 = SB3*SB4;
const float SB8 = 2*SB7; const ftype SB8 = 2*SB7;
const float SB9 = 2*SB6; const ftype SB9 = 2*SB6;
H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3); H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2); H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
@ -1021,7 +1021,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} }
// Get the 321 euler angles // Get the 321 euler angles
Vector3f euler321; Vector3F euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
yawAngPredicted = euler321.z; yawAngPredicted = euler321.z;
@ -1031,46 +1031,46 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} else if (order == rotationOrder::TAIT_BRYAN_312) { } 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 // calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false; bool canUseA = false;
const float SA0 = 2*q3; const ftype SA0 = 2*q3;
const float SA1 = 2*q2; const ftype SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1; const ftype SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv; ftype SA4, SA5_inv;
if (is_positive(sq(SA3))) { if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3); SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1; SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv)); canUseA = is_positive(fabsF(SA5_inv));
} }
bool canUseB = false; bool canUseB = false;
const float SB0 = 2*q0; const ftype SB0 = 2*q0;
const float SB1 = 2*q1; const ftype SB1 = 2*q1;
const float SB2 = -SB0*q3 + SB1*q2; const ftype SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv; ftype SB3, SB5_inv;
if (is_positive(sq(SB2))) { if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2); SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1; 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))) { if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv; const ftype SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3; const ftype SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4; const ftype SA7 = SA2*SA4;
const float SA8 = 2*SA7; const ftype SA8 = 2*SA7;
const float SA9 = 2*SA6; const ftype SA9 = 2*SA6;
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0); H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1); H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1); H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0); H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv; const ftype SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2; const ftype SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4; const ftype SB7 = SB3*SB4;
const float SB8 = 2*SB7; const ftype SB8 = 2*SB7;
const float SB9 = 2*SB6; const ftype SB9 = 2*SB6;
H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3); H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2); 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 // Get the 312 Tait Bryan rotation angles
Vector3f euler312 = stateStruct.quat.to_vector312(); Vector3F euler312 = stateStruct.quat.to_vector312();
yawAngPredicted = euler312.z; yawAngPredicted = euler312.z;
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame // 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 // Use the difference between the horizontal projection and declination to give the measured yaw
// rotate measured mag components into earth frame // rotate measured mag components into earth frame
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured); innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured);
break; 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 // 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]; ftype PH[4];
float varInnov = R_YAW; ftype varInnov = R_YAW;
for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) { for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) {
PH[rowIndex] = 0.0f; PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0; colIndex<=3; colIndex++) { for (uint8_t colIndex=0; colIndex<=3; colIndex++) {
@ -1137,7 +1137,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} }
varInnov += H_YAW[rowIndex]*PH[rowIndex]; varInnov += H_YAW[rowIndex]*PH[rowIndex];
} }
float varInnovInv; ftype varInnovInv;
if (varInnov >= R_YAW) { if (varInnov >= R_YAW) {
varInnovInv = 1.0f / varInnov; varInnovInv = 1.0f / varInnov;
// output numerical health status // output numerical health status
@ -1161,7 +1161,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
} }
// calculate the innovation test ratio // 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 // Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) { 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 row = 0; row <= stateIndexLim; row++) {
for (uint8_t column = 0; column <= stateIndexLim; column++) { 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][1] * P[1][column];
tmp += KH[row][2] * P[2][column]; tmp += KH[row][2] * P[2][column];
tmp += KH[row][3] * P[3][column]; tmp += KH[row][3] * P[3][column];
@ -1213,7 +1213,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
// correct the state vector // correct the state vector
for (uint8_t i=0; i<=stateIndexLim; i++) { 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(); 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 * 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 * 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) // declination error variance (rad^2)
const float R_DECL = sq(declErr); const ftype R_DECL = sq(declErr);
// copy required states to local variables // copy required states to local variables
float magN = stateStruct.earth_magfield.x; ftype magN = stateStruct.earth_magfield.x;
float magE = stateStruct.earth_magfield.y; ftype magE = stateStruct.earth_magfield.y;
// prevent bad earth field states from causing numerical errors or exceptions // prevent bad earth field states from causing numerical errors or exceptions
if (magN < 1e-3f) { if (magN < 1e-3f) {
@ -1250,34 +1250,34 @@ void NavEKF3_core::FuseDeclination(float declErr)
// Calculate observation Jacobian and Kalman gains // Calculate observation Jacobian and Kalman gains
// Calculate intermediate variables // Calculate intermediate variables
float t2 = magE*magE; ftype t2 = magE*magE;
float t3 = magN*magN; ftype t3 = magN*magN;
float t4 = t2+t3; ftype t4 = t2+t3;
// if the horizontal magnetic field is too small, this calculation will be badly conditioned // if the horizontal magnetic field is too small, this calculation will be badly conditioned
if (t4 < 1e-4f) { if (t4 < 1e-4f) {
return; return;
} }
float t5 = P[16][16]*t2; ftype t5 = P[16][16]*t2;
float t6 = P[17][17]*t3; ftype t6 = P[17][17]*t3;
float t7 = t2*t2; ftype t7 = t2*t2;
float t8 = R_DECL*t7; ftype t8 = R_DECL*t7;
float t9 = t3*t3; ftype t9 = t3*t3;
float t10 = R_DECL*t9; ftype t10 = R_DECL*t9;
float t11 = R_DECL*t2*t3*2.0f; ftype t11 = R_DECL*t2*t3*2.0f;
float t14 = P[16][17]*magE*magN; ftype t14 = P[16][17]*magE*magN;
float t15 = P[17][16]*magE*magN; ftype t15 = P[17][16]*magE*magN;
float t12 = t5+t6+t8+t10+t11-t14-t15; ftype t12 = t5+t6+t8+t10+t11-t14-t15;
float t13; ftype t13;
if (fabsf(t12) > 1e-6f) { if (fabsF(t12) > 1e-6f) {
t13 = 1.0f / t12; t13 = 1.0f / t12;
} else { } else {
return; return;
} }
float t18 = magE*magE; ftype t18 = magE*magE;
float t19 = magN*magN; ftype t19 = magN*magN;
float t20 = t18+t19; ftype t20 = t18+t19;
float t21; ftype t21;
if (fabsf(t20) > 1e-6f) { if (fabsF(t20) > 1e-6f) {
t21 = 1.0f/t20; t21 = 1.0f/t20;
} else { } else {
return; return;
@ -1285,7 +1285,7 @@ void NavEKF3_core::FuseDeclination(float declErr)
// Calculate the observation Jacobian // 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 // 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[16] = -magE*t21;
H_DECL[17] = magN*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[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
} else { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -1319,8 +1319,8 @@ void NavEKF3_core::FuseDeclination(float declErr)
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
if (!inhibitMagStates) { 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[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
} else { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
if (!inhibitWindStates) { if (!inhibitWindStates) {
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
} else { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); zero_range(&Kfusion[0], 22, 23);
} }
// get the magnetic declination // get the magnetic declination
float magDecAng = MagDeclination(); ftype magDecAng = MagDeclination();
// Calculate the innovation // Calculate the innovation
float innovation = atan2f(magE , magN) - magDecAng; ftype innovation = atan2F(magE , magN) - magDecAng;
// limit the innovation to protect against data errors // limit the innovation to protect against data errors
if (innovation > 0.5f) { if (innovation > 0.5f) {
@ -1422,18 +1422,18 @@ void NavEKF3_core::alignMagStateDeclination()
} }
// get the magnetic declination // get the magnetic declination
float magDecAng = MagDeclination(); ftype magDecAng = MagDeclination();
// rotate the NE values so that the declination matches the published value // rotate the NE values so that the declination matches the published value
Vector3f initMagNED = stateStruct.earth_magfield; Vector3F initMagNED = stateStruct.earth_magfield;
float magLengthNE = norm(initMagNED.x,initMagNED.y); ftype magLengthNE = norm(initMagNED.x,initMagNED.y);
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng); stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng); stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
if (!inhibitMagStates) { if (!inhibitMagStates) {
// zero the corresponding state covariances if magnetic field state learning is active // zero the corresponding state covariances if magnetic field state learning is active
float var_16 = P[16][16]; ftype var_16 = P[16][16];
float var_17 = P[17][17]; ftype var_17 = P[17][17];
zeroRows(P,16,17); zeroRows(P,16,17);
zeroCols(P,16,17); zeroCols(P,16,17);
P[16][16] = var_16; P[16][16] = var_16;
@ -1484,13 +1484,13 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
} }
// combine yaw with current quaternion to get yaw corrected quaternion // 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) { if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) {
Vector3f euler321; Vector3F euler321;
quat.to_euler(euler321.x, euler321.y, euler321.z); quat.to_euler(euler321.x, euler321.y, euler321.z);
quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng); quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng);
} else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) { } 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); quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng);
} else { } else {
// rotation order not supported // rotation order not supported
@ -1498,19 +1498,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
} }
// build the expected body field from orientation and table earth field // build the expected body field from orientation and table earth field
Matrix3f dcm; Matrix3F dcm;
quat.rotation_matrix(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 // 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 // learn body frame mag biases
stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE; stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE;
// check if error is below threshold. If it is then we can // check if error is below threshold. If it is then we can
// fallback to magnetometer on failure of external yaw // 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 // we allow for yaw backback to compass if we have had 50 samples
// in a row below the threshold. This corresponds to 10 seconds // in a row below the threshold. This corresponds to 10 seconds
@ -1540,7 +1540,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
return false; return false;
}; };
float yawEKFGSF, yawVarianceEKFGSF; ftype yawEKFGSF, yawVarianceEKFGSF;
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) { if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
// keep roll and pitch and reset yaw // keep roll and pitch and reset yaw
rotationOrder order; rotationOrder order;
@ -1582,14 +1582,14 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
} }
// returns true on success and populates yaw (in radians) and yawVariance (rad^2) // 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 // return immediately if no yaw estimator
if (yawEstimator == nullptr) { if (yawEstimator == nullptr) {
return false; return false;
} }
float velInnovLength; ftype velInnovLength;
if (yawEstimator->getYawData(yaw, yawVariance) && if (yawEstimator->getYawData(yaw, yawVariance) &&
is_positive(yawVariance) && is_positive(yawVariance) &&
yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) && yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
@ -1600,14 +1600,14 @@ bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const
return false; 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 // check if we should use a 321 or 312 Rotation order and update the quaternion
// states using the preferred yaw definition // states using the preferred yaw definition
stateStruct.quat.inverse().rotation_matrix(prevTnb); stateStruct.quat.inverse().rotation_matrix(prevTnb);
Vector3f eulerAngles; Vector3F eulerAngles;
if (order == rotationOrder::TAIT_BRYAN_321) { if (order == rotationOrder::TAIT_BRYAN_321) {
// rolled more than pitched so use 321 rotation order // rolled more than pitched so use 321 rotation order
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); 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 // Update the rotation matrix
stateStruct.quat.inverse().rotation_matrix(prevTnb); 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 // 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); StoreQuatRotate(quat_delta);
// assume tilt uncertainty split equally between roll and pitch // 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); CovariancePrediction(&angleErrVarVec);
// record the yaw reset event // 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 // subtract delay from timestamp
timeStamp_ms -= delay_ms; timeStamp_ms -= delay_ms;
bodyOdmDataNew.body_offset = posOffset; bodyOdmDataNew.body_offset = posOffset.toftype();
bodyOdmDataNew.vel = delPos * (1.0f/delTime); bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms; bodyOdmDataNew.time_ms = timeStamp_ms;
bodyOdmDataNew.angRate = delAng * (1.0f/delTime); bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
bodyOdmMeasTime_ms = timeStamp_ms; bodyOdmMeasTime_ms = timeStamp_ms;
// simple model of accuracy // simple model of accuracy
@ -157,7 +157,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
} }
wheel_odm_elements wheelOdmDataNew = {}; wheel_odm_elements wheelOdmDataNew = {};
wheelOdmDataNew.hub_offset = posOffset; wheelOdmDataNew.hub_offset = posOffset.toftype();
wheelOdmDataNew.delAng = delAng; wheelOdmDataNew.delAng = delAng;
wheelOdmDataNew.radius = radius; wheelOdmDataNew.radius = radius;
wheelOdmDataNew.delTime = delTime; 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 // reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement // don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) { if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/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_float((rawGyroRates.y - delAngBodyOF.y/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(); delAngBodyOF.zero();
delTimeOF = 0.0f; delTimeOF = 0.0f;
} }
@ -219,9 +219,9 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
} }
// write uncorrected flow rate measurements // write uncorrected flow rate measurements
// note correction for different axis and sign conventions used by the px4flow sensor // 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 // 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 // write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; 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)) { ((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
// detect changes to magnetometer offset parameters and reset states // 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); bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) { if (changeDetected) {
// zero the learned magnetometer bias states // zero the learned magnetometer bias states
@ -357,7 +357,7 @@ void NavEKF3_core::readMagData()
magDataNew.time_ms -= localFilterTimeStep_ms/2; magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning // 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 // check for consistent data between magnetometers
consistentMagData = compass.consistent(); consistentMagData = compass.consistent();
@ -386,7 +386,7 @@ void NavEKF3_core::readIMUData()
const auto &ins = dal.ins(); const auto &ins = dal.ins();
// calculate an averaged IMU update rate using a spike and lowpass filter combination // 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 // the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000; imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
@ -427,7 +427,7 @@ void NavEKF3_core::readIMUData()
updateMovementCheck(); updateMovementCheck();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT); 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; imuDataNew.accel_index = accel_index_active;
// Get delta angle data from primary gyro or primary if not available // Get delta angle data from primary gyro or primary if not available
@ -454,7 +454,7 @@ void NavEKF3_core::readIMUData()
imuQuatDownSampleNew.normalize(); imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into body frame at the start of accumulation // Rotate the latest delta velocity into body frame at the start of accumulation
Matrix3f deltaRotMat; Matrix3F deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat); imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator // Apply the delta velocity to the delta velocity accumulator
@ -482,7 +482,7 @@ void NavEKF3_core::readIMUData()
storedIMU.push_youngest_element(imuDataDownSampledNew); storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF using a combination spike and LPF // 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; dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// do an addtional down sampling for data used to sample XY body frame drag specific forces // 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(); imuDataDelayed = storedIMU.get_oldest_element();
// protect against delta time going to zero // protect against delta time going to zero
float minDT = 0.1f * dtEkfAvg; ftype minDT = 0.1f * dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,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 // read the delta velocity and corresponding time interval from the IMU
// return false if data is not available // 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(); const auto &ins = dal.ins();
if (ins_index < ins.get_accel_count()) { if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel,dVel_dt); Vector3f dVelF;
dVel_dt = MAX(dVel_dt,1.0e-4f); 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 true;
} }
return false; return false;
@ -585,7 +589,7 @@ void NavEKF3_core::readGpsData()
gpsDataNew.sensor_idx = selected_gps; gpsDataNew.sensor_idx = selected_gps;
// read the NED velocity from the 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); gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
// position and velocity are not yet corrected for sensor position // 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. // 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 // 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); gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw; float gpsSpdAccRaw;
if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) { if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
@ -686,7 +690,7 @@ void NavEKF3_core::readGpsData()
if (gpsGoodToAlign && !have_table_earth_field) { if (gpsGoodToAlign && !have_table_earth_field) {
const auto *compass = dal.get_compass(); const auto *compass = dal.get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { 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, table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7)); gpsloc.lng*1.0e-7));
have_table_earth_field = true; 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 // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) { 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) { 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 { } else {
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt); 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 // accuracy. Set to min of 5 degrees here to prevent
// the user constantly receiving warnings about high // the user constantly receiving warnings about high
// normalised yaw innovations // 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); yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2); 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 // read the delta angle and corresponding time interval from the IMU
// return false if data is not available // 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(); const auto &ins = dal.ins();
if (ins_index < ins.get_gyro_count()) { 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 true;
} }
return false; return false;
@ -773,7 +781,7 @@ void NavEKF3_core::readBaroData()
void NavEKF3_core::calcFiltBaroOffset() void NavEKF3_core::calcFiltBaroOffset()
{ {
// Apply a first order LPF with spike protection // 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. // 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 // 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 // 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) { if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
// Use the baro drift rate // Use the baro drift rate
const float baroDriftRate = 0.05f; const ftype baroDriftRate = 0.05;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime); ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) { } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
// use the worse case expected terrain gradient and vehicle horizontal speed // 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); ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else { } else {
// by definition our height source is absolute so cannot run this filter // 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 // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
// when not using GPS as height source // 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 // calculate the correction gain
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
// calculate the innovation // calculate the innovation
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
// check the innovation variance ratio // 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 // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if (ratio < 25.0f && gpsAccuracyGood) { if (ratio < 25.0f && gpsAccuracyGood) {
@ -910,7 +918,7 @@ void NavEKF3_core::readRngBcnData()
rngBcnDataNew.rng = beacon->beacon_distance(index); rngBcnDataNew.rng = beacon->beacon_distance(index);
// set the beacon position // set the beacon position
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index); rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
// identify the beacon identifier // identify the beacon identifier
rngBcnDataNew.beacon_ID = index; rngBcnDataNew.beacon_ID = index;
@ -927,15 +935,19 @@ void NavEKF3_core::readRngBcnData()
} }
// Check if the beacon system has returned a 3D fix // 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; rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
} }
beaconVehiclePosNED = bp.toftype();
beaconVehiclePosErr = bperr;
// Check if the range beacon data can be used to align the vehicle position // Check if the range beacon data can be used to align the vehicle position
if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) { 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 // 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 ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) { if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true; rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set // 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.posReset = false;
} }
extNavDataNew.pos = pos; extNavDataNew.pos = pos.toftype();
extNavDataNew.posErr = posErr; extNavDataNew.posErr = posErr;
// calculate timestamp // calculate timestamp
@ -1052,7 +1064,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
// protect against attitude or angle being NaN // protect against attitude or angle being NaN
if (!quat.is_nan() && !isnan(angErr)) { if (!quat.is_nan() && !isnan(angErr)) {
// extract yaw from the attitude // 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); quat.to_euler(roll_rad, pitch_rad, yaw_rad);
yaw_elements extNavYawAngDataNew; yaw_elements extNavYawAngDataNew;
extNavYawAngDataNew.yawAng = yaw_rad; 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; ext_nav_vel_elements extNavVelNew;
extNavVelNew.time_ms = timeStamp_ms; extNavVelNew.time_ms = timeStamp_ms;
extNavVelNew.vel = vel; extNavVelNew.vel = vel.toftype();
extNavVelNew.err = err; extNavVelNew.err = err;
extNavVelNew.corrected = false; extNavVelNew.corrected = false;
@ -1246,15 +1258,15 @@ void NavEKF3_core::learnInactiveBiases(void)
// get filtered gyro and use the difference between the // get filtered gyro and use the difference between the
// corrected gyro on the active IMU and the inactive IMU // corrected gyro on the active IMU and the inactive IMU
// to move the inactive bias towards the right value // 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_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].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; Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
// prevent a single large error from contaminating bias estimate // prevent a single large error from contaminating bias estimate
const float bias_limit = radians(5); const ftype bias_limit = radians(5);
error.x = constrain_float(error.x, -bias_limit, bias_limit); error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
error.y = constrain_float(error.y, -bias_limit, bias_limit); error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
error.z = constrain_float(error.z, -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 // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
// gyro bias error in around 1 minute // 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 // get filtered accel and use the difference between the
// corrected accel on the active IMU and the inactive IMU // corrected accel on the active IMU and the inactive IMU
// to move the inactive bias towards the right value // 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_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg);
Vector3f filtered_accel_inactive = ins.get_accel(i) - (inactiveBias[i].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; Vector3F error = filtered_accel_active - filtered_accel_inactive;
// prevent a single large error from contaminating bias estimate // prevent a single large error from contaminating bias estimate
const float bias_limit = 1.0; // m/s/s const ftype bias_limit = 1.0; // m/s/s
error.x = constrain_float(error.x, -bias_limit, bias_limit); error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
error.y = constrain_float(error.y, -bias_limit, bias_limit); error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
error.z = constrain_float(error.z, -bias_limit, bias_limit); error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
// slowly bring the inactive accel in line with the active // slowly bring the inactive accel in line with the active
// accel. This corrects a 0.5 m/s/s accel bias error in // 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 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 // if we are using the WMM tables then use the table declination
// to ensure consistency with the table mag field. Otherwise use // to ensure consistency with the table mag field. Otherwise use
@ -1327,17 +1339,17 @@ void NavEKF3_core::updateMovementCheck(void)
return; return;
} }
const float gyro_limit = radians(3.0f); const ftype gyro_limit = radians(3.0f);
const float gyro_diff_limit = 0.2f; const ftype gyro_diff_limit = 0.2f;
const float accel_limit = 1.0f; const ftype accel_limit = 1.0f;
const float accel_diff_limit = 5.0f; const ftype accel_diff_limit = 5.0f;
const float hysteresis_ratio = 0.7f; const ftype hysteresis_ratio = 0.7f;
const float dtEkfAvgInv = 1.0f / dtEkfAvg; const ftype dtEkfAvgInv = 1.0f / dtEkfAvg;
// get latest bias corrected gyro and accelerometer data // get latest bias corrected gyro and accelerometer data
const auto &ins = dal.ins(); const auto &ins = dal.ins();
Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv; Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv;
Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv; Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv;
if (!prevOnGround) { if (!prevOnGround) {
gyro_prev = gyro; gyro_prev = gyro;
@ -1349,7 +1361,7 @@ void NavEKF3_core::updateMovementCheck(void)
} }
// calculate a gyro rate of change metric // calculate a gyro rate of change metric
Vector3f temp = (gyro - gyro_prev) * dtEkfAvgInv; Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv;
gyro_prev = gyro; gyro_prev = gyro;
gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length(); gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();
@ -1358,14 +1370,14 @@ void NavEKF3_core::updateMovementCheck(void)
accel_prev = accel; accel_prev = accel;
accel_diff = 0.99f * accel_diff + 0.01f * temp.length(); accel_diff = 0.99f * accel_diff + 0.01f * temp.length();
const float gyro_length_ratio = gyro.length() / gyro_limit; const ftype gyro_length_ratio = gyro.length() / gyro_limit;
const float accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit; const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
const float gyro_diff_ratio = gyro_diff / gyro_diff_limit; const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit;
const float accel_diff_ratio = accel_diff / accel_diff_limit; const ftype accel_diff_ratio = accel_diff / accel_diff_limit;
bool logStatusChange = false; bool logStatusChange = false;
if (onGroundNotMoving) { if (onGroundNotMoving) {
if (gyro_length_ratio > frontend->_ognmTestScaleFactor || if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
fabsf(accel_length_ratio) > frontend->_ognmTestScaleFactor || fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
gyro_diff_ratio > frontend->_ognmTestScaleFactor || gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
accel_diff_ratio > frontend->_ognmTestScaleFactor) accel_diff_ratio > frontend->_ognmTestScaleFactor)
{ {
@ -1373,7 +1385,7 @@ void NavEKF3_core::updateMovementCheck(void)
logStatusChange = true; logStatusChange = true;
} }
} else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio && } 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 && gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
accel_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(), time_us : dal.micros64(),
core : core_index, core : core_index,
ongroundnotmoving : onGroundNotMoving, ongroundnotmoving : onGroundNotMoving,
gyro_length_ratio : gyro_length_ratio, gyro_length_ratio : float(gyro_length_ratio),
accel_length_ratio : accel_length_ratio, accel_length_ratio : float(accel_length_ratio),
gyro_diff_ratio : gyro_diff_ratio, gyro_diff_ratio : float(gyro_diff_ratio),
accel_diff_ratio : accel_diff_ratio, accel_diff_ratio : float(accel_diff_ratio),
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
@ -1401,9 +1413,9 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
{ {
#if EK3_FEATURE_DRAG_FUSION #if EK3_FEATURE_DRAG_FUSION
// Average and down sample to 5Hz // Average and down sample to 5Hz
const float bcoef_x = frontend->_ballisticCoef_x; const ftype bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_y; const ftype bcoef_y = frontend->_ballisticCoef_y;
const float mcoef = frontend->_momentumDragCoef.get(); const ftype mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef_x > 1.0f; const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef_y > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f; 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) void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
{ {
// horizontal velocity squared // 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 LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits // 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 // 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 // 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); distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
prevPosN = stateStruct.position[0]; prevPosN = stateStruct.position[0];
prevPosE = stateStruct.position[1]; 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 // 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); ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6]; ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
Popt += Pincrement; Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
// fuse range finder data // fuse range finder data
if (rangeDataToFuse) { if (rangeDataToFuse) {
// predict range // 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 // Copy required states to local variable names
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // 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 // 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 // calculate Kalman gain
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3); ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))); ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
// Calculate the innovation variance for data logging // Calculate the innovation variance for data logging
varInnovRng = (R_RNG + Popt/sq(SK_RNG)); varInnovRng = (R_RNG + Popt/sq(SK_RNG));
@ -131,7 +131,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
innovRng = predRngMeas - rangeDataDelayed.rng; innovRng = predRngMeas - rangeDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // Check the innovation test ratio and don't fuse if too large
if (auxRngTestRatio < 1.0f) { if (auxRngTestRatio < 1.0f) {
@ -152,18 +152,18 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
if (!cantFuseFlowData) { if (!cantFuseFlowData) {
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
Vector2f losPred; // predicted optical flow angular rate measurement Vector2F losPred; // predicted optical flow angular rate measurement
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
float K_OPT; ftype K_OPT;
float H_OPT; ftype H_OPT;
Vector2f auxFlowObsInnovVar; Vector2F auxFlowObsInnovVar;
// predict range to centre of image // 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 // constrain terrain height to be below the vehicle
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
@ -180,20 +180,20 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp; auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
// calculate observation jacobians // calculate observation jacobians
float t2 = q0*q0; ftype t2 = q0*q0;
float t3 = q1*q1; ftype t3 = q1*q1;
float t4 = q2*q2; ftype t4 = q2*q2;
float t5 = q3*q3; ftype t5 = q3*q3;
float t6 = stateStruct.position.z - terrainState; ftype t6 = stateStruct.position.z - terrainState;
float t7 = 1.0f / (t6*t6); ftype t7 = 1.0f / (t6*t6);
float t8 = q0*q3*2.0f; ftype t8 = q0*q3*2.0f;
float t9 = t2-t3-t4+t5; ftype t9 = t2-t3-t4+t5;
// prevent the state variances from becoming badly conditioned // prevent the state variances from becoming badly conditioned
Popt = MAX(Popt,1E-6f); Popt = MAX(Popt,1E-6f);
// calculate observation noise variance from parameter // 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 // Fuse Y axis data
@ -207,7 +207,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y; K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
// calculate the innovation consistency test ratio // 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 // don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.y < 1.0f) { if (auxFlowTestRatio.y < 1.0f) {
@ -239,7 +239,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x; K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
// calculate the innovation consistency test ratio // 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 // don't fuse if optical flow data is outside valid range
if (auxFlowTestRatio.x < 1.0f) { 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) void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
{ {
Vector24 H_LOS; Vector24 H_LOS;
Vector3f relVelSensor; Vector3F relVelSensor;
Vector14 SH_LOS; Vector14 SH_LOS;
Vector2 losPred; Vector2 losPred;
// Copy required states to local variable names // Copy required states to local variable names
float q0 = stateStruct.quat[0]; ftype q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1]; ftype q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2]; ftype q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3]; ftype q3 = stateStruct.quat[3];
float vn = stateStruct.velocity.x; ftype vn = stateStruct.velocity.x;
float ve = stateStruct.velocity.y; ftype ve = stateStruct.velocity.y;
float vd = stateStruct.velocity.z; ftype vd = stateStruct.velocity.z;
float pd = stateStruct.position.z; ftype pd = stateStruct.position.z;
// constrain height above ground to be above range measured on ground // constrain height above ground to be above range measured on ground
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
float ptd = pd + heightAboveGndEst; ftype ptd = pd + heightAboveGndEst;
// Calculate common expressions for observation jacobians // Calculate common expressions for observation jacobians
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); 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 // 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 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 // 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 // correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the // the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain // 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()) { if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z; 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)); memset(&H_LOS[0], 0, sizeof(H_LOS));
if (obsIndex == 0) { if (obsIndex == 0) {
// calculate X axis observation Jacobian // 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[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[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); 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); H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
// calculate intermediate variables for the X observation innovation variance and Kalman gains // calculate intermediate variables for the X observation innovation variance and Kalman gains
float t3 = q1*vd*2.0f; ftype t3 = q1*vd*2.0f;
float t4 = q0*ve*2.0f; ftype t4 = q0*ve*2.0f;
float t11 = q3*vn*2.0f; ftype t11 = q3*vn*2.0f;
float t5 = t3+t4-t11; ftype t5 = t3+t4-t11;
float t6 = q0*q3*2.0f; ftype t6 = q0*q3*2.0f;
float t29 = q1*q2*2.0f; ftype t29 = q1*q2*2.0f;
float t7 = t6-t29; ftype t7 = t6-t29;
float t8 = q0*q1*2.0f; ftype t8 = q0*q1*2.0f;
float t9 = q2*q3*2.0f; ftype t9 = q2*q3*2.0f;
float t10 = t8+t9; ftype t10 = t8+t9;
float t12 = P[0][0]*t2*t5; ftype t12 = P[0][0]*t2*t5;
float t13 = q0*vd*2.0f; ftype t13 = q0*vd*2.0f;
float t14 = q2*vn*2.0f; ftype t14 = q2*vn*2.0f;
float t28 = q1*ve*2.0f; ftype t28 = q1*ve*2.0f;
float t15 = t13+t14-t28; ftype t15 = t13+t14-t28;
float t16 = q3*vd*2.0f; ftype t16 = q3*vd*2.0f;
float t17 = q2*ve*2.0f; ftype t17 = q2*ve*2.0f;
float t18 = q1*vn*2.0f; ftype t18 = q1*vn*2.0f;
float t19 = t16+t17+t18; ftype t19 = t16+t17+t18;
float t20 = q3*ve*2.0f; ftype t20 = q3*ve*2.0f;
float t21 = q0*vn*2.0f; ftype t21 = q0*vn*2.0f;
float t30 = q2*vd*2.0f; ftype t30 = q2*vd*2.0f;
float t22 = t20+t21-t30; ftype t22 = t20+t21-t30;
float t23 = q0*q0; ftype t23 = q0*q0;
float t24 = q1*q1; ftype t24 = q1*q1;
float t25 = q2*q2; ftype t25 = q2*q2;
float t26 = q3*q3; ftype t26 = q3*q3;
float t27 = t23-t24+t25-t26; ftype t27 = t23-t24+t25-t26;
float t31 = P[1][1]*t2*t15; ftype t31 = P[1][1]*t2*t15;
float t32 = P[6][0]*t2*t10; ftype t32 = P[6][0]*t2*t10;
float t33 = P[1][0]*t2*t15; ftype t33 = P[1][0]*t2*t15;
float t34 = P[2][0]*t2*t19; ftype t34 = P[2][0]*t2*t19;
float t35 = P[5][0]*t2*t27; ftype t35 = P[5][0]*t2*t27;
float t79 = P[4][0]*t2*t7; ftype t79 = P[4][0]*t2*t7;
float t80 = P[3][0]*t2*t22; ftype t80 = P[3][0]*t2*t22;
float t36 = t12+t32+t33+t34+t35-t79-t80; ftype t36 = t12+t32+t33+t34+t35-t79-t80;
float t37 = t2*t5*t36; ftype t37 = t2*t5*t36;
float t38 = P[6][1]*t2*t10; ftype t38 = P[6][1]*t2*t10;
float t39 = P[0][1]*t2*t5; ftype t39 = P[0][1]*t2*t5;
float t40 = P[2][1]*t2*t19; ftype t40 = P[2][1]*t2*t19;
float t41 = P[5][1]*t2*t27; ftype t41 = P[5][1]*t2*t27;
float t81 = P[4][1]*t2*t7; ftype t81 = P[4][1]*t2*t7;
float t82 = P[3][1]*t2*t22; ftype t82 = P[3][1]*t2*t22;
float t42 = t31+t38+t39+t40+t41-t81-t82; ftype t42 = t31+t38+t39+t40+t41-t81-t82;
float t43 = t2*t15*t42; ftype t43 = t2*t15*t42;
float t44 = P[6][2]*t2*t10; ftype t44 = P[6][2]*t2*t10;
float t45 = P[0][2]*t2*t5; ftype t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t15; ftype t46 = P[1][2]*t2*t15;
float t47 = P[2][2]*t2*t19; ftype t47 = P[2][2]*t2*t19;
float t48 = P[5][2]*t2*t27; ftype t48 = P[5][2]*t2*t27;
float t83 = P[4][2]*t2*t7; ftype t83 = P[4][2]*t2*t7;
float t84 = P[3][2]*t2*t22; ftype t84 = P[3][2]*t2*t22;
float t49 = t44+t45+t46+t47+t48-t83-t84; ftype t49 = t44+t45+t46+t47+t48-t83-t84;
float t50 = t2*t19*t49; ftype t50 = t2*t19*t49;
float t51 = P[6][3]*t2*t10; ftype t51 = P[6][3]*t2*t10;
float t52 = P[0][3]*t2*t5; ftype t52 = P[0][3]*t2*t5;
float t53 = P[1][3]*t2*t15; ftype t53 = P[1][3]*t2*t15;
float t54 = P[2][3]*t2*t19; ftype t54 = P[2][3]*t2*t19;
float t55 = P[5][3]*t2*t27; ftype t55 = P[5][3]*t2*t27;
float t85 = P[4][3]*t2*t7; ftype t85 = P[4][3]*t2*t7;
float t86 = P[3][3]*t2*t22; ftype t86 = P[3][3]*t2*t22;
float t56 = t51+t52+t53+t54+t55-t85-t86; ftype t56 = t51+t52+t53+t54+t55-t85-t86;
float t57 = P[6][5]*t2*t10; ftype t57 = P[6][5]*t2*t10;
float t58 = P[0][5]*t2*t5; ftype t58 = P[0][5]*t2*t5;
float t59 = P[1][5]*t2*t15; ftype t59 = P[1][5]*t2*t15;
float t60 = P[2][5]*t2*t19; ftype t60 = P[2][5]*t2*t19;
float t61 = P[5][5]*t2*t27; ftype t61 = P[5][5]*t2*t27;
float t88 = P[4][5]*t2*t7; ftype t88 = P[4][5]*t2*t7;
float t89 = P[3][5]*t2*t22; ftype t89 = P[3][5]*t2*t22;
float t62 = t57+t58+t59+t60+t61-t88-t89; ftype t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62; ftype t63 = t2*t27*t62;
float t64 = P[6][4]*t2*t10; ftype t64 = P[6][4]*t2*t10;
float t65 = P[0][4]*t2*t5; ftype t65 = P[0][4]*t2*t5;
float t66 = P[1][4]*t2*t15; ftype t66 = P[1][4]*t2*t15;
float t67 = P[2][4]*t2*t19; ftype t67 = P[2][4]*t2*t19;
float t68 = P[5][4]*t2*t27; ftype t68 = P[5][4]*t2*t27;
float t90 = P[4][4]*t2*t7; ftype t90 = P[4][4]*t2*t7;
float t91 = P[3][4]*t2*t22; ftype t91 = P[3][4]*t2*t22;
float t69 = t64+t65+t66+t67+t68-t90-t91; ftype t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = P[6][6]*t2*t10; ftype t70 = P[6][6]*t2*t10;
float t71 = P[0][6]*t2*t5; ftype t71 = P[0][6]*t2*t5;
float t72 = P[1][6]*t2*t15; ftype t72 = P[1][6]*t2*t15;
float t73 = P[2][6]*t2*t19; ftype t73 = P[2][6]*t2*t19;
float t74 = P[5][6]*t2*t27; ftype t74 = P[5][6]*t2*t27;
float t93 = P[4][6]*t2*t7; ftype t93 = P[4][6]*t2*t7;
float t94 = P[3][6]*t2*t22; ftype t94 = P[3][6]*t2*t22;
float t75 = t70+t71+t72+t73+t74-t93-t94; ftype t75 = t70+t71+t72+t73+t74-t93-t94;
float t76 = t2*t10*t75; ftype t76 = t2*t10*t75;
float t87 = t2*t22*t56; ftype t87 = t2*t22*t56;
float t92 = t2*t7*t69; ftype t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; ftype t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78; ftype t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 > R_LOS) { 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[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); 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 { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -477,8 +477,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
if (!inhibitMagStates) { 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[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); 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 { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
if (!inhibitWindStates) { 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[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); 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 { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); zero_range(&Kfusion[0], 22, 23);
} }
} else { } else {
// calculate Y axis observation Jacobian // 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[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[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); 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); H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
// calculate intermediate variables for the Y observation innovation variance and Kalman gains // calculate intermediate variables for the Y observation innovation variance and Kalman gains
float t3 = q3*ve*2.0f; ftype t3 = q3*ve*2.0f;
float t4 = q0*vn*2.0f; ftype t4 = q0*vn*2.0f;
float t11 = q2*vd*2.0f; ftype t11 = q2*vd*2.0f;
float t5 = t3+t4-t11; ftype t5 = t3+t4-t11;
float t6 = q0*q3*2.0f; ftype t6 = q0*q3*2.0f;
float t7 = q1*q2*2.0f; ftype t7 = q1*q2*2.0f;
float t8 = t6+t7; ftype t8 = t6+t7;
float t9 = q0*q2*2.0f; ftype t9 = q0*q2*2.0f;
float t28 = q1*q3*2.0f; ftype t28 = q1*q3*2.0f;
float t10 = t9-t28; ftype t10 = t9-t28;
float t12 = P[0][0]*t2*t5; ftype t12 = P[0][0]*t2*t5;
float t13 = q3*vd*2.0f; ftype t13 = q3*vd*2.0f;
float t14 = q2*ve*2.0f; ftype t14 = q2*ve*2.0f;
float t15 = q1*vn*2.0f; ftype t15 = q1*vn*2.0f;
float t16 = t13+t14+t15; ftype t16 = t13+t14+t15;
float t17 = q0*vd*2.0f; ftype t17 = q0*vd*2.0f;
float t18 = q2*vn*2.0f; ftype t18 = q2*vn*2.0f;
float t29 = q1*ve*2.0f; ftype t29 = q1*ve*2.0f;
float t19 = t17+t18-t29; ftype t19 = t17+t18-t29;
float t20 = q1*vd*2.0f; ftype t20 = q1*vd*2.0f;
float t21 = q0*ve*2.0f; ftype t21 = q0*ve*2.0f;
float t30 = q3*vn*2.0f; ftype t30 = q3*vn*2.0f;
float t22 = t20+t21-t30; ftype t22 = t20+t21-t30;
float t23 = q0*q0; ftype t23 = q0*q0;
float t24 = q1*q1; ftype t24 = q1*q1;
float t25 = q2*q2; ftype t25 = q2*q2;
float t26 = q3*q3; ftype t26 = q3*q3;
float t27 = t23+t24-t25-t26; ftype t27 = t23+t24-t25-t26;
float t31 = P[1][1]*t2*t16; ftype t31 = P[1][1]*t2*t16;
float t32 = P[5][0]*t2*t8; ftype t32 = P[5][0]*t2*t8;
float t33 = P[1][0]*t2*t16; ftype t33 = P[1][0]*t2*t16;
float t34 = P[3][0]*t2*t22; ftype t34 = P[3][0]*t2*t22;
float t35 = P[4][0]*t2*t27; ftype t35 = P[4][0]*t2*t27;
float t80 = P[6][0]*t2*t10; ftype t80 = P[6][0]*t2*t10;
float t81 = P[2][0]*t2*t19; ftype t81 = P[2][0]*t2*t19;
float t36 = t12+t32+t33+t34+t35-t80-t81; ftype t36 = t12+t32+t33+t34+t35-t80-t81;
float t37 = t2*t5*t36; ftype t37 = t2*t5*t36;
float t38 = P[5][1]*t2*t8; ftype t38 = P[5][1]*t2*t8;
float t39 = P[0][1]*t2*t5; ftype t39 = P[0][1]*t2*t5;
float t40 = P[3][1]*t2*t22; ftype t40 = P[3][1]*t2*t22;
float t41 = P[4][1]*t2*t27; ftype t41 = P[4][1]*t2*t27;
float t82 = P[6][1]*t2*t10; ftype t82 = P[6][1]*t2*t10;
float t83 = P[2][1]*t2*t19; ftype t83 = P[2][1]*t2*t19;
float t42 = t31+t38+t39+t40+t41-t82-t83; ftype t42 = t31+t38+t39+t40+t41-t82-t83;
float t43 = t2*t16*t42; ftype t43 = t2*t16*t42;
float t44 = P[5][2]*t2*t8; ftype t44 = P[5][2]*t2*t8;
float t45 = P[0][2]*t2*t5; ftype t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t16; ftype t46 = P[1][2]*t2*t16;
float t47 = P[3][2]*t2*t22; ftype t47 = P[3][2]*t2*t22;
float t48 = P[4][2]*t2*t27; ftype t48 = P[4][2]*t2*t27;
float t79 = P[2][2]*t2*t19; ftype t79 = P[2][2]*t2*t19;
float t84 = P[6][2]*t2*t10; ftype t84 = P[6][2]*t2*t10;
float t49 = t44+t45+t46+t47+t48-t79-t84; ftype t49 = t44+t45+t46+t47+t48-t79-t84;
float t50 = P[5][3]*t2*t8; ftype t50 = P[5][3]*t2*t8;
float t51 = P[0][3]*t2*t5; ftype t51 = P[0][3]*t2*t5;
float t52 = P[1][3]*t2*t16; ftype t52 = P[1][3]*t2*t16;
float t53 = P[3][3]*t2*t22; ftype t53 = P[3][3]*t2*t22;
float t54 = P[4][3]*t2*t27; ftype t54 = P[4][3]*t2*t27;
float t86 = P[6][3]*t2*t10; ftype t86 = P[6][3]*t2*t10;
float t87 = P[2][3]*t2*t19; ftype t87 = P[2][3]*t2*t19;
float t55 = t50+t51+t52+t53+t54-t86-t87; ftype t55 = t50+t51+t52+t53+t54-t86-t87;
float t56 = t2*t22*t55; ftype t56 = t2*t22*t55;
float t57 = P[5][4]*t2*t8; ftype t57 = P[5][4]*t2*t8;
float t58 = P[0][4]*t2*t5; ftype t58 = P[0][4]*t2*t5;
float t59 = P[1][4]*t2*t16; ftype t59 = P[1][4]*t2*t16;
float t60 = P[3][4]*t2*t22; ftype t60 = P[3][4]*t2*t22;
float t61 = P[4][4]*t2*t27; ftype t61 = P[4][4]*t2*t27;
float t88 = P[6][4]*t2*t10; ftype t88 = P[6][4]*t2*t10;
float t89 = P[2][4]*t2*t19; ftype t89 = P[2][4]*t2*t19;
float t62 = t57+t58+t59+t60+t61-t88-t89; ftype t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62; ftype t63 = t2*t27*t62;
float t64 = P[5][5]*t2*t8; ftype t64 = P[5][5]*t2*t8;
float t65 = P[0][5]*t2*t5; ftype t65 = P[0][5]*t2*t5;
float t66 = P[1][5]*t2*t16; ftype t66 = P[1][5]*t2*t16;
float t67 = P[3][5]*t2*t22; ftype t67 = P[3][5]*t2*t22;
float t68 = P[4][5]*t2*t27; ftype t68 = P[4][5]*t2*t27;
float t90 = P[6][5]*t2*t10; ftype t90 = P[6][5]*t2*t10;
float t91 = P[2][5]*t2*t19; ftype t91 = P[2][5]*t2*t19;
float t69 = t64+t65+t66+t67+t68-t90-t91; ftype t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = t2*t8*t69; ftype t70 = t2*t8*t69;
float t71 = P[5][6]*t2*t8; ftype t71 = P[5][6]*t2*t8;
float t72 = P[0][6]*t2*t5; ftype t72 = P[0][6]*t2*t5;
float t73 = P[1][6]*t2*t16; ftype t73 = P[1][6]*t2*t16;
float t74 = P[3][6]*t2*t22; ftype t74 = P[3][6]*t2*t22;
float t75 = P[4][6]*t2*t27; ftype t75 = P[4][6]*t2*t27;
float t92 = P[6][6]*t2*t10; ftype t92 = P[6][6]*t2*t10;
float t93 = P[2][6]*t2*t19; ftype t93 = P[2][6]*t2*t19;
float t76 = t71+t72+t73+t74+t75-t92-t93; ftype t76 = t71+t72+t73+t74+t75-t92-t93;
float t85 = t2*t19*t49; ftype t85 = t2*t19*t49;
float t94 = t2*t10*t76; ftype t94 = t2*t10*t76;
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; ftype t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78; ftype t78;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 > R_LOS) { 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[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); 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 { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -653,8 +653,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); zero_range(&Kfusion[0], 13, 15);
} }
if (!inhibitMagStates) { 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[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); 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 { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
if (!inhibitWindStates) { 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[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); 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 { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); zero_range(&Kfusion[0], 22, 23);
} }
} }
// calculate the innovation consistency test ratio // 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 // 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)) { 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 // 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]); 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; return false;
} }
@ -98,7 +98,7 @@ void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero(); gyroBias.zero();
return; return;
} }
gyroBias = stateStruct.gyro_bias / dtEkfAvg; gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
} }
// return accelerometer bias in m/s/s // return accelerometer bias in m/s/s
@ -108,7 +108,7 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
accelBias.zero(); accelBias.zero();
return; return;
} }
accelBias = stateStruct.accel_bias / dtEkfAvg; accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
} }
// return the transformation matrix from XYZ (body) to NED axes // 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 // return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF3_core::getQuaternion(Quaternion& ret) const 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 // 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 // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
{ {
pos = posResetNE; pos = posResetNE.tofloat();
return lastPosReset_ms; 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 // returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
{ {
vel = velResetNE; vel = velResetNE.tofloat();
return lastVelReset_ms; return lastVelReset_ms;
} }
@ -171,7 +171,7 @@ bool NavEKF3_core::getWind(Vector3f &wind) const
void NavEKF3_core::getVelNED(Vector3f &vel) const void NavEKF3_core::getVelNED(Vector3f &vel) const
{ {
// correct for the IMU position offset (EKF calculations are at the IMU) // 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 // 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) { if (inhibitWindStates || PV_AidingMode == AID_NONE) {
return false; return false;
} }
vel = outputDataNew.velocity + velOffsetNED; vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x; vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y; vel.y -= stateStruct.wind_vel.y;
Matrix3f Tnb; // rotation from nav to body frame 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 ((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 // 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 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.x = tempPosNE.x;
posNE.y = tempPosNE.y; posNE.y = tempPosNE.y;
return false; return false;
@ -369,13 +369,13 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
// return earth magnetic field estimates in measurement units / 1000 // return earth magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagNED(Vector3f &magNED) const 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 // return body magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
{ {
magXYZ = stateStruct.body_magfield*1000.0f; magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
} }
// return magnetometer offsets // return magnetometer offsets
@ -394,7 +394,7 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
!inhibitMagStates && !inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) && dal.get_compass()->healthy(magSelectIndex) &&
variancesConverged) { 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; return true;
} else { } else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex); 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 // 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 void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {
velVar = sqrtf(velTestRatio); velVar = sqrtF(velTestRatio);
posVar = sqrtf(posTestRatio); posVar = sqrtF(posTestRatio);
hgtVar = sqrtf(hgtTestRatio); 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 // 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.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio)); magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio)); magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio); tasVar = sqrtF(tasTestRatio);
offset = posResetNE; offset = posResetNE.tofloat();
} }
// get a particular source's velocity innovations // 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) { if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
return false; return false;
} }
innovations = gpsVelInnov; innovations = gpsVelInnov.tofloat();
variances = gpsVelVarInnov; variances = gpsVelVarInnov.tofloat();
return true; return true;
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
case AP_NavEKF_Source::SourceXY::EXTNAV: 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) { if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
return false; return false;
} }
innovations = extNavVelInnov; innovations = extNavVelInnov.tofloat();
variances = extNavVelVarInnov; variances = extNavVelVarInnov.tofloat();
return true; return true;
#endif // EK3_FEATURE_EXTERNAL_NAV #endif // EK3_FEATURE_EXTERNAL_NAV
default: default:
@ -565,11 +565,11 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
// range finder is fitted for other applications // range finder is fitted for other applications
float temp; float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio); temp = sqrtF(auxRngTestRatio);
} else { } else {
temp = 0.0f; 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 // send message
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); 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() void NavEKF3_core::FuseRngBcn()
{ {
// declarations // declarations
float pn; ftype pn;
float pe; ftype pe;
float pd; ftype pd;
float bcn_pn; ftype bcn_pn;
float bcn_pe; ftype bcn_pe;
float bcn_pd; ftype bcn_pd;
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
float rngPred; ftype rngPred;
// health is set bad until test passed // health is set bad until test passed
rngBcnHealth = false; rngBcnHealth = false;
@ -70,7 +70,7 @@ void NavEKF3_core::FuseRngBcn()
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z; bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
// predicted range // predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
rngPred = deltaPosNED.length(); rngPred = deltaPosNED.length();
// calculate measurement innovation // calculate measurement innovation
@ -80,16 +80,16 @@ void NavEKF3_core::FuseRngBcn()
if (rngPred > 0.1f) if (rngPred > 0.1f)
{ {
// calculate observation jacobians // calculate observation jacobians
float H_BCN[24]; ftype H_BCN[24];
memset(H_BCN, 0, sizeof(H_BCN)); memset(H_BCN, 0, sizeof(H_BCN));
float t2 = bcn_pd-pd; ftype t2 = bcn_pd-pd;
float t3 = bcn_pe-pe; ftype t3 = bcn_pe-pe;
float t4 = bcn_pn-pn; ftype t4 = bcn_pn-pn;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
float t9 = 1.0f/sqrtf(t8); ftype t9 = 1.0f/sqrtF(t8);
H_BCN[7] = -t4*t9; H_BCN[7] = -t4*t9;
H_BCN[8] = -t3*t9; H_BCN[8] = -t3*t9;
// If we are not using the beacons as a height reference, we pretend that the beacons // 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; H_BCN[9] = -t2*t9;
// calculate Kalman gains // calculate Kalman gains
float t10 = P[9][9]*t2*t9; ftype t10 = P[9][9]*t2*t9;
float t11 = P[8][9]*t3*t9; ftype t11 = P[8][9]*t3*t9;
float t12 = P[7][9]*t4*t9; ftype t12 = P[7][9]*t4*t9;
float t13 = t10+t11+t12; ftype t13 = t10+t11+t12;
float t14 = t2*t9*t13; ftype t14 = t2*t9*t13;
float t15 = P[9][8]*t2*t9; ftype t15 = P[9][8]*t2*t9;
float t16 = P[8][8]*t3*t9; ftype t16 = P[8][8]*t3*t9;
float t17 = P[7][8]*t4*t9; ftype t17 = P[7][8]*t4*t9;
float t18 = t15+t16+t17; ftype t18 = t15+t16+t17;
float t19 = t3*t9*t18; ftype t19 = t3*t9*t18;
float t20 = P[9][7]*t2*t9; ftype t20 = P[9][7]*t2*t9;
float t21 = P[8][7]*t3*t9; ftype t21 = P[8][7]*t3*t9;
float t22 = P[7][7]*t4*t9; ftype t22 = P[7][7]*t4*t9;
float t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
float t24 = t4*t9*t23; ftype t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24; varInnovRngBcn = R_BCN+t14+t19+t24;
float t26; ftype t26;
if (varInnovRngBcn >= R_BCN) { if (varInnovRngBcn >= R_BCN) {
t26 = 1.0f/varInnovRngBcn; t26 = 1.0f/varInnovRngBcn;
faultStatus.bad_rngbcn = false; 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[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); Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9);
} else { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12
memset(&Kfusion[10], 0, 12); zero_range(&Kfusion[0], 10, 12);
} }
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {
@ -158,8 +158,8 @@ void NavEKF3_core::FuseRngBcn()
} }
} }
} else { } else {
// zero indexes 13 to 15 = 3*4 bytes // zero indexes 13 to 15
memset(&Kfusion[13], 0, 12); 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 // 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[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); Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9);
} else { } else {
// zero indexes 16 to 21 = 6*4 bytes // zero indexes 16 to 21
memset(&Kfusion[16], 0, 24); zero_range(&Kfusion[0], 16, 21);
} }
if (!inhibitWindStates) { if (!inhibitWindStates) {
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9); 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); Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
} else { } else {
// zero indexes 22 to 23 = 2*4 bytes // zero indexes 22 to 23
memset(&Kfusion[22], 0, 8); zero_range(&Kfusion[0], 22, 23);
} }
// Calculate innovation using the selected offset value // 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; innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata); rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
@ -283,7 +283,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
void NavEKF3_core::FuseRngBcnStatic() void NavEKF3_core::FuseRngBcnStatic()
{ {
// get the estimated range measurement variance // 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 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) { if (numBcnMeas >= 100) {
rngBcnAlignmentStarted = true; 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 // initialise the receiver position to the centre of the beacons and at zero height
receiverPos.x = rngBcnPosSum.x * tempVar; receiverPos.x = rngBcnPosSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * 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 // 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 // The position estimate should be stable after 100 iterations so we use a simple dual
// hypothesis 1-state EKF to estimate the offset // hypothesis 1-state EKF to estimate the offset
Vector3f refPosNED; Vector3F refPosNED;
refPosNED.x = receiverPos.x; refPosNED.x = receiverPos.x;
refPosNED.y = receiverPos.y; refPosNED.y = receiverPos.y;
refPosNED.z = stateStruct.position.z; refPosNED.z = stateStruct.position.z;
@ -355,14 +355,14 @@ void NavEKF3_core::FuseRngBcnStatic()
// and the main EKF vertical position // and the main EKF vertical position
// Calculate the mid vertical position of all beacons // 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 // 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 // calcuate the two hypothesis for our vertical position
float receverPosDownMax; ftype receverPosDownMax;
float receverPosDownMin; ftype receverPosDownMin;
if (delta >= 0.0f) { if (delta >= 0.0f) {
receverPosDownMax = receiverPos.z; receverPosDownMax = receiverPos.z;
receverPosDownMin = receiverPos.z - 2.0f * delta; receverPosDownMin = receiverPos.z - 2.0f * delta;
@ -385,62 +385,62 @@ void NavEKF3_core::FuseRngBcnStatic()
} }
// calculate the observation jacobian // calculate the observation jacobian
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z; ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
if (t8 < 0.1f) { if (t8 < 0.1f) {
// calculation will be badly conditioned // calculation will be badly conditioned
return; return;
} }
float t9 = 1.0f/sqrtf(t8); ftype t9 = 1.0f/sqrtF(t8);
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
float t15 = receiverPos.x*2.0f; ftype t15 = receiverPos.x*2.0f;
float t11 = t10-t15; ftype t11 = t10-t15;
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
float t14 = receiverPos.y*2.0f; ftype t14 = receiverPos.y*2.0f;
float t13 = t12-t14; ftype t13 = t12-t14;
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
float t18 = receiverPos.z*2.0f; ftype t18 = receiverPos.z*2.0f;
float t17 = t16-t18; ftype t17 = t16-t18;
float H_RNG[3]; ftype H_RNG[3];
H_RNG[0] = -t9*t11*0.5f; H_RNG[0] = -t9*t11*0.5f;
H_RNG[1] = -t9*t13*0.5f; H_RNG[1] = -t9*t13*0.5f;
H_RNG[2] = -t9*t17*0.5f; H_RNG[2] = -t9*t17*0.5f;
// calculate the Kalman gains // calculate the Kalman gains
float t19 = receiverPosCov[0][0]*t9*t11*0.5f; ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
float t20 = receiverPosCov[1][1]*t9*t13*0.5f; ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
float t21 = receiverPosCov[0][1]*t9*t11*0.5f; ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
float t22 = receiverPosCov[2][1]*t9*t17*0.5f; ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
float t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
float t24 = t9*t13*t23*0.5f; ftype t24 = t9*t13*t23*0.5f;
float t25 = receiverPosCov[1][2]*t9*t13*0.5f; ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
float t26 = receiverPosCov[0][2]*t9*t11*0.5f; ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
float t27 = receiverPosCov[2][2]*t9*t17*0.5f; ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
float t28 = t25+t26+t27; ftype t28 = t25+t26+t27;
float t29 = t9*t17*t28*0.5f; ftype t29 = t9*t17*t28*0.5f;
float t30 = receiverPosCov[1][0]*t9*t13*0.5f; ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
float t31 = receiverPosCov[2][0]*t9*t17*0.5f; ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
float t32 = t19+t30+t31; ftype t32 = t19+t30+t31;
float t33 = t9*t11*t32*0.5f; ftype t33 = t9*t11*t32*0.5f;
varInnovRngBcn = R_RNG+t24+t29+t33; varInnovRngBcn = R_RNG+t24+t29+t33;
float t35 = 1.0f/varInnovRngBcn; ftype t35 = 1.0f/varInnovRngBcn;
float K_RNG[3]; 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[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[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); K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
// calculate range measurement innovation // calculate range measurement innovation
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosOffsetNED.z; deltaPosNED.z -= bcnPosOffsetNED.z;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted); rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted);
@ -489,7 +489,7 @@ void NavEKF3_core::FuseRngBcnStatic()
// ensure the covariance matrix is symmetric // ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) { for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) { 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[i][j] = temp;
receiverPosCov[j][i] = 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 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 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 // 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 // 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 // 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 // The main filter then uses the offset with the smaller innovations
float innov; // range measurement innovation (m) ftype innov; // range measurement innovation (m)
float innovVar; // range measurement innovation variance (m^2) ftype innovVar; // range measurement innovation variance (m^2)
float gain; // Kalman gain ftype gain; // Kalman gain
float obsDeriv; // derivative of observation relative to state ftype obsDeriv; // derivative of observation relative to state
const float stateNoiseVar = 0.1f; // State process noise variance const ftype stateNoiseVar = 0.1f; // State process noise variance
const float filtAlpha = 0.1f; // LPF constant const ftype filtAlpha = 0.1f; // LPF constant
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
// estimate upper value for offset // estimate upper value for offset
// calculate observation derivative // calculate observation derivative
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax; ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
float t5 = t2*t2; ftype t5 = t2*t2;
float t6 = t3*t3; ftype t6 = t3*t3;
float t7 = t4*t4; ftype t7 = t4*t4;
float t8 = t5+t6+t7; ftype t8 = t5+t6+t7;
float t9; ftype t9;
if (t8 > 0.1f) { if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8); t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng; innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// covariance prediction // covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar; bcnPosOffsetMaxVar += stateNoiseVar;
@ -563,8 +563,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar; gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset // calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain; ftype stateChange = innov * gain;
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
@ -585,11 +585,11 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
t5 = t2*t2; t5 = t2*t2;
t8 = t5+t6+t7; t8 = t5+t6+t7;
if (t8 > 0.1f) { if (t8 > 0.1f) {
t9 = 1.0f/sqrtf(t8); t9 = 1.0f/sqrtF(t8);
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng; innov = sqrtF(t8) - rngBcnDataDelayed.rng;
// covariance prediction // covariance prediction
bcnPosOffsetMinVar += stateNoiseVar; bcnPosOffsetMinVar += stateNoiseVar;
@ -602,8 +602,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar; gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset // calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain; ftype stateChange = innov * gain;
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || 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 // 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 // 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); 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 // 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) { if (gpsGoodToAlign) {
/* /*
@ -51,9 +51,9 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
const auto &gps = dal.gps(); const auto &gps = dal.gps();
const struct Location &gpsloc = gps.location(preferred_gps); // Current location 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 // 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; lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved // Sum distance moved
gpsDriftNE += gpsloc_prev.get_distance(gpsloc); gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
@ -81,8 +81,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsDataNew.have_vz && onGround) { if (gpsDataNew.have_vz && onGround) {
// check that the average vertical GPS velocity is close to zero // check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else { } else {
gpsVertVelFail = false; gpsVertVelFail = false;
} }
@ -91,7 +91,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsVertVelFail) { if (gpsVertVelFail) {
dal.snprintf(prearm_fail_string, dal.snprintf(prearm_fail_string,
sizeof(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; gpsCheckStatus.bad_vert_vel = true;
} else { } else {
gpsCheckStatus.bad_vert_vel = false; gpsCheckStatus.bad_vert_vel = false;
@ -102,8 +102,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
bool gpsHorizVelFail; bool gpsHorizVelFail;
if (onGround) { if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else { } else {
gpsHorizVelFail = false; 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 // 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 // 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 ftype 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 tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) { if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms; lastGpsCheckTime_ms = imuSampleTime_ms;
} }
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f; ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms; 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 // get the receivers reported speed accuracy
float gpsSpdAccRaw; float gpsSpdAccRaw;
@ -256,7 +256,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
} }
// filter the raw speed accuracy using a LPF // 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 // apply a peak hold filter to the LPF output
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight()
if (assume_zero_sideslip()) { 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 // 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 highGndSpd = false;
bool highAirSpd = false; bool highAirSpd = false;
bool largeHgtChange = false; bool largeHgtChange = false;
@ -319,13 +319,13 @@ void NavEKF3_core::detectFlight()
} }
// trigger on ground speed // trigger on ground speed
const float gndSpdThresholdSq = sq(5.0f); const ftype gndSpdThresholdSq = sq(5.0f);
if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) { if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) {
highGndSpd = true; highGndSpd = true;
} }
// trigger if more than 10m away from initial height // trigger if more than 10m away from initial height
if (fabsf(hgtMea) > 10.0f) { if (fabsF(hgtMea) > 10.0f) {
largeHgtChange = true; 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 #define GSF_YAW_VALID_HISTORY_THRESHOLD 5
// minimum variances allowed for velocity and position states // minimum variances allowed for velocity and position states
#define VEL_STATE_MIN_VARIANCE 1E-4f #define VEL_STATE_MIN_VARIANCE 1E-4
#define POS_STATE_MIN_VARIANCE 1E-4f #define POS_STATE_MIN_VARIANCE 1E-4
// maximum number of times the vertical velocity variance can hit the lower limit before the // maximum number of times the vertical velocity variance can hit the lower limit before the
// associated states, variances and covariances are reset // 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) #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 class NavEKF3_core : public NavEKF_core_common
{ {
public: public:
@ -407,7 +414,6 @@ private:
uint8_t imu_buffer_length; uint8_t imu_buffer_length;
uint8_t obs_buffer_length; uint8_t obs_buffer_length;
typedef float ftype;
#if MATH_CHECK_INDEXES #if MATH_CHECK_INDEXES
typedef VectorN<ftype,2> Vector2; typedef VectorN<ftype,2> Vector2;
typedef VectorN<ftype,3> Vector3; typedef VectorN<ftype,3> Vector3;
@ -461,14 +467,14 @@ private:
// broken down as individual elements. Both are equivalent (same // broken down as individual elements. Both are equivalent (same
// memory) // memory)
struct state_elements { struct state_elements {
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame 0..3 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 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 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 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 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 earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
Vector3f body_magfield; // body frame magnetic field vector (Gauss) 19..21 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 Vector2F wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
}; };
union { union {
@ -477,73 +483,73 @@ private:
}; };
struct output_elements { struct output_elements {
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame 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 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) Vector3F position; // position of body frame origin in local NED earth frame (m)
}; };
struct imu_elements { struct imu_elements {
Vector3f delAng; // IMU delta angle measurements in body frame (rad) Vector3F delAng; // IMU delta angle measurements in body frame (rad)
Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec) Vector3F delVel; // IMU delta velocity measurements in body frame (m/sec)
float delAngDT; // time interval over which delAng has been measured (sec) ftype delAngDT; // time interval over which delAng has been measured (sec)
float delVelDT; // time interval over which delVelDT has been measured (sec) ftype delVelDT; // time interval over which delVelDT has been measured (sec)
uint32_t time_ms; // measurement timestamp (msec) uint32_t time_ms; // measurement timestamp (msec)
uint8_t gyro_index; uint8_t gyro_index;
uint8_t accel_index; uint8_t accel_index;
}; };
struct gps_elements : EKF_obs_element_t { struct gps_elements : EKF_obs_element_t {
Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) 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) 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) Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
uint8_t sensor_idx; // unique integer identifying the GPS sensor 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 corrected; // true when the position and velocity have been corrected for sensor position
bool have_vz; // true when vertical velocity is valid bool have_vz; // true when vertical velocity is valid
}; };
struct mag_elements : EKF_obs_element_t { 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 { 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 { 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 uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
}; };
struct rng_bcn_elements : EKF_obs_element_t { struct rng_bcn_elements : EKF_obs_element_t {
float rng; // range measurement to each beacon (m) ftype rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m) Vector3F beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m) ftype rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number uint8_t beacon_ID; // beacon identification number
}; };
struct tas_elements : EKF_obs_element_t { 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 { 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 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) 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 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) Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m)
}; };
struct vel_odm_elements : EKF_obs_element_t { struct vel_odm_elements : EKF_obs_element_t {
Vector3f vel; // XYZ velocity measured in body frame (m/s) Vector3F vel; // XYZ velocity measured in body frame (m/s)
float velErr; // velocity measurement error 1-std (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 body_offset;// XYZ position of the velocity sensor in body frame (m)
Vector3f angRate; // angular rate estimated from odometry (rad/sec) Vector3F angRate; // angular rate estimated from odometry (rad/sec)
}; };
struct wheel_odm_elements : EKF_obs_element_t { 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) ftype delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
float radius; // wheel radius (m) ftype radius; // wheel radius (m)
Vector3f hub_offset; // XYZ position of the wheel hub in body frame (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 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 // 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 { struct yaw_elements : EKF_obs_element_t {
float yawAng; // yaw angle measurement (rad) ftype yawAng; // yaw angle measurement (rad)
float yawAngErr; // yaw angle 1SD measurement accuracy (rad) ftype yawAngErr; // yaw angle 1SD measurement accuracy (rad)
rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY) rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY)
}; };
struct ext_nav_elements : EKF_obs_element_t { struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos; // XYZ position measured in a RH navigation frame (m) Vector3F pos; // XYZ position measured in a RH navigation frame (m)
float posErr; // spherical position measurement error 1-std (m) ftype posErr; // spherical position measurement error 1-std (m)
bool posReset; // true when the position measurement has been reset bool posReset; // true when the position measurement has been reset
bool corrected; // true when the position has been corrected for sensor position bool corrected; // true when the position has been corrected for sensor position
}; };
struct ext_nav_vel_elements : EKF_obs_element_t { struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m/s) Vector3F vel; // velocity in NED (m/s)
float err; // velocity measurement error (m/s) ftype err; // velocity measurement error (m/s)
bool corrected; // true when the velocity has been corrected for sensor position 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 // bias estimates for the IMUs that are enabled but not being used
// by this core. // by this core.
struct { struct {
Vector3f gyro_bias; Vector3F gyro_bias;
Vector3f accel_bias; Vector3F accel_bias;
} inactiveBias[INS_MAX_INSTANCES]; } inactiveBias[INS_MAX_INSTANCES];
// Specify source of data to be used for a partial state reset // Specify source of data to be used for a partial state reset
@ -614,7 +620,7 @@ private:
// calculate the predicted state covariance matrix // calculate the predicted state covariance matrix
// Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states // 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. // 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 // force symmetry on the state covariance matrix
void ForceSymmetry(); void ForceSymmetry();
@ -641,7 +647,7 @@ private:
void FuseRngBcnStatic(); void FuseRngBcnStatic();
// calculate the offset from EKF vertical position datum to the range beacon system datum // 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 // fuse magnetometer measurements
void FuseMagnetometer(); void FuseMagnetometer();
@ -665,21 +671,21 @@ private:
void StoreQuatReset(void); void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation // 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 // 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 // initialise the covariance matrix
void CovarianceInit(); void CovarianceInit();
// helper functions for readIMUData // helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt); bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
// helper functions for correcting IMU data // helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index); void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index); void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
// update IMU delta angle and delta velocity measurements // update IMU delta angle and delta velocity measurements
void readIMUData(); void readIMUData();
@ -744,10 +750,10 @@ private:
void ResetPosition(resetDataSource posResetSource); void ResetPosition(resetDataSource posResetSource);
// reset the stateStruct's NE position to the specified position // 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 // reset the stateStruct's D position
void ResetPositionD(float posD); void ResetPositionD(ftype posD);
// reset velocity states using the last GPS measurement // reset velocity states using the last GPS measurement
void ResetVelocity(resetDataSource velResetSource); void ResetVelocity(resetDataSource velResetSource);
@ -768,7 +774,7 @@ private:
void checkDivergence(void); void checkDivergence(void);
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 // 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 // return true if the filter is ready to start using optical flow measurements for position and velocity estimation
bool readyToUseOptFlow(void) const; 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. // 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 // Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr); void FuseDeclination(ftype declErr);
// return magnetic declination in radians // 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 // Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer // 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 NE velocity measurement variance due to manoeuvre acceleration
// Scale factor applied to vertical 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 // 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 // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data. // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -921,14 +927,14 @@ private:
// yaw : new yaw angle (rad) // yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2) // yaw_variance : variance of new yaw angle (rad^2)
// order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle // 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 // attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful // returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw(); bool EKFGSF_resetMainFilterYaw();
// returns true on success and populates yaw (in radians) and yawVariance (rad^2) // 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 // Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
void FuseDragForces(); void FuseDragForces();
@ -948,7 +954,7 @@ private:
bool badIMUdata; // boolean true if the bad IMU data is detected 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 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 Matrix24 P; // covariance matrix
EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
EKF_obs_buffer_t<gps_elements> storedGPS; // GPS 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<tas_elements> storedTAS; // TAS data buffer
EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
EKF_IMU_buffer_t<output_elements> storedOutput;// output state 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 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) 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 dtIMUavg; // expected time between IMU measurements (sec)
ftype dtEkfAvg; // expected time between EKF updates (sec) ftype dtEkfAvg; // expected time between EKF updates (sec)
ftype dt; // time lapsed since the last covariance prediction (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 fuseVelData; // this boolean causes the velNED measurements to be fused
bool fusePosData; // this boolean causes the posNE 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 bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag; // innovation 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 Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance 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. ftype 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 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 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 MagCal effectiveMagCal; // the actual mag calibration being used as the default
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
@ -989,8 +995,8 @@ private:
ftype innovBeta; // synthetic sideslip innovation (rad) ftype innovBeta; // synthetic sideslip innovation (rad)
uint32_t lastMagUpdate_us; // last time compass was updated in usec uint32_t lastMagUpdate_us; // last time compass was updated in usec
uint32_t lastMagRead_ms; // last time compass data was successfully read uint32_t lastMagRead_ms; // last time compass data was successfully read
Vector3f velDotNED; // rate of change of velocity in NED frame Vector3F velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED Vector3F velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool tasDataToFuse; // true when new airspeed data is waiting to be fused bool tasDataToFuse; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms; // time last time we received baro height data 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 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 lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (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; uint32_t lastLaunchAccelTime_ms;
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold ftype 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 ftype 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 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 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 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 inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states are inactive bool inhibitMagStates; // true when magnetic field states are inactive
bool lastInhibitMagStates; // previous inhibitMagStates bool lastInhibitMagStates; // previous inhibitMagStates
@ -1021,15 +1027,15 @@ private:
bool gpsNotAvailable; // bool true when valid GPS data is not available bool gpsNotAvailable; // bool true when valid GPS data is not available
struct Location EKF_origin; // LLH origin of the NED axis system struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height 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 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 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 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 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 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 tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw 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 imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current 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 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 baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion 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 rangeDataNew; // Range finder data at the current time horizon
range_elements rangeDataDelayed;// Range finder data at the fusion 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 tasDataNew; // TAS data at the current time horizon
tas_elements tasDataDelayed; // TAS data at the fusion 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 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 mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
gps_elements gpsDataNew; // GPS data at the current 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 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 outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // 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 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 velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec) Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
float innovYaw; // compass yaw angle innovation (rad) ftype innovYaw; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms; // time last TAS data was received (msec) 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 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 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 airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip 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 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 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 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 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 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 ftype 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 QuaternionF 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 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 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 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 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 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 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) 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 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) Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned; // true when the magnetic field has been learned 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 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 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 bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned; // true when the gyro bias has been learned bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs 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) 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 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 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 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 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 firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (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 // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
struct { struct {
float pos; ftype pos;
float vel; ftype vel;
float acc; ftype acc;
} vertCompFiltState; } vertCompFiltState;
// variables used by the pre-initialisation GPS checks // variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev; // LLH location of previous GPS measurement 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 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 ftype 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 ftype 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 gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check // variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass 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 ftype 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 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 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 lastInnovPassTime_ms; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed 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. bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
Vector3f gpsVelInnov; // gps velocity innovations Vector3F gpsVelInnov; // gps velocity innovations
Vector3f gpsVelVarInnov; // gps velocity innovation variances Vector3F gpsVelVarInnov; // gps velocity innovation variances
uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts) uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts)
// variables added for optical flow fusion // variables added for optical flow fusion
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataNew; // OF data at the current time horizon
bool flowDataValid; // true while optical flow data is still fresh 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 flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range 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 flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (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 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
float Popt; // Optical flow terrain height state covariance (m^2) ftype Popt; // Optical flow terrain height state covariance (m^2)
float terrainState; // terrain position state (m) ftype terrainState; // terrain position state (m)
float prevPosN; // north position at last measurement ftype prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement ftype prevPosE; // east position at last measurement
float varInnovRng; // range finder observation innovation variance (m^2) ftype varInnovRng; // range finder observation innovation variance (m^2)
float innovRng; // range finder observation innovation (m) ftype innovRng; // range finder observation innovation (m)
float hgtMea; // height measurement derived from either baro, gps or range finder data (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 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 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 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 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 ftype 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 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 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 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 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. 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_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 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 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 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 ftype delTimeOF; // time that delAngBodyOF is summed across
bool flowFusionActive; // true when optical flow fusion is active 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 // Range finder
float baroHgtOffset; // offset applied when when switching to use of Baro height ftype baroHgtOffset; // offset applied when when switching to use of Baro height
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground ftype 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 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 storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors 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 EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon 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) 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 bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
float varInnovRngBcn; // range beacon observation innovation variance (m^2) ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m) ftype innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec) 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 bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED) Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr; // estimated position error from the beacon system (m) ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec) 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 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 uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
float receiverPosCov[3][3]; // Receiver position covariance (m^2) - 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 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 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 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 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 uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m) ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum 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 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) ftype bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
float maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh 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) ftype bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
float minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow 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 bool bcnOriginEstInit; // True when the beacon origin has been initialised
// Range Beacon Fusion Debug Reporting // Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct rngBcnFusionReport_t { struct rngBcnFusionReport_t {
float rng; // measured range to beacon (m) ftype rng; // measured range to beacon (m)
float innov; // range innovation (m) ftype innov; // range innovation (m)
float innovVar; // innovation variance (m^2) ftype innovVar; // innovation variance (m^2)
float testRatio; // innovation consistency test ratio ftype testRatio; // innovation consistency test ratio
Vector3f beaconPosNED; // beacon NED position Vector3F beaconPosNED; // beacon NED position
} *rngBcnFusionReport; } *rngBcnFusionReport;
#if EK3_FEATURE_DRAG_FUSION #if EK3_FEATURE_DRAG_FUSION
@ -1265,10 +1271,10 @@ private:
drag_elements dragSampleDelayed; drag_elements dragSampleDelayed;
drag_elements dragDownSampled; // down sampled from filter prediction rate to observation rate 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 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) ftype dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
Vector2f innovDrag; // multirotor drag measurement innovation (m/sec**2) Vector2F innovDrag; // multirotor drag measurement innovation (m/sec**2)
Vector2f innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2) Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
Vector2f dragTestRatio; // drag innovation consistency check ratio Vector2F dragTestRatio; // drag innovation consistency check ratio
#endif #endif
bool dragFusionEnabled; bool dragFusionEnabled;
@ -1278,11 +1284,11 @@ private:
// Movement detector // Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected 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 uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// baro ground effect // 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 // control of post takeoff magnetic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed 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 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 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 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) ftype 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) ftype 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 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 // Used by on ground movement check required when operating on ground without a yaw reference
float gyro_diff; // filtered gyro difference (rad/s) ftype gyro_diff; // filtered gyro difference (rad/s)
float accel_diff; // filtered acceerometer difference (m/s/s) ftype accel_diff; // filtered acceerometer difference (m/s/s)
Vector3f gyro_prev; // gyro vector from previous time step (rad/s) Vector3F gyro_prev; // gyro vector from previous time step (rad/s)
Vector3f accel_prev; // accelerometer vector from previous time step (m/s/s) Vector3F accel_prev; // accelerometer vector from previous time step (m/s/s)
bool onGroundNotMoving; // true when on the ground and not moving bool onGroundNotMoving; // true when on the ground and not moving
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec) uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
// variables used to inhibit accel bias learning // variables used to inhibit accel bias learning
bool inhibitDelVelBiasStates; // true when all IMU delta velocity bias states are de-activated 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 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 #if EK3_FEATURE_EXTERNAL_NAV
// external navigation fusion // 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 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) 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 bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
Vector3f extNavVelInnov; // external nav velocity innovations Vector3F extNavVelInnov; // external nav velocity innovations
Vector3f extNavVelVarInnov; // external nav velocity innovation variances Vector3F extNavVelVarInnov; // external nav velocity innovation variances
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts) 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 EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
@ -1384,8 +1390,8 @@ private:
ftype magXbias; ftype magXbias;
ftype magYbias; ftype magYbias;
ftype magZbias; ftype magZbias;
Matrix3f DCM; Matrix3F DCM;
Vector3f MagPred; Vector3F MagPred;
ftype R_MAG; ftype R_MAG;
Vector9 SH_MAG; Vector9 SH_MAG;
} mag_state; } mag_state;
@ -1395,8 +1401,8 @@ private:
// earth field from WMM tables // earth field from WMM tables
bool have_table_earth_field; // true when we have initialised table_earth_field_ga bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga; // earth field from WMM tables Vector3F table_earth_field_ga; // earth field from WMM tables
float table_declination; // declination in radians from the tables ftype table_declination; // declination in radians from the tables
// timing statistics // timing statistics
struct ekf_timing timing; struct ekf_timing timing;
@ -1408,7 +1414,7 @@ private:
bool assume_zero_sideslip(void) const; bool assume_zero_sideslip(void) const;
// vehicle specific initial gyro bias uncertainty // vehicle specific initial gyro bias uncertainty
float InitialGyroBiasUncertainty(void) const; ftype InitialGyroBiasUncertainty(void) const;
/* /*
learn magnetometer biases from GPS yaw. Return true if the learn magnetometer biases from GPS yaw. Return true if the