mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: allow for double EKF build
This commit is contained in:
parent
380b9b39ed
commit
a6c5b65f4a
|
@ -1057,7 +1057,7 @@ void NavEKF3::updateCoreRelativeErrors()
|
|||
// reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes
|
||||
if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
|
||||
coreRelativeErrors[i] += error;
|
||||
coreRelativeErrors[i] = constrain_float(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
|
||||
coreRelativeErrors[i] = constrain_ftype(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
File diff suppressed because one or more lines are too long
|
@ -71,15 +71,15 @@ void NavEKF3_core::setWindMagStateLearningMode()
|
|||
if (yawAlignComplete && useAirspeed()) {
|
||||
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
|
||||
// which assumes the vehicle has launched into the wind
|
||||
Vector3f tempEuler;
|
||||
Vector3F tempEuler;
|
||||
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
|
||||
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
|
||||
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
|
||||
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
|
||||
ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
|
||||
stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
|
||||
stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
|
||||
|
||||
// set the wind state variances to the measurement uncertainty
|
||||
for (uint8_t index=22; index<=23; index++) {
|
||||
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f));
|
||||
P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f));
|
||||
}
|
||||
} else {
|
||||
// set the variances using a typical wind speed
|
||||
|
@ -232,7 +232,7 @@ void NavEKF3_core::setAidingMode()
|
|||
}
|
||||
}
|
||||
// keep the IMU bias state variances, but zero the covariances
|
||||
float oldBiasVariance[6];
|
||||
ftype oldBiasVariance[6];
|
||||
for (uint8_t row=0; row<6; row++) {
|
||||
oldBiasVariance[row] = P[row+10][row+10];
|
||||
}
|
||||
|
@ -407,7 +407,7 @@ void NavEKF3_core::setAidingMode()
|
|||
}
|
||||
// handle height reset as special case
|
||||
hgtMea = -extNavDataDelayed.pos.z;
|
||||
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
||||
posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
||||
ResetHeight();
|
||||
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||
}
|
||||
|
@ -438,7 +438,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
|
|||
// Once the tilt variances have reduced, re-set the yaw and magnetic field states
|
||||
// and declare the tilt alignment complete
|
||||
if (!tiltAlignComplete) {
|
||||
if (tiltErrorVariance < sq(radians(5.0f))) {
|
||||
if (tiltErrorVariance < sq(radians(5.0))) {
|
||||
tiltAlignComplete = true;
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index);
|
||||
}
|
||||
|
@ -631,16 +631,16 @@ void NavEKF3_core::recordYawReset()
|
|||
void NavEKF3_core::checkGyroCalStatus(void)
|
||||
{
|
||||
// check delta angle bias variances
|
||||
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
|
||||
const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg));
|
||||
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
|
||||
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
|
||||
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
|
||||
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
|
||||
// which can make this check fail
|
||||
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);
|
||||
Vector3f temp = prevTnb * delAngBiasVarVec;
|
||||
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) &&
|
||||
(fabsf(temp.y) < delAngBiasVarMax);
|
||||
Vector3F delAngBiasVarVec = Vector3F(P[10][10],P[11][11],P[12][12]);
|
||||
Vector3F temp = prevTnb * delAngBiasVarVec;
|
||||
delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
|
||||
(fabsF(temp.y) < delAngBiasVarMax);
|
||||
} else {
|
||||
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax) &&
|
||||
|
@ -697,7 +697,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
|
|||
return;
|
||||
}
|
||||
|
||||
float trueAirspeed;
|
||||
ftype trueAirspeed;
|
||||
if (assume_zero_sideslip()) {
|
||||
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
|
||||
} else {
|
||||
|
@ -720,13 +720,13 @@ void NavEKF3_core::runYawEstimatorCorrection()
|
|||
|
||||
if (EKFGSF_run_filterbank) {
|
||||
if (gpsDataToFuse) {
|
||||
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
|
||||
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
|
||||
Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
|
||||
ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
|
||||
yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
|
||||
|
||||
// after velocity data has been fused the yaw variance estimate will have been refreshed and
|
||||
// is used maintain a history of validity
|
||||
float gsfYaw, gsfYawVariance;
|
||||
ftype gsfYaw, gsfYawVariance;
|
||||
if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
|
||||
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
|
||||
EKFGSF_yaw_valid_count++;
|
||||
|
|
|
@ -17,7 +17,7 @@ void NavEKF3_core::resetGyroBias(void)
|
|||
/*
|
||||
vehicle specific initial gyro bias uncertainty in deg/sec
|
||||
*/
|
||||
float NavEKF3_core::InitialGyroBiasUncertainty(void) const
|
||||
ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const
|
||||
{
|
||||
return 2.5f;
|
||||
}
|
||||
|
|
|
@ -6,6 +6,8 @@
|
|||
|
||||
#include <AP_DAL/AP_DAL.h>
|
||||
|
||||
#pragma GCC diagnostic ignored "-Wnarrowing"
|
||||
|
||||
void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
|
||||
{
|
||||
// Write first EKF packet
|
||||
|
@ -147,7 +149,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
|
|||
|
||||
nav_filter_status solutionStatus {};
|
||||
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
||||
float tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
|
||||
getFilterFaults(_faultStatus);
|
||||
getFilterStatus(solutionStatus);
|
||||
const struct log_NKF4 pkt4{
|
||||
|
@ -159,7 +161,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
|
|||
sqrtvarH : (int16_t)(100*hgtVar),
|
||||
sqrtvarM : (int16_t)(100*tempVar),
|
||||
sqrtvarVT : (int16_t)(100*tasVar),
|
||||
tiltErr : sqrtf(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
|
||||
tiltErr : sqrtF(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
|
||||
offsetNorth : offset.x,
|
||||
offsetEast : offset.y,
|
||||
faults : _faultStatus,
|
||||
|
@ -191,7 +193,7 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
|
|||
offset : (int16_t)(100*terrainState), // filter ground offset state error
|
||||
RI : (int16_t)(100*innovRng), // range finder innovations
|
||||
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
|
||||
errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
angErr : (float)outputTrackError.x, // output predictor angle error
|
||||
velErr : (float)outputTrackError.y, // output predictor velocity error
|
||||
posErr : (float)outputTrackError.z // output predictor position tracking error
|
||||
|
@ -248,8 +250,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
|
|||
ID : rngBcnFuseDataReportIndex,
|
||||
rng : (int16_t)(100*report.rng),
|
||||
innov : (int16_t)(100*report.innov),
|
||||
sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)),
|
||||
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)),
|
||||
sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
|
||||
testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
|
||||
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
|
||||
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
|
||||
beaconPosD : (int16_t)(100*report.beaconPosNED.z),
|
||||
|
|
|
@ -25,8 +25,8 @@ void NavEKF3_core::controlMagYawReset()
|
|||
}
|
||||
|
||||
// Quaternion and delta rotation vector that are re-used for different calculations
|
||||
Vector3f deltaRotVecTemp;
|
||||
Quaternion deltaQuatTemp;
|
||||
Vector3F deltaRotVecTemp;
|
||||
QuaternionF deltaQuatTemp;
|
||||
|
||||
bool flightResetAllowed = false;
|
||||
bool initialResetAllowed = false;
|
||||
|
@ -61,7 +61,7 @@ void NavEKF3_core::controlMagYawReset()
|
|||
|
||||
// check for increasing height
|
||||
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
|
||||
float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset);
|
||||
ftype yawInnovIncrease = fabsF(innovYaw) - fabsF(yawInnovAtLastMagReset);
|
||||
|
||||
// check for increasing yaw innovations
|
||||
bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
|
||||
|
@ -131,18 +131,18 @@ void NavEKF3_core::controlMagYawReset()
|
|||
void NavEKF3_core::realignYawGPS()
|
||||
{
|
||||
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
|
||||
Vector3f eulerAngles;
|
||||
Vector3F eulerAngles;
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
|
||||
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
|
||||
// calculate course yaw angle
|
||||
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
|
||||
ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x);
|
||||
|
||||
// calculate course yaw angle from GPS velocity
|
||||
float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
|
||||
ftype gpsYaw = atan2F(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
|
||||
|
||||
// Check the yaw angles for consistency
|
||||
float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z)));
|
||||
ftype yawErr = MAX(fabsF(wrap_PI(gpsYaw - velYaw)),fabsF(wrap_PI(gpsYaw - eulerAngles.z)));
|
||||
|
||||
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
|
||||
bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
|
||||
|
@ -180,7 +180,7 @@ void NavEKF3_core::realignYawGPS()
|
|||
void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
|
||||
{
|
||||
// update quaternion states and covariances
|
||||
resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2f)), yawAngData.order);
|
||||
resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2)), yawAngData.order);
|
||||
|
||||
// send yaw alignment information to console
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
|
||||
|
@ -206,14 +206,14 @@ void NavEKF3_core::SelectMagFusion()
|
|||
|
||||
// Store yaw angle when moving for use as a static reference when not moving
|
||||
if (!onGroundNotMoving) {
|
||||
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
|
||||
if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) {
|
||||
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
|
||||
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321;
|
||||
yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
|
||||
yawAngDataStatic.yawAng = atan2F(prevTnb[0][1], prevTnb[0][0]);
|
||||
} else {
|
||||
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
|
||||
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312;
|
||||
yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
|
||||
yawAngDataStatic.yawAng = atan2F(-prevTnb[1][0], prevTnb[1][1]);
|
||||
}
|
||||
yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
|
||||
yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
|
||||
|
@ -236,7 +236,7 @@ void NavEKF3_core::SelectMagFusion()
|
|||
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
|
||||
// use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
|
||||
// for non fixed wing platform types
|
||||
float gsfYaw, gsfYawVariance;
|
||||
ftype gsfYaw, gsfYawVariance;
|
||||
const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
|
||||
|
||||
// fallback methods
|
||||
|
@ -453,8 +453,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
ftype &magXbias = mag_state.magXbias;
|
||||
ftype &magYbias = mag_state.magYbias;
|
||||
ftype &magZbias = mag_state.magZbias;
|
||||
Matrix3f &DCM = mag_state.DCM;
|
||||
Vector3f &MagPred = mag_state.MagPred;
|
||||
Matrix3F &DCM = mag_state.DCM;
|
||||
Vector3F &MagPred = mag_state.MagPred;
|
||||
ftype &R_MAG = mag_state.R_MAG;
|
||||
ftype *SH_MAG = &mag_state.SH_MAG[0];
|
||||
Vector24 H_MAG;
|
||||
|
@ -503,7 +503,7 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
}
|
||||
|
||||
// scale magnetometer observation error with total angular rate to allow for timing errors
|
||||
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
|
||||
R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
|
||||
|
||||
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
|
@ -555,7 +555,7 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
|
||||
// calculate the innovation test ratios
|
||||
for (uint8_t i = 0; i<=2; i++) {
|
||||
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
|
||||
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
|
||||
}
|
||||
|
||||
// check the last values from all components and set magnetometer health accordingly
|
||||
|
@ -605,8 +605,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -619,8 +619,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
// zero Kalman gains to inhibit magnetic field state estimation
|
||||
if (!inhibitMagStates) {
|
||||
|
@ -631,8 +631,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
// zero Kalman gains to inhibit wind state estimation
|
||||
|
@ -640,8 +640,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23 = 2
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
||||
|
@ -685,8 +685,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -699,8 +699,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
// zero Kalman gains to inhibit magnetic field state estimation
|
||||
|
@ -712,8 +712,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
// zero Kalman gains to inhibit wind state estimation
|
||||
|
@ -721,8 +721,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
||||
|
@ -767,8 +767,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -781,8 +781,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
// zero Kalman gains to inhibit magnetic field state estimation
|
||||
|
@ -794,8 +794,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
// zero Kalman gains to inhibit wind state estimation
|
||||
|
@ -803,8 +803,8 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
||||
|
@ -897,12 +897,12 @@ void NavEKF3_core::FuseMagnetometer()
|
|||
*/
|
||||
bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
||||
{
|
||||
const float &q0 = stateStruct.quat[0];
|
||||
const float &q1 = stateStruct.quat[1];
|
||||
const float &q2 = stateStruct.quat[2];
|
||||
const float &q3 = stateStruct.quat[3];
|
||||
const ftype &q0 = stateStruct.quat[0];
|
||||
const ftype &q1 = stateStruct.quat[1];
|
||||
const ftype &q2 = stateStruct.quat[2];
|
||||
const ftype &q3 = stateStruct.quat[3];
|
||||
|
||||
float gsfYaw, gsfYawVariance;
|
||||
ftype gsfYaw, gsfYawVariance;
|
||||
if (method == yawFusionMethod::GSF) {
|
||||
if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
|
||||
return false;
|
||||
|
@ -910,7 +910,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// yaw measurement error variance (rad^2)
|
||||
float R_YAW;
|
||||
ftype R_YAW;
|
||||
switch (method) {
|
||||
case yawFusionMethod::GPS:
|
||||
R_YAW = sq(yawAngDataDelayed.yawAngErr);
|
||||
|
@ -953,7 +953,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
case yawFusionMethod::PREDICTED:
|
||||
default:
|
||||
// determined automatically
|
||||
order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
|
||||
order = (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
|
||||
break;
|
||||
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
|
@ -964,53 +964,53 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
|
||||
float yawAngPredicted;
|
||||
float H_YAW[4];
|
||||
Matrix3f Tbn_zeroYaw;
|
||||
ftype yawAngPredicted;
|
||||
ftype H_YAW[4];
|
||||
Matrix3F Tbn_zeroYaw;
|
||||
|
||||
if (order == rotationOrder::TAIT_BRYAN_321) {
|
||||
// calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
|
||||
bool canUseA = false;
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 + SA1*q1;
|
||||
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
float SA4, SA5_inv;
|
||||
const ftype SA0 = 2*q3;
|
||||
const ftype SA1 = 2*q2;
|
||||
const ftype SA2 = SA0*q0 + SA1*q1;
|
||||
const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
ftype SA4, SA5_inv;
|
||||
if (is_positive(sq(SA3))) {
|
||||
SA4 = 1.0F/sq(SA3);
|
||||
SA5_inv = sq(SA2)*SA4 + 1;
|
||||
canUseA = is_positive(fabsf(SA5_inv));
|
||||
canUseA = is_positive(fabsF(SA5_inv));
|
||||
}
|
||||
|
||||
bool canUseB = false;
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = SB0*q3 + SB1*q2;
|
||||
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
float SB3, SB5_inv;
|
||||
const ftype SB0 = 2*q0;
|
||||
const ftype SB1 = 2*q1;
|
||||
const ftype SB2 = SB0*q3 + SB1*q2;
|
||||
const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
ftype SB3, SB5_inv;
|
||||
if (is_positive(sq(SB2))) {
|
||||
SB3 = 1.0F/sq(SB2);
|
||||
SB5_inv = SB3*sq(SB4) + 1;
|
||||
canUseB = is_positive(fabsf(SB5_inv));
|
||||
canUseB = is_positive(fabsF(SB5_inv));
|
||||
}
|
||||
|
||||
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
|
||||
const float SA5 = 1.0F/SA5_inv;
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
|
||||
const ftype SA5 = 1.0F/SA5_inv;
|
||||
const ftype SA6 = 1.0F/SA3;
|
||||
const ftype SA7 = SA2*SA4;
|
||||
const ftype SA8 = 2*SA7;
|
||||
const ftype SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
|
||||
H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
|
||||
const float SB5 = 1.0F/SB5_inv;
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
} else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
|
||||
const ftype SB5 = 1.0F/SB5_inv;
|
||||
const ftype SB6 = 1.0F/SB2;
|
||||
const ftype SB7 = SB3*SB4;
|
||||
const ftype SB8 = 2*SB7;
|
||||
const ftype SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
|
@ -1021,7 +1021,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// Get the 321 euler angles
|
||||
Vector3f euler321;
|
||||
Vector3F euler321;
|
||||
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
|
||||
yawAngPredicted = euler321.z;
|
||||
|
||||
|
@ -1031,46 +1031,46 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
} else if (order == rotationOrder::TAIT_BRYAN_312) {
|
||||
// calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
|
||||
bool canUseA = false;
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 - SA1*q1;
|
||||
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
float SA4, SA5_inv;
|
||||
const ftype SA0 = 2*q3;
|
||||
const ftype SA1 = 2*q2;
|
||||
const ftype SA2 = SA0*q0 - SA1*q1;
|
||||
const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
ftype SA4, SA5_inv;
|
||||
if (is_positive(sq(SA3))) {
|
||||
SA4 = 1.0F/sq(SA3);
|
||||
SA5_inv = sq(SA2)*SA4 + 1;
|
||||
canUseA = is_positive(fabsf(SA5_inv));
|
||||
canUseA = is_positive(fabsF(SA5_inv));
|
||||
}
|
||||
|
||||
bool canUseB = false;
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = -SB0*q3 + SB1*q2;
|
||||
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
float SB3, SB5_inv;
|
||||
const ftype SB0 = 2*q0;
|
||||
const ftype SB1 = 2*q1;
|
||||
const ftype SB2 = -SB0*q3 + SB1*q2;
|
||||
const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
ftype SB3, SB5_inv;
|
||||
if (is_positive(sq(SB2))) {
|
||||
SB3 = 1.0F/sq(SB2);
|
||||
SB5_inv = SB3*sq(SB4) + 1;
|
||||
canUseB = is_positive(fabsf(SB5_inv));
|
||||
canUseB = is_positive(fabsF(SB5_inv));
|
||||
}
|
||||
|
||||
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
|
||||
const float SA5 = 1.0F/SA5_inv;
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
|
||||
const ftype SA5 = 1.0F/SA5_inv;
|
||||
const ftype SA6 = 1.0F/SA3;
|
||||
const ftype SA7 = SA2*SA4;
|
||||
const ftype SA8 = 2*SA7;
|
||||
const ftype SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
|
||||
H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
|
||||
const float SB5 = 1.0F/SB5_inv;
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
} else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
|
||||
const ftype SB5 = 1.0F/SB5_inv;
|
||||
const ftype SB6 = 1.0F/SB2;
|
||||
const ftype SB7 = SB3*SB4;
|
||||
const ftype SB8 = 2*SB7;
|
||||
const ftype SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
|
@ -1081,7 +1081,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// Get the 312 Tait Bryan rotation angles
|
||||
Vector3f euler312 = stateStruct.quat.to_vector312();
|
||||
Vector3F euler312 = stateStruct.quat.to_vector312();
|
||||
yawAngPredicted = euler312.z;
|
||||
|
||||
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
|
||||
|
@ -1097,8 +1097,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
{
|
||||
// Use the difference between the horizontal projection and declination to give the measured yaw
|
||||
// rotate measured mag components into earth frame
|
||||
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
|
||||
float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
||||
Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
|
||||
ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
||||
innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured);
|
||||
break;
|
||||
}
|
||||
|
@ -1128,8 +1128,8 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
|
||||
float PH[4];
|
||||
float varInnov = R_YAW;
|
||||
ftype PH[4];
|
||||
ftype varInnov = R_YAW;
|
||||
for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) {
|
||||
PH[rowIndex] = 0.0f;
|
||||
for (uint8_t colIndex=0; colIndex<=3; colIndex++) {
|
||||
|
@ -1137,7 +1137,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
varInnov += H_YAW[rowIndex]*PH[rowIndex];
|
||||
}
|
||||
float varInnovInv;
|
||||
ftype varInnovInv;
|
||||
if (varInnov >= R_YAW) {
|
||||
varInnovInv = 1.0f / varInnov;
|
||||
// output numerical health status
|
||||
|
@ -1161,7 +1161,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
|
||||
// calculate the innovation test ratio
|
||||
yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov);
|
||||
yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov);
|
||||
|
||||
// Declare the magnetometer unhealthy if the innovation test fails
|
||||
if (yawTestRatio > 1.0f) {
|
||||
|
@ -1184,7 +1184,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
}
|
||||
for (uint8_t row = 0; row <= stateIndexLim; row++) {
|
||||
for (uint8_t column = 0; column <= stateIndexLim; column++) {
|
||||
float tmp = KH[row][0] * P[0][column];
|
||||
ftype tmp = KH[row][0] * P[0][column];
|
||||
tmp += KH[row][1] * P[1][column];
|
||||
tmp += KH[row][2] * P[2][column];
|
||||
tmp += KH[row][3] * P[3][column];
|
||||
|
@ -1213,7 +1213,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
|
||||
// correct the state vector
|
||||
for (uint8_t i=0; i<=stateIndexLim; i++) {
|
||||
statesArray[i] -= Kfusion[i] * constrain_float(innovYaw, -0.5f, 0.5f);
|
||||
statesArray[i] -= Kfusion[i] * constrain_ftype(innovYaw, -0.5f, 0.5f);
|
||||
}
|
||||
stateStruct.quat.normalize();
|
||||
|
||||
|
@ -1234,14 +1234,14 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
|
|||
* This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
|
||||
* or some other absolute position or velocity reference
|
||||
*/
|
||||
void NavEKF3_core::FuseDeclination(float declErr)
|
||||
void NavEKF3_core::FuseDeclination(ftype declErr)
|
||||
{
|
||||
// declination error variance (rad^2)
|
||||
const float R_DECL = sq(declErr);
|
||||
const ftype R_DECL = sq(declErr);
|
||||
|
||||
// copy required states to local variables
|
||||
float magN = stateStruct.earth_magfield.x;
|
||||
float magE = stateStruct.earth_magfield.y;
|
||||
ftype magN = stateStruct.earth_magfield.x;
|
||||
ftype magE = stateStruct.earth_magfield.y;
|
||||
|
||||
// prevent bad earth field states from causing numerical errors or exceptions
|
||||
if (magN < 1e-3f) {
|
||||
|
@ -1250,34 +1250,34 @@ void NavEKF3_core::FuseDeclination(float declErr)
|
|||
|
||||
// Calculate observation Jacobian and Kalman gains
|
||||
// Calculate intermediate variables
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
ftype t2 = magE*magE;
|
||||
ftype t3 = magN*magN;
|
||||
ftype t4 = t2+t3;
|
||||
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
|
||||
if (t4 < 1e-4f) {
|
||||
return;
|
||||
}
|
||||
float t5 = P[16][16]*t2;
|
||||
float t6 = P[17][17]*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P[16][17]*magE*magN;
|
||||
float t15 = P[17][16]*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13;
|
||||
if (fabsf(t12) > 1e-6f) {
|
||||
ftype t5 = P[16][16]*t2;
|
||||
ftype t6 = P[17][17]*t3;
|
||||
ftype t7 = t2*t2;
|
||||
ftype t8 = R_DECL*t7;
|
||||
ftype t9 = t3*t3;
|
||||
ftype t10 = R_DECL*t9;
|
||||
ftype t11 = R_DECL*t2*t3*2.0f;
|
||||
ftype t14 = P[16][17]*magE*magN;
|
||||
ftype t15 = P[17][16]*magE*magN;
|
||||
ftype t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
ftype t13;
|
||||
if (fabsF(t12) > 1e-6f) {
|
||||
t13 = 1.0f / t12;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t18 = magE*magE;
|
||||
float t19 = magN*magN;
|
||||
float t20 = t18+t19;
|
||||
float t21;
|
||||
if (fabsf(t20) > 1e-6f) {
|
||||
ftype t18 = magE*magE;
|
||||
ftype t19 = magN*magN;
|
||||
ftype t20 = t18+t19;
|
||||
ftype t21;
|
||||
if (fabsF(t20) > 1e-6f) {
|
||||
t21 = 1.0f/t20;
|
||||
} else {
|
||||
return;
|
||||
|
@ -1285,7 +1285,7 @@ void NavEKF3_core::FuseDeclination(float declErr)
|
|||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
float H_DECL[24] = {};
|
||||
ftype H_DECL[24] = {};
|
||||
H_DECL[16] = -magE*t21;
|
||||
H_DECL[17] = magN*t21;
|
||||
|
||||
|
@ -1305,8 +1305,8 @@ void NavEKF3_core::FuseDeclination(float declErr)
|
|||
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
|
||||
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -1319,8 +1319,8 @@ void NavEKF3_core::FuseDeclination(float declErr)
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
|
@ -1331,23 +1331,23 @@ void NavEKF3_core::FuseDeclination(float declErr)
|
|||
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
|
||||
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
|
||||
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
// get the magnetic declination
|
||||
float magDecAng = MagDeclination();
|
||||
ftype magDecAng = MagDeclination();
|
||||
|
||||
// Calculate the innovation
|
||||
float innovation = atan2f(magE , magN) - magDecAng;
|
||||
ftype innovation = atan2F(magE , magN) - magDecAng;
|
||||
|
||||
// limit the innovation to protect against data errors
|
||||
if (innovation > 0.5f) {
|
||||
|
@ -1422,18 +1422,18 @@ void NavEKF3_core::alignMagStateDeclination()
|
|||
}
|
||||
|
||||
// get the magnetic declination
|
||||
float magDecAng = MagDeclination();
|
||||
ftype magDecAng = MagDeclination();
|
||||
|
||||
// rotate the NE values so that the declination matches the published value
|
||||
Vector3f initMagNED = stateStruct.earth_magfield;
|
||||
float magLengthNE = norm(initMagNED.x,initMagNED.y);
|
||||
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
|
||||
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
|
||||
Vector3F initMagNED = stateStruct.earth_magfield;
|
||||
ftype magLengthNE = norm(initMagNED.x,initMagNED.y);
|
||||
stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
|
||||
stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
// zero the corresponding state covariances if magnetic field state learning is active
|
||||
float var_16 = P[16][16];
|
||||
float var_17 = P[17][17];
|
||||
ftype var_16 = P[16][16];
|
||||
ftype var_17 = P[17][17];
|
||||
zeroRows(P,16,17);
|
||||
zeroCols(P,16,17);
|
||||
P[16][16] = var_16;
|
||||
|
@ -1484,13 +1484,13 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
|
|||
}
|
||||
|
||||
// combine yaw with current quaternion to get yaw corrected quaternion
|
||||
Quaternion quat = stateStruct.quat;
|
||||
QuaternionF quat = stateStruct.quat;
|
||||
if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) {
|
||||
Vector3f euler321;
|
||||
Vector3F euler321;
|
||||
quat.to_euler(euler321.x, euler321.y, euler321.z);
|
||||
quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng);
|
||||
} else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) {
|
||||
Vector3f euler312 = quat.to_vector312();
|
||||
Vector3F euler312 = quat.to_vector312();
|
||||
quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng);
|
||||
} else {
|
||||
// rotation order not supported
|
||||
|
@ -1498,19 +1498,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
|
|||
}
|
||||
|
||||
// build the expected body field from orientation and table earth field
|
||||
Matrix3f dcm;
|
||||
Matrix3F dcm;
|
||||
quat.rotation_matrix(dcm);
|
||||
Vector3f expected_body_field = dcm.transposed() * table_earth_field_ga;
|
||||
Vector3F expected_body_field = dcm.transposed() * table_earth_field_ga;
|
||||
|
||||
// calculate error in field
|
||||
Vector3f err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield;
|
||||
Vector3F err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield;
|
||||
|
||||
// learn body frame mag biases
|
||||
stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE;
|
||||
|
||||
// check if error is below threshold. If it is then we can
|
||||
// fallback to magnetometer on failure of external yaw
|
||||
float err_length = err.length();
|
||||
ftype err_length = err.length();
|
||||
|
||||
// we allow for yaw backback to compass if we have had 50 samples
|
||||
// in a row below the threshold. This corresponds to 10 seconds
|
||||
|
@ -1540,7 +1540,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
|
|||
return false;
|
||||
};
|
||||
|
||||
float yawEKFGSF, yawVarianceEKFGSF;
|
||||
ftype yawEKFGSF, yawVarianceEKFGSF;
|
||||
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
|
||||
// keep roll and pitch and reset yaw
|
||||
rotationOrder order;
|
||||
|
@ -1582,14 +1582,14 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
|
|||
}
|
||||
|
||||
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
|
||||
bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const
|
||||
bool NavEKF3_core::EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const
|
||||
{
|
||||
// return immediately if no yaw estimator
|
||||
if (yawEstimator == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float velInnovLength;
|
||||
ftype velInnovLength;
|
||||
if (yawEstimator->getYawData(yaw, yawVariance) &&
|
||||
is_positive(yawVariance) &&
|
||||
yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
|
||||
|
@ -1600,14 +1600,14 @@ bool NavEKF3_core::EKFGSF_getYaw(float &yaw, float &yawVariance) const
|
|||
return false;
|
||||
}
|
||||
|
||||
void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order)
|
||||
void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order)
|
||||
{
|
||||
Quaternion quatBeforeReset = stateStruct.quat;
|
||||
QuaternionF quatBeforeReset = stateStruct.quat;
|
||||
|
||||
// check if we should use a 321 or 312 Rotation order and update the quaternion
|
||||
// states using the preferred yaw definition
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
Vector3f eulerAngles;
|
||||
Vector3F eulerAngles;
|
||||
if (order == rotationOrder::TAIT_BRYAN_321) {
|
||||
// rolled more than pitched so use 321 rotation order
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
|
@ -1624,14 +1624,14 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationO
|
|||
// Update the rotation matrix
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
|
||||
float deltaYaw = wrap_PI(yaw - eulerAngles.z);
|
||||
ftype deltaYaw = wrap_PI(yaw - eulerAngles.z);
|
||||
|
||||
// calculate the change in the quaternion state and apply it to the output history buffer
|
||||
Quaternion quat_delta = stateStruct.quat / quatBeforeReset;
|
||||
QuaternionF quat_delta = stateStruct.quat / quatBeforeReset;
|
||||
StoreQuatRotate(quat_delta);
|
||||
|
||||
// assume tilt uncertainty split equally between roll and pitch
|
||||
Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, yawVariance);
|
||||
Vector3F angleErrVarVec = Vector3F(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, yawVariance);
|
||||
CovariancePrediction(&angleErrVarVec);
|
||||
|
||||
// record the yaw reset event
|
||||
|
|
|
@ -128,10 +128,10 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
|
|||
// subtract delay from timestamp
|
||||
timeStamp_ms -= delay_ms;
|
||||
|
||||
bodyOdmDataNew.body_offset = posOffset;
|
||||
bodyOdmDataNew.vel = delPos * (1.0f/delTime);
|
||||
bodyOdmDataNew.body_offset = posOffset.toftype();
|
||||
bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
|
||||
bodyOdmDataNew.time_ms = timeStamp_ms;
|
||||
bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
|
||||
bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
|
||||
bodyOdmMeasTime_ms = timeStamp_ms;
|
||||
|
||||
// simple model of accuracy
|
||||
|
@ -157,7 +157,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
|
|||
}
|
||||
|
||||
wheel_odm_elements wheelOdmDataNew = {};
|
||||
wheelOdmDataNew.hub_offset = posOffset;
|
||||
wheelOdmDataNew.hub_offset = posOffset.toftype();
|
||||
wheelOdmDataNew.delAng = delAng;
|
||||
wheelOdmDataNew.radius = radius;
|
||||
wheelOdmDataNew.delTime = delTime;
|
||||
|
@ -190,8 +190,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
|
|||
// reset the accumulated body delta angle and time
|
||||
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
|
||||
if (delTimeOF > 0.01f) {
|
||||
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
|
||||
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
|
||||
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
|
||||
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
|
||||
delAngBodyOF.zero();
|
||||
delTimeOF = 0.0f;
|
||||
}
|
||||
|
@ -219,9 +219,9 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
|
|||
}
|
||||
// write uncorrected flow rate measurements
|
||||
// note correction for different axis and sign conventions used by the px4flow sensor
|
||||
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
|
||||
ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
|
||||
// write the flow sensor position in body frame
|
||||
ofDataNew.body_offset = posOffset;
|
||||
ofDataNew.body_offset = posOffset.toftype();
|
||||
// write flow rate measurements corrected for body rates
|
||||
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
|
||||
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
|
||||
|
@ -328,7 +328,7 @@ void NavEKF3_core::readMagData()
|
|||
((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
|
||||
|
||||
// detect changes to magnetometer offset parameters and reset states
|
||||
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
|
||||
Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
|
||||
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
|
||||
if (changeDetected) {
|
||||
// zero the learned magnetometer bias states
|
||||
|
@ -357,7 +357,7 @@ void NavEKF3_core::readMagData()
|
|||
magDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// read compass data and scale to improve numerical conditioning
|
||||
magDataNew.mag = compass.get_field(magSelectIndex) * 0.001f;
|
||||
magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype();
|
||||
|
||||
// check for consistent data between magnetometers
|
||||
consistentMagData = compass.consistent();
|
||||
|
@ -386,7 +386,7 @@ void NavEKF3_core::readIMUData()
|
|||
const auto &ins = dal.ins();
|
||||
|
||||
// calculate an averaged IMU update rate using a spike and lowpass filter combination
|
||||
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
|
||||
dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
|
||||
|
||||
// the imu sample time is used as a common time reference throughout the filter
|
||||
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
|
||||
|
@ -427,7 +427,7 @@ void NavEKF3_core::readIMUData()
|
|||
updateMovementCheck();
|
||||
|
||||
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
|
||||
accelPosOffset = ins.get_imu_pos_offset(accel_index_active);
|
||||
accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
|
||||
imuDataNew.accel_index = accel_index_active;
|
||||
|
||||
// Get delta angle data from primary gyro or primary if not available
|
||||
|
@ -454,7 +454,7 @@ void NavEKF3_core::readIMUData()
|
|||
imuQuatDownSampleNew.normalize();
|
||||
|
||||
// Rotate the latest delta velocity into body frame at the start of accumulation
|
||||
Matrix3f deltaRotMat;
|
||||
Matrix3F deltaRotMat;
|
||||
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
|
||||
|
||||
// Apply the delta velocity to the delta velocity accumulator
|
||||
|
@ -482,7 +482,7 @@ void NavEKF3_core::readIMUData()
|
|||
storedIMU.push_youngest_element(imuDataDownSampledNew);
|
||||
|
||||
// calculate the achieved average time step rate for the EKF using a combination spike and LPF
|
||||
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
|
||||
ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
|
||||
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
|
||||
|
||||
// do an addtional down sampling for data used to sample XY body frame drag specific forces
|
||||
|
@ -506,7 +506,7 @@ void NavEKF3_core::readIMUData()
|
|||
imuDataDelayed = storedIMU.get_oldest_element();
|
||||
|
||||
// protect against delta time going to zero
|
||||
float minDT = 0.1f * dtEkfAvg;
|
||||
ftype minDT = 0.1f * dtEkfAvg;
|
||||
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
|
||||
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
|
||||
|
||||
|
@ -526,12 +526,16 @@ void NavEKF3_core::readIMUData()
|
|||
|
||||
// read the delta velocity and corresponding time interval from the IMU
|
||||
// return false if data is not available
|
||||
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
|
||||
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
|
||||
const auto &ins = dal.ins();
|
||||
|
||||
if (ins_index < ins.get_accel_count()) {
|
||||
ins.get_delta_velocity(ins_index,dVel,dVel_dt);
|
||||
dVel_dt = MAX(dVel_dt,1.0e-4f);
|
||||
Vector3f dVelF;
|
||||
float dVel_dtF;
|
||||
ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
|
||||
dVel = dVelF.toftype();
|
||||
dVel_dt = dVel_dtF;
|
||||
dVel_dt = MAX(dVel_dt,1.0e-4);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -585,7 +589,7 @@ void NavEKF3_core::readGpsData()
|
|||
gpsDataNew.sensor_idx = selected_gps;
|
||||
|
||||
// read the NED velocity from the GPS
|
||||
gpsDataNew.vel = gps.velocity(selected_gps);
|
||||
gpsDataNew.vel = gps.velocity(selected_gps).toftype();
|
||||
gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
|
||||
|
||||
// position and velocity are not yet corrected for sensor position
|
||||
|
@ -593,7 +597,7 @@ void NavEKF3_core::readGpsData()
|
|||
|
||||
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
|
||||
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
|
||||
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
|
||||
ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
|
||||
gpsSpdAccuracy *= (1.0f - alpha);
|
||||
float gpsSpdAccRaw;
|
||||
if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
|
||||
|
@ -686,7 +690,7 @@ void NavEKF3_core::readGpsData()
|
|||
if (gpsGoodToAlign && !have_table_earth_field) {
|
||||
const auto *compass = dal.get_compass();
|
||||
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
|
||||
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
|
||||
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
|
||||
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
|
||||
gpsloc.lng*1.0e-7));
|
||||
have_table_earth_field = true;
|
||||
|
@ -699,9 +703,9 @@ void NavEKF3_core::readGpsData()
|
|||
|
||||
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
|
||||
if (validOrigin) {
|
||||
gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc);
|
||||
gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
|
||||
if ((frontend->_originHgtMode & (1<<2)) == 0) {
|
||||
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
|
||||
gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
|
||||
} else {
|
||||
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
|
||||
}
|
||||
|
@ -717,7 +721,7 @@ void NavEKF3_core::readGpsData()
|
|||
// accuracy. Set to min of 5 degrees here to prevent
|
||||
// the user constantly receiving warnings about high
|
||||
// normalised yaw innovations
|
||||
const float min_yaw_accuracy_deg = 5.0f;
|
||||
const ftype min_yaw_accuracy_deg = 5.0f;
|
||||
yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
|
||||
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
|
||||
}
|
||||
|
@ -725,11 +729,15 @@ void NavEKF3_core::readGpsData()
|
|||
|
||||
// read the delta angle and corresponding time interval from the IMU
|
||||
// return false if data is not available
|
||||
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAngDT) {
|
||||
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) {
|
||||
const auto &ins = dal.ins();
|
||||
|
||||
if (ins_index < ins.get_gyro_count()) {
|
||||
ins.get_delta_angle(ins_index, dAng, dAngDT);
|
||||
Vector3f dAngF;
|
||||
float dAngDTF;
|
||||
ins.get_delta_angle(ins_index, dAngF, dAngDTF);
|
||||
dAng = dAngF.toftype();
|
||||
dAngDT = dAngDTF;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -773,7 +781,7 @@ void NavEKF3_core::readBaroData()
|
|||
void NavEKF3_core::calcFiltBaroOffset()
|
||||
{
|
||||
// Apply a first order LPF with spike protection
|
||||
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
|
||||
baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
|
||||
}
|
||||
|
||||
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
|
||||
|
@ -782,14 +790,14 @@ void NavEKF3_core::correctEkfOriginHeight()
|
|||
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
|
||||
|
||||
// calculate the variance of our a-priori estimate of the ekf origin height
|
||||
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
|
||||
ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0, 1.0);
|
||||
if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
|
||||
// Use the baro drift rate
|
||||
const float baroDriftRate = 0.05f;
|
||||
const ftype baroDriftRate = 0.05;
|
||||
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
|
||||
} else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
|
||||
// use the worse case expected terrain gradient and vehicle horizontal speed
|
||||
const float maxTerrGrad = 0.25f;
|
||||
const ftype maxTerrGrad = 0.25;
|
||||
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
|
||||
} else {
|
||||
// by definition our height source is absolute so cannot run this filter
|
||||
|
@ -799,16 +807,16 @@ void NavEKF3_core::correctEkfOriginHeight()
|
|||
|
||||
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
|
||||
// when not using GPS as height source
|
||||
float originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
|
||||
ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
|
||||
|
||||
// calculate the correction gain
|
||||
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
|
||||
ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
|
||||
|
||||
// calculate the innovation
|
||||
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
|
||||
ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
|
||||
|
||||
// check the innovation variance ratio
|
||||
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
|
||||
ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
|
||||
|
||||
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
|
||||
if (ratio < 25.0f && gpsAccuracyGood) {
|
||||
|
@ -910,7 +918,7 @@ void NavEKF3_core::readRngBcnData()
|
|||
rngBcnDataNew.rng = beacon->beacon_distance(index);
|
||||
|
||||
// set the beacon position
|
||||
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index);
|
||||
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
|
||||
|
||||
// identify the beacon identifier
|
||||
rngBcnDataNew.beacon_ID = index;
|
||||
|
@ -927,15 +935,19 @@ void NavEKF3_core::readRngBcnData()
|
|||
}
|
||||
|
||||
// Check if the beacon system has returned a 3D fix
|
||||
if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) {
|
||||
Vector3f bp;
|
||||
float bperr;
|
||||
if (beacon->get_vehicle_position_ned(bp, bperr)) {
|
||||
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
beaconVehiclePosNED = bp.toftype();
|
||||
beaconVehiclePosErr = bperr;
|
||||
|
||||
// Check if the range beacon data can be used to align the vehicle position
|
||||
if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) {
|
||||
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
|
||||
const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
|
||||
const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
|
||||
const ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
|
||||
const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
|
||||
if (posDiffSq < 9.0f * posDiffVar) {
|
||||
rngBcnGoodToAlign = true;
|
||||
// Set the EKF origin and magnetic field declination if not previously set
|
||||
|
@ -1035,7 +1047,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
|
|||
extNavDataNew.posReset = false;
|
||||
}
|
||||
|
||||
extNavDataNew.pos = pos;
|
||||
extNavDataNew.pos = pos.toftype();
|
||||
extNavDataNew.posErr = posErr;
|
||||
|
||||
// calculate timestamp
|
||||
|
@ -1052,7 +1064,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
|
|||
// protect against attitude or angle being NaN
|
||||
if (!quat.is_nan() && !isnan(angErr)) {
|
||||
// extract yaw from the attitude
|
||||
float roll_rad, pitch_rad, yaw_rad;
|
||||
ftype roll_rad, pitch_rad, yaw_rad;
|
||||
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
|
||||
yaw_elements extNavYawAngDataNew;
|
||||
extNavYawAngDataNew.yawAng = yaw_rad;
|
||||
|
@ -1087,7 +1099,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
|
|||
|
||||
ext_nav_vel_elements extNavVelNew;
|
||||
extNavVelNew.time_ms = timeStamp_ms;
|
||||
extNavVelNew.vel = vel;
|
||||
extNavVelNew.vel = vel.toftype();
|
||||
extNavVelNew.err = err;
|
||||
extNavVelNew.corrected = false;
|
||||
|
||||
|
@ -1246,15 +1258,15 @@ void NavEKF3_core::learnInactiveBiases(void)
|
|||
// get filtered gyro and use the difference between the
|
||||
// corrected gyro on the active IMU and the inactive IMU
|
||||
// to move the inactive bias towards the right value
|
||||
Vector3f filtered_gyro_active = ins.get_gyro(gyro_index_active) - (stateStruct.gyro_bias/dtEkfAvg);
|
||||
Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].gyro_bias/dtEkfAvg);
|
||||
Vector3f error = filtered_gyro_active - filtered_gyro_inactive;
|
||||
Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
|
||||
Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg);
|
||||
Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
|
||||
|
||||
// prevent a single large error from contaminating bias estimate
|
||||
const float bias_limit = radians(5);
|
||||
error.x = constrain_float(error.x, -bias_limit, bias_limit);
|
||||
error.y = constrain_float(error.y, -bias_limit, bias_limit);
|
||||
error.z = constrain_float(error.z, -bias_limit, bias_limit);
|
||||
const ftype bias_limit = radians(5);
|
||||
error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
|
||||
error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
|
||||
error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
|
||||
|
||||
// slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
|
||||
// gyro bias error in around 1 minute
|
||||
|
@ -1275,15 +1287,15 @@ void NavEKF3_core::learnInactiveBiases(void)
|
|||
// get filtered accel and use the difference between the
|
||||
// corrected accel on the active IMU and the inactive IMU
|
||||
// to move the inactive bias towards the right value
|
||||
Vector3f filtered_accel_active = ins.get_accel(accel_index_active) - (stateStruct.accel_bias/dtEkfAvg);
|
||||
Vector3f filtered_accel_inactive = ins.get_accel(i) - (inactiveBias[i].accel_bias/dtEkfAvg);
|
||||
Vector3f error = filtered_accel_active - filtered_accel_inactive;
|
||||
Vector3F filtered_accel_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg);
|
||||
Vector3F filtered_accel_inactive = ins.get_accel(i).toftype() - (inactiveBias[i].accel_bias/dtEkfAvg);
|
||||
Vector3F error = filtered_accel_active - filtered_accel_inactive;
|
||||
|
||||
// prevent a single large error from contaminating bias estimate
|
||||
const float bias_limit = 1.0; // m/s/s
|
||||
error.x = constrain_float(error.x, -bias_limit, bias_limit);
|
||||
error.y = constrain_float(error.y, -bias_limit, bias_limit);
|
||||
error.z = constrain_float(error.z, -bias_limit, bias_limit);
|
||||
const ftype bias_limit = 1.0; // m/s/s
|
||||
error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
|
||||
error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
|
||||
error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
|
||||
|
||||
// slowly bring the inactive accel in line with the active
|
||||
// accel. This corrects a 0.5 m/s/s accel bias error in
|
||||
|
@ -1297,7 +1309,7 @@ void NavEKF3_core::learnInactiveBiases(void)
|
|||
/*
|
||||
return declination in radians
|
||||
*/
|
||||
float NavEKF3_core::MagDeclination(void) const
|
||||
ftype NavEKF3_core::MagDeclination(void) const
|
||||
{
|
||||
// if we are using the WMM tables then use the table declination
|
||||
// to ensure consistency with the table mag field. Otherwise use
|
||||
|
@ -1327,17 +1339,17 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
return;
|
||||
}
|
||||
|
||||
const float gyro_limit = radians(3.0f);
|
||||
const float gyro_diff_limit = 0.2f;
|
||||
const float accel_limit = 1.0f;
|
||||
const float accel_diff_limit = 5.0f;
|
||||
const float hysteresis_ratio = 0.7f;
|
||||
const float dtEkfAvgInv = 1.0f / dtEkfAvg;
|
||||
const ftype gyro_limit = radians(3.0f);
|
||||
const ftype gyro_diff_limit = 0.2f;
|
||||
const ftype accel_limit = 1.0f;
|
||||
const ftype accel_diff_limit = 5.0f;
|
||||
const ftype hysteresis_ratio = 0.7f;
|
||||
const ftype dtEkfAvgInv = 1.0f / dtEkfAvg;
|
||||
|
||||
// get latest bias corrected gyro and accelerometer data
|
||||
const auto &ins = dal.ins();
|
||||
Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv;
|
||||
Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv;
|
||||
Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv;
|
||||
Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv;
|
||||
|
||||
if (!prevOnGround) {
|
||||
gyro_prev = gyro;
|
||||
|
@ -1349,7 +1361,7 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
}
|
||||
|
||||
// calculate a gyro rate of change metric
|
||||
Vector3f temp = (gyro - gyro_prev) * dtEkfAvgInv;
|
||||
Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv;
|
||||
gyro_prev = gyro;
|
||||
gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();
|
||||
|
||||
|
@ -1358,14 +1370,14 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
accel_prev = accel;
|
||||
accel_diff = 0.99f * accel_diff + 0.01f * temp.length();
|
||||
|
||||
const float gyro_length_ratio = gyro.length() / gyro_limit;
|
||||
const float accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
|
||||
const float gyro_diff_ratio = gyro_diff / gyro_diff_limit;
|
||||
const float accel_diff_ratio = accel_diff / accel_diff_limit;
|
||||
const ftype gyro_length_ratio = gyro.length() / gyro_limit;
|
||||
const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
|
||||
const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit;
|
||||
const ftype accel_diff_ratio = accel_diff / accel_diff_limit;
|
||||
bool logStatusChange = false;
|
||||
if (onGroundNotMoving) {
|
||||
if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
|
||||
fabsf(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
|
||||
fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
|
||||
gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
|
||||
accel_diff_ratio > frontend->_ognmTestScaleFactor)
|
||||
{
|
||||
|
@ -1373,7 +1385,7 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
logStatusChange = true;
|
||||
}
|
||||
} else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
|
||||
fabsf(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
|
||||
fabsF(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
|
||||
gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
|
||||
accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio)
|
||||
{
|
||||
|
@ -1388,10 +1400,10 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
time_us : dal.micros64(),
|
||||
core : core_index,
|
||||
ongroundnotmoving : onGroundNotMoving,
|
||||
gyro_length_ratio : gyro_length_ratio,
|
||||
accel_length_ratio : accel_length_ratio,
|
||||
gyro_diff_ratio : gyro_diff_ratio,
|
||||
accel_diff_ratio : accel_diff_ratio,
|
||||
gyro_length_ratio : float(gyro_length_ratio),
|
||||
accel_length_ratio : float(accel_length_ratio),
|
||||
gyro_diff_ratio : float(gyro_diff_ratio),
|
||||
accel_diff_ratio : float(accel_diff_ratio),
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
@ -1401,9 +1413,9 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
|
|||
{
|
||||
#if EK3_FEATURE_DRAG_FUSION
|
||||
// Average and down sample to 5Hz
|
||||
const float bcoef_x = frontend->_ballisticCoef_x;
|
||||
const float bcoef_y = frontend->_ballisticCoef_y;
|
||||
const float mcoef = frontend->_momentumDragCoef.get();
|
||||
const ftype bcoef_x = frontend->_ballisticCoef_x;
|
||||
const ftype bcoef_y = frontend->_ballisticCoef_y;
|
||||
const ftype mcoef = frontend->_momentumDragCoef.get();
|
||||
const bool using_bcoef_x = bcoef_x > 1.0f;
|
||||
const bool using_bcoef_y = bcoef_y > 1.0f;
|
||||
const bool using_mcoef = mcoef > 0.001f;
|
||||
|
|
|
@ -71,7 +71,7 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri
|
|||
void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
||||
{
|
||||
// horizontal velocity squared
|
||||
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
|
||||
ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
|
||||
|
||||
// don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
|
||||
// don't fuse flow data if it exceeds validity limits
|
||||
|
@ -92,34 +92,34 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
|
||||
ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
|
||||
distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
|
||||
prevPosN = stateStruct.position[0];
|
||||
prevPosE = stateStruct.position[1];
|
||||
|
||||
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
|
||||
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||||
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
|
||||
ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||||
ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
|
||||
Popt += Pincrement;
|
||||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||||
|
||||
// fuse range finder data
|
||||
if (rangeDataToFuse) {
|
||||
// predict range
|
||||
float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
|
||||
ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
|
||||
|
||||
// Copy required states to local variable names
|
||||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
|
||||
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
float R_RNG = frontend->_rngNoise;
|
||||
ftype R_RNG = frontend->_rngNoise;
|
||||
|
||||
// calculate Kalman gain
|
||||
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
|
||||
ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
|
||||
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
|
||||
|
@ -131,7 +131,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
innovRng = predRngMeas - rangeDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
|
||||
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
|
||||
|
||||
// Check the innovation test ratio and don't fuse if too large
|
||||
if (auxRngTestRatio < 1.0f) {
|
||||
|
@ -152,18 +152,18 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
|
||||
if (!cantFuseFlowData) {
|
||||
|
||||
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||||
Vector2f losPred; // predicted optical flow angular rate measurement
|
||||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
float K_OPT;
|
||||
float H_OPT;
|
||||
Vector2f auxFlowObsInnovVar;
|
||||
Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||||
Vector2F losPred; // predicted optical flow angular rate measurement
|
||||
ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
ftype K_OPT;
|
||||
ftype H_OPT;
|
||||
Vector2F auxFlowObsInnovVar;
|
||||
|
||||
// predict range to centre of image
|
||||
float flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
|
||||
ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
|
||||
|
@ -180,20 +180,20 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
|
||||
|
||||
// calculate observation jacobians
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = stateStruct.position.z - terrainState;
|
||||
float t7 = 1.0f / (t6*t6);
|
||||
float t8 = q0*q3*2.0f;
|
||||
float t9 = t2-t3-t4+t5;
|
||||
ftype t2 = q0*q0;
|
||||
ftype t3 = q1*q1;
|
||||
ftype t4 = q2*q2;
|
||||
ftype t5 = q3*q3;
|
||||
ftype t6 = stateStruct.position.z - terrainState;
|
||||
ftype t7 = 1.0f / (t6*t6);
|
||||
ftype t8 = q0*q3*2.0f;
|
||||
ftype t9 = t2-t3-t4+t5;
|
||||
|
||||
// prevent the state variances from becoming badly conditioned
|
||||
Popt = MAX(Popt,1E-6f);
|
||||
|
||||
// calculate observation noise variance from parameter
|
||||
float flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
|
||||
ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
|
||||
|
||||
// Fuse Y axis data
|
||||
|
||||
|
@ -207,7 +207,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
|
||||
auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (auxFlowTestRatio.y < 1.0f) {
|
||||
|
@ -239,7 +239,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
|
||||
auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (auxFlowTestRatio.x < 1.0f) {
|
||||
|
@ -269,23 +269,23 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
|
|||
void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
||||
{
|
||||
Vector24 H_LOS;
|
||||
Vector3f relVelSensor;
|
||||
Vector3F relVelSensor;
|
||||
Vector14 SH_LOS;
|
||||
Vector2 losPred;
|
||||
|
||||
// Copy required states to local variable names
|
||||
float q0 = stateStruct.quat[0];
|
||||
float q1 = stateStruct.quat[1];
|
||||
float q2 = stateStruct.quat[2];
|
||||
float q3 = stateStruct.quat[3];
|
||||
float vn = stateStruct.velocity.x;
|
||||
float ve = stateStruct.velocity.y;
|
||||
float vd = stateStruct.velocity.z;
|
||||
float pd = stateStruct.position.z;
|
||||
ftype q0 = stateStruct.quat[0];
|
||||
ftype q1 = stateStruct.quat[1];
|
||||
ftype q2 = stateStruct.quat[2];
|
||||
ftype q3 = stateStruct.quat[3];
|
||||
ftype vn = stateStruct.velocity.x;
|
||||
ftype ve = stateStruct.velocity.y;
|
||||
ftype vd = stateStruct.velocity.z;
|
||||
ftype pd = stateStruct.position.z;
|
||||
|
||||
// constrain height above ground to be above range measured on ground
|
||||
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
|
||||
float ptd = pd + heightAboveGndEst;
|
||||
ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
|
||||
ftype ptd = pd + heightAboveGndEst;
|
||||
|
||||
// Calculate common expressions for observation jacobians
|
||||
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
|
@ -306,14 +306,14 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
|
||||
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
|
||||
ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
|
||||
|
||||
// correct range for flow sensor offset body frame position offset
|
||||
// the corrected value is the predicted range from the sensor focal point to the
|
||||
// centre of the image on the ground assuming flat terrain
|
||||
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
|
||||
Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
range -= posOffsetEarth.z / prevTnb.c.z;
|
||||
}
|
||||
|
||||
|
@ -328,7 +328,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
memset(&H_LOS[0], 0, sizeof(H_LOS));
|
||||
if (obsIndex == 0) {
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
ftype t2 = 1.0f / range;
|
||||
H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
|
@ -338,98 +338,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observation innovation variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P[1][1]*t2*t15;
|
||||
float t32 = P[6][0]*t2*t10;
|
||||
float t33 = P[1][0]*t2*t15;
|
||||
float t34 = P[2][0]*t2*t19;
|
||||
float t35 = P[5][0]*t2*t27;
|
||||
float t79 = P[4][0]*t2*t7;
|
||||
float t80 = P[3][0]*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[6][1]*t2*t10;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[2][1]*t2*t19;
|
||||
float t41 = P[5][1]*t2*t27;
|
||||
float t81 = P[4][1]*t2*t7;
|
||||
float t82 = P[3][1]*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P[6][2]*t2*t10;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t15;
|
||||
float t47 = P[2][2]*t2*t19;
|
||||
float t48 = P[5][2]*t2*t27;
|
||||
float t83 = P[4][2]*t2*t7;
|
||||
float t84 = P[3][2]*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P[6][3]*t2*t10;
|
||||
float t52 = P[0][3]*t2*t5;
|
||||
float t53 = P[1][3]*t2*t15;
|
||||
float t54 = P[2][3]*t2*t19;
|
||||
float t55 = P[5][3]*t2*t27;
|
||||
float t85 = P[4][3]*t2*t7;
|
||||
float t86 = P[3][3]*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P[6][5]*t2*t10;
|
||||
float t58 = P[0][5]*t2*t5;
|
||||
float t59 = P[1][5]*t2*t15;
|
||||
float t60 = P[2][5]*t2*t19;
|
||||
float t61 = P[5][5]*t2*t27;
|
||||
float t88 = P[4][5]*t2*t7;
|
||||
float t89 = P[3][5]*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[6][4]*t2*t10;
|
||||
float t65 = P[0][4]*t2*t5;
|
||||
float t66 = P[1][4]*t2*t15;
|
||||
float t67 = P[2][4]*t2*t19;
|
||||
float t68 = P[5][4]*t2*t27;
|
||||
float t90 = P[4][4]*t2*t7;
|
||||
float t91 = P[3][4]*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P[6][6]*t2*t10;
|
||||
float t71 = P[0][6]*t2*t5;
|
||||
float t72 = P[1][6]*t2*t15;
|
||||
float t73 = P[2][6]*t2*t19;
|
||||
float t74 = P[5][6]*t2*t27;
|
||||
float t93 = P[4][6]*t2*t7;
|
||||
float t94 = P[3][6]*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78;
|
||||
ftype t3 = q1*vd*2.0f;
|
||||
ftype t4 = q0*ve*2.0f;
|
||||
ftype t11 = q3*vn*2.0f;
|
||||
ftype t5 = t3+t4-t11;
|
||||
ftype t6 = q0*q3*2.0f;
|
||||
ftype t29 = q1*q2*2.0f;
|
||||
ftype t7 = t6-t29;
|
||||
ftype t8 = q0*q1*2.0f;
|
||||
ftype t9 = q2*q3*2.0f;
|
||||
ftype t10 = t8+t9;
|
||||
ftype t12 = P[0][0]*t2*t5;
|
||||
ftype t13 = q0*vd*2.0f;
|
||||
ftype t14 = q2*vn*2.0f;
|
||||
ftype t28 = q1*ve*2.0f;
|
||||
ftype t15 = t13+t14-t28;
|
||||
ftype t16 = q3*vd*2.0f;
|
||||
ftype t17 = q2*ve*2.0f;
|
||||
ftype t18 = q1*vn*2.0f;
|
||||
ftype t19 = t16+t17+t18;
|
||||
ftype t20 = q3*ve*2.0f;
|
||||
ftype t21 = q0*vn*2.0f;
|
||||
ftype t30 = q2*vd*2.0f;
|
||||
ftype t22 = t20+t21-t30;
|
||||
ftype t23 = q0*q0;
|
||||
ftype t24 = q1*q1;
|
||||
ftype t25 = q2*q2;
|
||||
ftype t26 = q3*q3;
|
||||
ftype t27 = t23-t24+t25-t26;
|
||||
ftype t31 = P[1][1]*t2*t15;
|
||||
ftype t32 = P[6][0]*t2*t10;
|
||||
ftype t33 = P[1][0]*t2*t15;
|
||||
ftype t34 = P[2][0]*t2*t19;
|
||||
ftype t35 = P[5][0]*t2*t27;
|
||||
ftype t79 = P[4][0]*t2*t7;
|
||||
ftype t80 = P[3][0]*t2*t22;
|
||||
ftype t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
ftype t37 = t2*t5*t36;
|
||||
ftype t38 = P[6][1]*t2*t10;
|
||||
ftype t39 = P[0][1]*t2*t5;
|
||||
ftype t40 = P[2][1]*t2*t19;
|
||||
ftype t41 = P[5][1]*t2*t27;
|
||||
ftype t81 = P[4][1]*t2*t7;
|
||||
ftype t82 = P[3][1]*t2*t22;
|
||||
ftype t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
ftype t43 = t2*t15*t42;
|
||||
ftype t44 = P[6][2]*t2*t10;
|
||||
ftype t45 = P[0][2]*t2*t5;
|
||||
ftype t46 = P[1][2]*t2*t15;
|
||||
ftype t47 = P[2][2]*t2*t19;
|
||||
ftype t48 = P[5][2]*t2*t27;
|
||||
ftype t83 = P[4][2]*t2*t7;
|
||||
ftype t84 = P[3][2]*t2*t22;
|
||||
ftype t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
ftype t50 = t2*t19*t49;
|
||||
ftype t51 = P[6][3]*t2*t10;
|
||||
ftype t52 = P[0][3]*t2*t5;
|
||||
ftype t53 = P[1][3]*t2*t15;
|
||||
ftype t54 = P[2][3]*t2*t19;
|
||||
ftype t55 = P[5][3]*t2*t27;
|
||||
ftype t85 = P[4][3]*t2*t7;
|
||||
ftype t86 = P[3][3]*t2*t22;
|
||||
ftype t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
ftype t57 = P[6][5]*t2*t10;
|
||||
ftype t58 = P[0][5]*t2*t5;
|
||||
ftype t59 = P[1][5]*t2*t15;
|
||||
ftype t60 = P[2][5]*t2*t19;
|
||||
ftype t61 = P[5][5]*t2*t27;
|
||||
ftype t88 = P[4][5]*t2*t7;
|
||||
ftype t89 = P[3][5]*t2*t22;
|
||||
ftype t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
ftype t63 = t2*t27*t62;
|
||||
ftype t64 = P[6][4]*t2*t10;
|
||||
ftype t65 = P[0][4]*t2*t5;
|
||||
ftype t66 = P[1][4]*t2*t15;
|
||||
ftype t67 = P[2][4]*t2*t19;
|
||||
ftype t68 = P[5][4]*t2*t27;
|
||||
ftype t90 = P[4][4]*t2*t7;
|
||||
ftype t91 = P[3][4]*t2*t22;
|
||||
ftype t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
ftype t70 = P[6][6]*t2*t10;
|
||||
ftype t71 = P[0][6]*t2*t5;
|
||||
ftype t72 = P[1][6]*t2*t15;
|
||||
ftype t73 = P[2][6]*t2*t19;
|
||||
ftype t74 = P[5][6]*t2*t27;
|
||||
ftype t93 = P[4][6]*t2*t7;
|
||||
ftype t94 = P[3][6]*t2*t22;
|
||||
ftype t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
ftype t76 = t2*t10*t75;
|
||||
ftype t87 = t2*t22*t56;
|
||||
ftype t92 = t2*t7*t69;
|
||||
ftype t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
ftype t78;
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
if (t77 > R_LOS) {
|
||||
|
@ -463,8 +463,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -477,8 +477,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
|
@ -489,22 +489,22 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
ftype t2 = 1.0f / range;
|
||||
H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
|
@ -514,98 +514,98 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y observation innovation variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P[1][1]*t2*t16;
|
||||
float t32 = P[5][0]*t2*t8;
|
||||
float t33 = P[1][0]*t2*t16;
|
||||
float t34 = P[3][0]*t2*t22;
|
||||
float t35 = P[4][0]*t2*t27;
|
||||
float t80 = P[6][0]*t2*t10;
|
||||
float t81 = P[2][0]*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[5][1]*t2*t8;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[3][1]*t2*t22;
|
||||
float t41 = P[4][1]*t2*t27;
|
||||
float t82 = P[6][1]*t2*t10;
|
||||
float t83 = P[2][1]*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P[5][2]*t2*t8;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t16;
|
||||
float t47 = P[3][2]*t2*t22;
|
||||
float t48 = P[4][2]*t2*t27;
|
||||
float t79 = P[2][2]*t2*t19;
|
||||
float t84 = P[6][2]*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P[5][3]*t2*t8;
|
||||
float t51 = P[0][3]*t2*t5;
|
||||
float t52 = P[1][3]*t2*t16;
|
||||
float t53 = P[3][3]*t2*t22;
|
||||
float t54 = P[4][3]*t2*t27;
|
||||
float t86 = P[6][3]*t2*t10;
|
||||
float t87 = P[2][3]*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P[5][4]*t2*t8;
|
||||
float t58 = P[0][4]*t2*t5;
|
||||
float t59 = P[1][4]*t2*t16;
|
||||
float t60 = P[3][4]*t2*t22;
|
||||
float t61 = P[4][4]*t2*t27;
|
||||
float t88 = P[6][4]*t2*t10;
|
||||
float t89 = P[2][4]*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[5][5]*t2*t8;
|
||||
float t65 = P[0][5]*t2*t5;
|
||||
float t66 = P[1][5]*t2*t16;
|
||||
float t67 = P[3][5]*t2*t22;
|
||||
float t68 = P[4][5]*t2*t27;
|
||||
float t90 = P[6][5]*t2*t10;
|
||||
float t91 = P[2][5]*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P[5][6]*t2*t8;
|
||||
float t72 = P[0][6]*t2*t5;
|
||||
float t73 = P[1][6]*t2*t16;
|
||||
float t74 = P[3][6]*t2*t22;
|
||||
float t75 = P[4][6]*t2*t27;
|
||||
float t92 = P[6][6]*t2*t10;
|
||||
float t93 = P[2][6]*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
float t78;
|
||||
ftype t3 = q3*ve*2.0f;
|
||||
ftype t4 = q0*vn*2.0f;
|
||||
ftype t11 = q2*vd*2.0f;
|
||||
ftype t5 = t3+t4-t11;
|
||||
ftype t6 = q0*q3*2.0f;
|
||||
ftype t7 = q1*q2*2.0f;
|
||||
ftype t8 = t6+t7;
|
||||
ftype t9 = q0*q2*2.0f;
|
||||
ftype t28 = q1*q3*2.0f;
|
||||
ftype t10 = t9-t28;
|
||||
ftype t12 = P[0][0]*t2*t5;
|
||||
ftype t13 = q3*vd*2.0f;
|
||||
ftype t14 = q2*ve*2.0f;
|
||||
ftype t15 = q1*vn*2.0f;
|
||||
ftype t16 = t13+t14+t15;
|
||||
ftype t17 = q0*vd*2.0f;
|
||||
ftype t18 = q2*vn*2.0f;
|
||||
ftype t29 = q1*ve*2.0f;
|
||||
ftype t19 = t17+t18-t29;
|
||||
ftype t20 = q1*vd*2.0f;
|
||||
ftype t21 = q0*ve*2.0f;
|
||||
ftype t30 = q3*vn*2.0f;
|
||||
ftype t22 = t20+t21-t30;
|
||||
ftype t23 = q0*q0;
|
||||
ftype t24 = q1*q1;
|
||||
ftype t25 = q2*q2;
|
||||
ftype t26 = q3*q3;
|
||||
ftype t27 = t23+t24-t25-t26;
|
||||
ftype t31 = P[1][1]*t2*t16;
|
||||
ftype t32 = P[5][0]*t2*t8;
|
||||
ftype t33 = P[1][0]*t2*t16;
|
||||
ftype t34 = P[3][0]*t2*t22;
|
||||
ftype t35 = P[4][0]*t2*t27;
|
||||
ftype t80 = P[6][0]*t2*t10;
|
||||
ftype t81 = P[2][0]*t2*t19;
|
||||
ftype t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
ftype t37 = t2*t5*t36;
|
||||
ftype t38 = P[5][1]*t2*t8;
|
||||
ftype t39 = P[0][1]*t2*t5;
|
||||
ftype t40 = P[3][1]*t2*t22;
|
||||
ftype t41 = P[4][1]*t2*t27;
|
||||
ftype t82 = P[6][1]*t2*t10;
|
||||
ftype t83 = P[2][1]*t2*t19;
|
||||
ftype t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
ftype t43 = t2*t16*t42;
|
||||
ftype t44 = P[5][2]*t2*t8;
|
||||
ftype t45 = P[0][2]*t2*t5;
|
||||
ftype t46 = P[1][2]*t2*t16;
|
||||
ftype t47 = P[3][2]*t2*t22;
|
||||
ftype t48 = P[4][2]*t2*t27;
|
||||
ftype t79 = P[2][2]*t2*t19;
|
||||
ftype t84 = P[6][2]*t2*t10;
|
||||
ftype t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
ftype t50 = P[5][3]*t2*t8;
|
||||
ftype t51 = P[0][3]*t2*t5;
|
||||
ftype t52 = P[1][3]*t2*t16;
|
||||
ftype t53 = P[3][3]*t2*t22;
|
||||
ftype t54 = P[4][3]*t2*t27;
|
||||
ftype t86 = P[6][3]*t2*t10;
|
||||
ftype t87 = P[2][3]*t2*t19;
|
||||
ftype t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
ftype t56 = t2*t22*t55;
|
||||
ftype t57 = P[5][4]*t2*t8;
|
||||
ftype t58 = P[0][4]*t2*t5;
|
||||
ftype t59 = P[1][4]*t2*t16;
|
||||
ftype t60 = P[3][4]*t2*t22;
|
||||
ftype t61 = P[4][4]*t2*t27;
|
||||
ftype t88 = P[6][4]*t2*t10;
|
||||
ftype t89 = P[2][4]*t2*t19;
|
||||
ftype t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
ftype t63 = t2*t27*t62;
|
||||
ftype t64 = P[5][5]*t2*t8;
|
||||
ftype t65 = P[0][5]*t2*t5;
|
||||
ftype t66 = P[1][5]*t2*t16;
|
||||
ftype t67 = P[3][5]*t2*t22;
|
||||
ftype t68 = P[4][5]*t2*t27;
|
||||
ftype t90 = P[6][5]*t2*t10;
|
||||
ftype t91 = P[2][5]*t2*t19;
|
||||
ftype t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
ftype t70 = t2*t8*t69;
|
||||
ftype t71 = P[5][6]*t2*t8;
|
||||
ftype t72 = P[0][6]*t2*t5;
|
||||
ftype t73 = P[1][6]*t2*t16;
|
||||
ftype t74 = P[3][6]*t2*t22;
|
||||
ftype t75 = P[4][6]*t2*t27;
|
||||
ftype t92 = P[6][6]*t2*t10;
|
||||
ftype t93 = P[2][6]*t2*t19;
|
||||
ftype t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
ftype t85 = t2*t19*t49;
|
||||
ftype t94 = t2*t10*t76;
|
||||
ftype t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
ftype t78;
|
||||
|
||||
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
||||
if (t77 > R_LOS) {
|
||||
|
@ -639,8 +639,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -653,8 +653,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
|
@ -665,21 +665,21 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed)
|
|||
Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
|
||||
|
|
|
@ -23,7 +23,7 @@ bool NavEKF3_core::healthy(void) const
|
|||
}
|
||||
// position and height innovations must be within limits when on-ground and in a static mode of operation
|
||||
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
|
||||
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
|
||||
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -98,7 +98,7 @@ void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
|
|||
gyroBias.zero();
|
||||
return;
|
||||
}
|
||||
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
|
||||
gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
|
||||
}
|
||||
|
||||
// return accelerometer bias in m/s/s
|
||||
|
@ -108,7 +108,7 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
|
|||
accelBias.zero();
|
||||
return;
|
||||
}
|
||||
accelBias = stateStruct.accel_bias / dtEkfAvg;
|
||||
accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
|
||||
}
|
||||
|
||||
// return the transformation matrix from XYZ (body) to NED axes
|
||||
|
@ -121,7 +121,7 @@ void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
|
|||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
||||
void NavEKF3_core::getQuaternion(Quaternion& ret) const
|
||||
{
|
||||
ret = outputDataNew.quat;
|
||||
ret = outputDataNew.quat.tofloat();
|
||||
}
|
||||
|
||||
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
||||
|
@ -136,7 +136,7 @@ uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const
|
|||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
|
||||
{
|
||||
pos = posResetNE;
|
||||
pos = posResetNE.tofloat();
|
||||
return lastPosReset_ms;
|
||||
}
|
||||
|
||||
|
@ -152,7 +152,7 @@ uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const
|
|||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
|
||||
{
|
||||
vel = velResetNE;
|
||||
vel = velResetNE.tofloat();
|
||||
return lastVelReset_ms;
|
||||
}
|
||||
|
||||
|
@ -171,7 +171,7 @@ bool NavEKF3_core::getWind(Vector3f &wind) const
|
|||
void NavEKF3_core::getVelNED(Vector3f &vel) const
|
||||
{
|
||||
// correct for the IMU position offset (EKF calculations are at the IMU)
|
||||
vel = outputDataNew.velocity + velOffsetNED;
|
||||
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
|
||||
}
|
||||
|
||||
// return estimate of true airspeed vector in body frame in m/s
|
||||
|
@ -181,7 +181,7 @@ bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
|
|||
if (inhibitWindStates || PV_AidingMode == AID_NONE) {
|
||||
return false;
|
||||
}
|
||||
vel = outputDataNew.velocity + velOffsetNED;
|
||||
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
|
||||
vel.x -= stateStruct.wind_vel.x;
|
||||
vel.y -= stateStruct.wind_vel.y;
|
||||
Matrix3f Tnb; // rotation from nav to body frame
|
||||
|
@ -218,7 +218,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
|
|||
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
|
||||
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
|
||||
const struct Location &gpsloc = gps.location(selected_gps);
|
||||
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
|
||||
const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
|
||||
posNE.x = tempPosNE.x;
|
||||
posNE.y = tempPosNE.y;
|
||||
return false;
|
||||
|
@ -369,13 +369,13 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
|
|||
// return earth magnetic field estimates in measurement units / 1000
|
||||
void NavEKF3_core::getMagNED(Vector3f &magNED) const
|
||||
{
|
||||
magNED = stateStruct.earth_magfield * 1000.0f;
|
||||
magNED = (stateStruct.earth_magfield * 1000.0f).tofloat();
|
||||
}
|
||||
|
||||
// return body magnetic field estimates in measurement units / 1000
|
||||
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
|
||||
{
|
||||
magXYZ = stateStruct.body_magfield*1000.0f;
|
||||
magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
|
||||
}
|
||||
|
||||
// return magnetometer offsets
|
||||
|
@ -394,7 +394,7 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
|||
!inhibitMagStates &&
|
||||
dal.get_compass()->healthy(magSelectIndex) &&
|
||||
variancesConverged) {
|
||||
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
|
||||
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
|
||||
return true;
|
||||
} else {
|
||||
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
|
||||
|
@ -440,15 +440,15 @@ void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaIn
|
|||
// also return the delta in position due to the last position reset
|
||||
void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
|
||||
{
|
||||
velVar = sqrtf(velTestRatio);
|
||||
posVar = sqrtf(posTestRatio);
|
||||
hgtVar = sqrtf(hgtTestRatio);
|
||||
velVar = sqrtF(velTestRatio);
|
||||
posVar = sqrtF(posTestRatio);
|
||||
hgtVar = sqrtF(hgtTestRatio);
|
||||
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
|
||||
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
|
||||
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
|
||||
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
|
||||
tasVar = sqrtf(tasTestRatio);
|
||||
offset = posResetNE;
|
||||
magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
|
||||
magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
|
||||
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
|
||||
tasVar = sqrtF(tasTestRatio);
|
||||
offset = posResetNE.tofloat();
|
||||
}
|
||||
|
||||
// get a particular source's velocity innovations
|
||||
|
@ -461,8 +461,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
|
|||
if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
|
||||
return false;
|
||||
}
|
||||
innovations = gpsVelInnov;
|
||||
variances = gpsVelVarInnov;
|
||||
innovations = gpsVelInnov.tofloat();
|
||||
variances = gpsVelVarInnov.tofloat();
|
||||
return true;
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
case AP_NavEKF_Source::SourceXY::EXTNAV:
|
||||
|
@ -470,8 +470,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
|
|||
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
|
||||
return false;
|
||||
}
|
||||
innovations = extNavVelInnov;
|
||||
variances = extNavVelVarInnov;
|
||||
innovations = extNavVelInnov.tofloat();
|
||||
variances = extNavVelVarInnov.tofloat();
|
||||
return true;
|
||||
#endif // EK3_FEATURE_EXTERNAL_NAV
|
||||
default:
|
||||
|
@ -565,11 +565,11 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
|
|||
// range finder is fitted for other applications
|
||||
float temp;
|
||||
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
|
||||
temp = sqrtf(auxRngTestRatio);
|
||||
temp = sqrtF(auxRngTestRatio);
|
||||
} else {
|
||||
temp = 0.0f;
|
||||
}
|
||||
const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
|
||||
const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
|
||||
|
||||
// send message
|
||||
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar);
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -42,14 +42,14 @@ void NavEKF3_core::SelectRngBcnFusion()
|
|||
void NavEKF3_core::FuseRngBcn()
|
||||
{
|
||||
// declarations
|
||||
float pn;
|
||||
float pe;
|
||||
float pd;
|
||||
float bcn_pn;
|
||||
float bcn_pe;
|
||||
float bcn_pd;
|
||||
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
float rngPred;
|
||||
ftype pn;
|
||||
ftype pe;
|
||||
ftype pd;
|
||||
ftype bcn_pn;
|
||||
ftype bcn_pe;
|
||||
ftype bcn_pd;
|
||||
const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
ftype rngPred;
|
||||
|
||||
// health is set bad until test passed
|
||||
rngBcnHealth = false;
|
||||
|
@ -70,7 +70,7 @@ void NavEKF3_core::FuseRngBcn()
|
|||
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
|
||||
|
||||
// predicted range
|
||||
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
rngPred = deltaPosNED.length();
|
||||
|
||||
// calculate measurement innovation
|
||||
|
@ -80,16 +80,16 @@ void NavEKF3_core::FuseRngBcn()
|
|||
if (rngPred > 0.1f)
|
||||
{
|
||||
// calculate observation jacobians
|
||||
float H_BCN[24];
|
||||
ftype H_BCN[24];
|
||||
memset(H_BCN, 0, sizeof(H_BCN));
|
||||
float t2 = bcn_pd-pd;
|
||||
float t3 = bcn_pe-pe;
|
||||
float t4 = bcn_pn-pn;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
float t9 = 1.0f/sqrtf(t8);
|
||||
ftype t2 = bcn_pd-pd;
|
||||
ftype t3 = bcn_pe-pe;
|
||||
ftype t4 = bcn_pn-pn;
|
||||
ftype t5 = t2*t2;
|
||||
ftype t6 = t3*t3;
|
||||
ftype t7 = t4*t4;
|
||||
ftype t8 = t5+t6+t7;
|
||||
ftype t9 = 1.0f/sqrtF(t8);
|
||||
H_BCN[7] = -t4*t9;
|
||||
H_BCN[8] = -t3*t9;
|
||||
// If we are not using the beacons as a height reference, we pretend that the beacons
|
||||
|
@ -102,23 +102,23 @@ void NavEKF3_core::FuseRngBcn()
|
|||
H_BCN[9] = -t2*t9;
|
||||
|
||||
// calculate Kalman gains
|
||||
float t10 = P[9][9]*t2*t9;
|
||||
float t11 = P[8][9]*t3*t9;
|
||||
float t12 = P[7][9]*t4*t9;
|
||||
float t13 = t10+t11+t12;
|
||||
float t14 = t2*t9*t13;
|
||||
float t15 = P[9][8]*t2*t9;
|
||||
float t16 = P[8][8]*t3*t9;
|
||||
float t17 = P[7][8]*t4*t9;
|
||||
float t18 = t15+t16+t17;
|
||||
float t19 = t3*t9*t18;
|
||||
float t20 = P[9][7]*t2*t9;
|
||||
float t21 = P[8][7]*t3*t9;
|
||||
float t22 = P[7][7]*t4*t9;
|
||||
float t23 = t20+t21+t22;
|
||||
float t24 = t4*t9*t23;
|
||||
ftype t10 = P[9][9]*t2*t9;
|
||||
ftype t11 = P[8][9]*t3*t9;
|
||||
ftype t12 = P[7][9]*t4*t9;
|
||||
ftype t13 = t10+t11+t12;
|
||||
ftype t14 = t2*t9*t13;
|
||||
ftype t15 = P[9][8]*t2*t9;
|
||||
ftype t16 = P[8][8]*t3*t9;
|
||||
ftype t17 = P[7][8]*t4*t9;
|
||||
ftype t18 = t15+t16+t17;
|
||||
ftype t19 = t3*t9*t18;
|
||||
ftype t20 = P[9][7]*t2*t9;
|
||||
ftype t21 = P[8][7]*t3*t9;
|
||||
ftype t22 = P[7][7]*t4*t9;
|
||||
ftype t23 = t20+t21+t22;
|
||||
ftype t24 = t4*t9*t23;
|
||||
varInnovRngBcn = R_BCN+t14+t19+t24;
|
||||
float t26;
|
||||
ftype t26;
|
||||
if (varInnovRngBcn >= R_BCN) {
|
||||
t26 = 1.0f/varInnovRngBcn;
|
||||
faultStatus.bad_rngbcn = false;
|
||||
|
@ -144,8 +144,8 @@ void NavEKF3_core::FuseRngBcn()
|
|||
Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9);
|
||||
Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9);
|
||||
} else {
|
||||
// zero indexes 10 to 12 = 3*4 bytes
|
||||
memset(&Kfusion[10], 0, 12);
|
||||
// zero indexes 10 to 12
|
||||
zero_range(&Kfusion[0], 10, 12);
|
||||
}
|
||||
|
||||
if (!inhibitDelVelBiasStates) {
|
||||
|
@ -158,8 +158,8 @@ void NavEKF3_core::FuseRngBcn()
|
|||
}
|
||||
}
|
||||
} else {
|
||||
// zero indexes 13 to 15 = 3*4 bytes
|
||||
memset(&Kfusion[13], 0, 12);
|
||||
// zero indexes 13 to 15
|
||||
zero_range(&Kfusion[0], 13, 15);
|
||||
}
|
||||
|
||||
// only allow the range observations to modify the vertical states if we are using it as a height reference
|
||||
|
@ -179,24 +179,24 @@ void NavEKF3_core::FuseRngBcn()
|
|||
Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9);
|
||||
Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
// zero indexes 16 to 21
|
||||
zero_range(&Kfusion[0], 16, 21);
|
||||
}
|
||||
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
|
||||
Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
|
||||
} else {
|
||||
// zero indexes 22 to 23 = 2*4 bytes
|
||||
memset(&Kfusion[22], 0, 8);
|
||||
// zero indexes 22 to 23
|
||||
zero_range(&Kfusion[0], 22, 23);
|
||||
}
|
||||
|
||||
// Calculate innovation using the selected offset value
|
||||
Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
|
||||
|
@ -283,7 +283,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
|
|||
void NavEKF3_core::FuseRngBcnStatic()
|
||||
{
|
||||
// get the estimated range measurement variance
|
||||
const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
|
||||
/*
|
||||
The first thing to do is to check if we have started the alignment and if not, initialise the
|
||||
|
@ -307,7 +307,7 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
}
|
||||
if (numBcnMeas >= 100) {
|
||||
rngBcnAlignmentStarted = true;
|
||||
float tempVar = 1.0f / (float)numBcnMeas;
|
||||
ftype tempVar = 1.0f / (ftype)numBcnMeas;
|
||||
// initialise the receiver position to the centre of the beacons and at zero height
|
||||
receiverPos.x = rngBcnPosSum.x * tempVar;
|
||||
receiverPos.y = rngBcnPosSum.y * tempVar;
|
||||
|
@ -336,7 +336,7 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
// position offset to be applied to the beacon system that minimises the range innovations
|
||||
// The position estimate should be stable after 100 iterations so we use a simple dual
|
||||
// hypothesis 1-state EKF to estimate the offset
|
||||
Vector3f refPosNED;
|
||||
Vector3F refPosNED;
|
||||
refPosNED.x = receiverPos.x;
|
||||
refPosNED.y = receiverPos.y;
|
||||
refPosNED.z = stateStruct.position.z;
|
||||
|
@ -355,14 +355,14 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
// and the main EKF vertical position
|
||||
|
||||
// Calculate the mid vertical position of all beacons
|
||||
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
|
||||
// calculate the delta to the estimated receiver position
|
||||
float delta = receiverPos.z - bcnMidPosD;
|
||||
ftype delta = receiverPos.z - bcnMidPosD;
|
||||
|
||||
// calcuate the two hypothesis for our vertical position
|
||||
float receverPosDownMax;
|
||||
float receverPosDownMin;
|
||||
ftype receverPosDownMax;
|
||||
ftype receverPosDownMin;
|
||||
if (delta >= 0.0f) {
|
||||
receverPosDownMax = receiverPos.z;
|
||||
receverPosDownMin = receiverPos.z - 2.0f * delta;
|
||||
|
@ -385,62 +385,62 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
}
|
||||
|
||||
// calculate the observation jacobian
|
||||
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
|
||||
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
|
||||
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
|
||||
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
|
||||
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
|
||||
ftype t5 = t2*t2;
|
||||
ftype t6 = t3*t3;
|
||||
ftype t7 = t4*t4;
|
||||
ftype t8 = t5+t6+t7;
|
||||
if (t8 < 0.1f) {
|
||||
// calculation will be badly conditioned
|
||||
return;
|
||||
}
|
||||
float t9 = 1.0f/sqrtf(t8);
|
||||
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
|
||||
float t15 = receiverPos.x*2.0f;
|
||||
float t11 = t10-t15;
|
||||
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
|
||||
float t14 = receiverPos.y*2.0f;
|
||||
float t13 = t12-t14;
|
||||
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
|
||||
float t18 = receiverPos.z*2.0f;
|
||||
float t17 = t16-t18;
|
||||
float H_RNG[3];
|
||||
ftype t9 = 1.0f/sqrtF(t8);
|
||||
ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
|
||||
ftype t15 = receiverPos.x*2.0f;
|
||||
ftype t11 = t10-t15;
|
||||
ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
|
||||
ftype t14 = receiverPos.y*2.0f;
|
||||
ftype t13 = t12-t14;
|
||||
ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
|
||||
ftype t18 = receiverPos.z*2.0f;
|
||||
ftype t17 = t16-t18;
|
||||
ftype H_RNG[3];
|
||||
H_RNG[0] = -t9*t11*0.5f;
|
||||
H_RNG[1] = -t9*t13*0.5f;
|
||||
H_RNG[2] = -t9*t17*0.5f;
|
||||
|
||||
// calculate the Kalman gains
|
||||
float t19 = receiverPosCov[0][0]*t9*t11*0.5f;
|
||||
float t20 = receiverPosCov[1][1]*t9*t13*0.5f;
|
||||
float t21 = receiverPosCov[0][1]*t9*t11*0.5f;
|
||||
float t22 = receiverPosCov[2][1]*t9*t17*0.5f;
|
||||
float t23 = t20+t21+t22;
|
||||
float t24 = t9*t13*t23*0.5f;
|
||||
float t25 = receiverPosCov[1][2]*t9*t13*0.5f;
|
||||
float t26 = receiverPosCov[0][2]*t9*t11*0.5f;
|
||||
float t27 = receiverPosCov[2][2]*t9*t17*0.5f;
|
||||
float t28 = t25+t26+t27;
|
||||
float t29 = t9*t17*t28*0.5f;
|
||||
float t30 = receiverPosCov[1][0]*t9*t13*0.5f;
|
||||
float t31 = receiverPosCov[2][0]*t9*t17*0.5f;
|
||||
float t32 = t19+t30+t31;
|
||||
float t33 = t9*t11*t32*0.5f;
|
||||
ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
|
||||
ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
|
||||
ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
|
||||
ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
|
||||
ftype t23 = t20+t21+t22;
|
||||
ftype t24 = t9*t13*t23*0.5f;
|
||||
ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
|
||||
ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
|
||||
ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
|
||||
ftype t28 = t25+t26+t27;
|
||||
ftype t29 = t9*t17*t28*0.5f;
|
||||
ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
|
||||
ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
|
||||
ftype t32 = t19+t30+t31;
|
||||
ftype t33 = t9*t11*t32*0.5f;
|
||||
varInnovRngBcn = R_RNG+t24+t29+t33;
|
||||
float t35 = 1.0f/varInnovRngBcn;
|
||||
float K_RNG[3];
|
||||
ftype t35 = 1.0f/varInnovRngBcn;
|
||||
ftype K_RNG[3];
|
||||
K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
|
||||
K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
|
||||
K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
|
||||
|
||||
// calculate range measurement innovation
|
||||
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
|
||||
Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
|
||||
deltaPosNED.z -= bcnPosOffsetNED.z;
|
||||
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted);
|
||||
|
@ -489,7 +489,7 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
// ensure the covariance matrix is symmetric
|
||||
for (uint8_t i=1; i<=2; i++) {
|
||||
for (uint8_t j=0; j<=i-1; j++) {
|
||||
float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
|
||||
ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
|
||||
receiverPosCov[i][j] = temp;
|
||||
receiverPosCov[j][i] = temp;
|
||||
}
|
||||
|
@ -517,7 +517,7 @@ void NavEKF3_core::FuseRngBcnStatic()
|
|||
Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
|
||||
Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
|
||||
*/
|
||||
void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning)
|
||||
void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning)
|
||||
{
|
||||
// Handle height offsets between the primary height source and the range beacons by estimating
|
||||
// the beacon systems global vertical position offset using a single state Kalman filter
|
||||
|
@ -525,32 +525,32 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||
// A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
|
||||
// The main filter then uses the offset with the smaller innovations
|
||||
|
||||
float innov; // range measurement innovation (m)
|
||||
float innovVar; // range measurement innovation variance (m^2)
|
||||
float gain; // Kalman gain
|
||||
float obsDeriv; // derivative of observation relative to state
|
||||
ftype innov; // range measurement innovation (m)
|
||||
ftype innovVar; // range measurement innovation variance (m^2)
|
||||
ftype gain; // Kalman gain
|
||||
ftype obsDeriv; // derivative of observation relative to state
|
||||
|
||||
const float stateNoiseVar = 0.1f; // State process noise variance
|
||||
const float filtAlpha = 0.1f; // LPF constant
|
||||
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
|
||||
const ftype stateNoiseVar = 0.1f; // State process noise variance
|
||||
const ftype filtAlpha = 0.1f; // LPF constant
|
||||
const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
|
||||
|
||||
// estimate upper value for offset
|
||||
|
||||
// calculate observation derivative
|
||||
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
|
||||
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
|
||||
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
float t9;
|
||||
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
|
||||
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
|
||||
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
|
||||
ftype t5 = t2*t2;
|
||||
ftype t6 = t3*t3;
|
||||
ftype t7 = t4*t4;
|
||||
ftype t8 = t5+t6+t7;
|
||||
ftype t9;
|
||||
if (t8 > 0.1f) {
|
||||
t9 = 1.0f/sqrtf(t8);
|
||||
t9 = 1.0f/sqrtF(t8);
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
|
||||
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMaxVar += stateNoiseVar;
|
||||
|
@ -563,8 +563,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
|
||||
|
||||
// calculate a filtered state change magnitude to be used to select between the high or low offset
|
||||
float stateChange = innov * gain;
|
||||
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
|
||||
ftype stateChange = innov * gain;
|
||||
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
|
@ -585,11 +585,11 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||
t5 = t2*t2;
|
||||
t8 = t5+t6+t7;
|
||||
if (t8 > 0.1f) {
|
||||
t9 = 1.0f/sqrtf(t8);
|
||||
t9 = 1.0f/sqrtF(t8);
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
|
||||
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMinVar += stateNoiseVar;
|
||||
|
@ -602,8 +602,8 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
|
||||
|
||||
// calculate a filtered state change magnitude to be used to select between the high or low offset
|
||||
float stateChange = innov * gain;
|
||||
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
|
||||
ftype stateChange = innov * gain;
|
||||
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
|
@ -618,7 +618,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|||
}
|
||||
|
||||
// calculate the mid vertical position of all beacons
|
||||
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
|
||||
// ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
|
||||
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
|
||||
|
|
|
@ -22,7 +22,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
}
|
||||
|
||||
// User defined multiplier to be applied to check thresholds
|
||||
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
|
||||
ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler;
|
||||
|
||||
if (gpsGoodToAlign) {
|
||||
/*
|
||||
|
@ -51,9 +51,9 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
const auto &gps = dal.gps();
|
||||
|
||||
const struct Location &gpsloc = gps.location(preferred_gps); // Current location
|
||||
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
|
||||
const ftype posFiltTimeConst = 10.0; // time constant used to decay position drift
|
||||
// calculate time lapsed since last update and limit to prevent numerical errors
|
||||
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
|
||||
ftype deltaTime = constrain_ftype(ftype(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
|
||||
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
|
||||
// Sum distance moved
|
||||
gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
|
||||
|
@ -81,8 +81,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
if (gpsDataNew.have_vz && onGround) {
|
||||
// check that the average vertical GPS velocity is close to zero
|
||||
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
|
||||
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
|
||||
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
|
||||
gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
|
||||
gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
|
||||
} else {
|
||||
gpsVertVelFail = false;
|
||||
}
|
||||
|
@ -91,7 +91,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
if (gpsVertVelFail) {
|
||||
dal.snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
|
||||
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler));
|
||||
gpsCheckStatus.bad_vert_vel = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_vert_vel = false;
|
||||
|
@ -102,8 +102,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
bool gpsHorizVelFail;
|
||||
if (onGround) {
|
||||
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
|
||||
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
|
||||
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
|
||||
gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
|
||||
gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
|
||||
} else {
|
||||
gpsHorizVelFail = false;
|
||||
}
|
||||
|
@ -240,14 +240,14 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
|
|||
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
|
||||
|
||||
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
|
||||
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
|
||||
const float tau = 10.0f; // time constant (sec) of peak hold decay
|
||||
const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
|
||||
const ftype tau = 10.0f; // time constant (sec) of peak hold decay
|
||||
if (lastGpsCheckTime_ms == 0) {
|
||||
lastGpsCheckTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
|
||||
ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
|
||||
lastGpsCheckTime_ms = imuSampleTime_ms;
|
||||
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
|
||||
ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f);
|
||||
|
||||
// get the receivers reported speed accuracy
|
||||
float gpsSpdAccRaw;
|
||||
|
@ -256,7 +256,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
|
|||
}
|
||||
|
||||
// filter the raw speed accuracy using a LPF
|
||||
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
|
||||
sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
|
||||
|
||||
// apply a peak hold filter to the LPF output
|
||||
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
|
||||
|
@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight()
|
|||
|
||||
if (assume_zero_sideslip()) {
|
||||
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
|
||||
float gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
|
||||
ftype gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
|
||||
bool highGndSpd = false;
|
||||
bool highAirSpd = false;
|
||||
bool largeHgtChange = false;
|
||||
|
@ -319,13 +319,13 @@ void NavEKF3_core::detectFlight()
|
|||
}
|
||||
|
||||
// trigger on ground speed
|
||||
const float gndSpdThresholdSq = sq(5.0f);
|
||||
const ftype gndSpdThresholdSq = sq(5.0f);
|
||||
if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) {
|
||||
highGndSpd = true;
|
||||
}
|
||||
|
||||
// trigger if more than 10m away from initial height
|
||||
if (fabsf(hgtMea) > 10.0f) {
|
||||
if (fabsF(hgtMea) > 10.0f) {
|
||||
largeHgtChange = true;
|
||||
}
|
||||
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -81,14 +81,21 @@
|
|||
#define GSF_YAW_VALID_HISTORY_THRESHOLD 5
|
||||
|
||||
// minimum variances allowed for velocity and position states
|
||||
#define VEL_STATE_MIN_VARIANCE 1E-4f
|
||||
#define POS_STATE_MIN_VARIANCE 1E-4f
|
||||
#define VEL_STATE_MIN_VARIANCE 1E-4
|
||||
#define POS_STATE_MIN_VARIANCE 1E-4
|
||||
|
||||
// maximum number of times the vertical velocity variance can hit the lower limit before the
|
||||
// associated states, variances and covariances are reset
|
||||
#define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT)
|
||||
#define EKF_TARGET_RATE_HZ uint32_t(1.0 / EKF_TARGET_DT)
|
||||
#define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
|
||||
|
||||
// limit on horizontal position states
|
||||
#if HAL_WITH_EKF_DOUBLE
|
||||
#define EK3_POSXY_STATE_LIMIT 50.0e6
|
||||
#else
|
||||
#define EK3_POSXY_STATE_LIMIT 1.0e6
|
||||
#endif
|
||||
|
||||
class NavEKF3_core : public NavEKF_core_common
|
||||
{
|
||||
public:
|
||||
|
@ -407,7 +414,6 @@ private:
|
|||
uint8_t imu_buffer_length;
|
||||
uint8_t obs_buffer_length;
|
||||
|
||||
typedef float ftype;
|
||||
#if MATH_CHECK_INDEXES
|
||||
typedef VectorN<ftype,2> Vector2;
|
||||
typedef VectorN<ftype,3> Vector3;
|
||||
|
@ -461,14 +467,14 @@ private:
|
|||
// broken down as individual elements. Both are equivalent (same
|
||||
// memory)
|
||||
struct state_elements {
|
||||
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame 0..3
|
||||
Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6
|
||||
Vector3f position; // position of IMU in local NED earth frame (m) 7..9
|
||||
Vector3f gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12
|
||||
Vector3f accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15
|
||||
Vector3f earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
|
||||
Vector3f body_magfield; // body frame magnetic field vector (Gauss) 19..21
|
||||
Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
|
||||
QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame 0..3
|
||||
Vector3F velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6
|
||||
Vector3F position; // position of IMU in local NED earth frame (m) 7..9
|
||||
Vector3F gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12
|
||||
Vector3F accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15
|
||||
Vector3F earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
|
||||
Vector3F body_magfield; // body frame magnetic field vector (Gauss) 19..21
|
||||
Vector2F wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
|
||||
};
|
||||
|
||||
union {
|
||||
|
@ -477,73 +483,73 @@ private:
|
|||
};
|
||||
|
||||
struct output_elements {
|
||||
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame
|
||||
Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec)
|
||||
Vector3f position; // position of body frame origin in local NED earth frame (m)
|
||||
QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame
|
||||
Vector3F velocity; // velocity of body frame origin in local NED earth frame (m/sec)
|
||||
Vector3F position; // position of body frame origin in local NED earth frame (m)
|
||||
};
|
||||
|
||||
struct imu_elements {
|
||||
Vector3f delAng; // IMU delta angle measurements in body frame (rad)
|
||||
Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec)
|
||||
float delAngDT; // time interval over which delAng has been measured (sec)
|
||||
float delVelDT; // time interval over which delVelDT has been measured (sec)
|
||||
Vector3F delAng; // IMU delta angle measurements in body frame (rad)
|
||||
Vector3F delVel; // IMU delta velocity measurements in body frame (m/sec)
|
||||
ftype delAngDT; // time interval over which delAng has been measured (sec)
|
||||
ftype delVelDT; // time interval over which delVelDT has been measured (sec)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
uint8_t gyro_index;
|
||||
uint8_t accel_index;
|
||||
};
|
||||
|
||||
struct gps_elements : EKF_obs_element_t {
|
||||
Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
|
||||
float hgt; // height of the GPS antenna in local NED earth frame (m)
|
||||
Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
|
||||
Vector2F pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
|
||||
ftype hgt; // height of the GPS antenna in local NED earth frame (m)
|
||||
Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
|
||||
uint8_t sensor_idx; // unique integer identifying the GPS sensor
|
||||
bool corrected; // true when the position and velocity have been corrected for sensor position
|
||||
bool have_vz; // true when vertical velocity is valid
|
||||
};
|
||||
|
||||
struct mag_elements : EKF_obs_element_t {
|
||||
Vector3f mag; // body frame magnetic field measurements (Gauss)
|
||||
Vector3F mag; // body frame magnetic field measurements (Gauss)
|
||||
};
|
||||
|
||||
struct baro_elements : EKF_obs_element_t {
|
||||
float hgt; // height of the pressure sensor in local NED earth frame (m)
|
||||
ftype hgt; // height of the pressure sensor in local NED earth frame (m)
|
||||
};
|
||||
|
||||
struct range_elements : EKF_obs_element_t {
|
||||
float rng; // distance measured by the range sensor (m)
|
||||
ftype rng; // distance measured by the range sensor (m)
|
||||
uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
|
||||
};
|
||||
|
||||
struct rng_bcn_elements : EKF_obs_element_t {
|
||||
float rng; // range measurement to each beacon (m)
|
||||
Vector3f beacon_posNED; // NED position of the beacon (m)
|
||||
float rngErr; // range measurement error 1-std (m)
|
||||
ftype rng; // range measurement to each beacon (m)
|
||||
Vector3F beacon_posNED; // NED position of the beacon (m)
|
||||
ftype rngErr; // range measurement error 1-std (m)
|
||||
uint8_t beacon_ID; // beacon identification number
|
||||
};
|
||||
|
||||
struct tas_elements : EKF_obs_element_t {
|
||||
float tas; // true airspeed measurement (m/sec)
|
||||
ftype tas; // true airspeed measurement (m/sec)
|
||||
};
|
||||
|
||||
struct of_elements : EKF_obs_element_t {
|
||||
Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
|
||||
Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
|
||||
Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
|
||||
Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m)
|
||||
Vector2F flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
|
||||
Vector2F flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
|
||||
Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
|
||||
Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m)
|
||||
};
|
||||
|
||||
struct vel_odm_elements : EKF_obs_element_t {
|
||||
Vector3f vel; // XYZ velocity measured in body frame (m/s)
|
||||
float velErr; // velocity measurement error 1-std (m/s)
|
||||
Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m)
|
||||
Vector3f angRate; // angular rate estimated from odometry (rad/sec)
|
||||
Vector3F vel; // XYZ velocity measured in body frame (m/s)
|
||||
ftype velErr; // velocity measurement error 1-std (m/s)
|
||||
Vector3F body_offset;// XYZ position of the velocity sensor in body frame (m)
|
||||
Vector3F angRate; // angular rate estimated from odometry (rad/sec)
|
||||
};
|
||||
|
||||
struct wheel_odm_elements : EKF_obs_element_t {
|
||||
float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
|
||||
float radius; // wheel radius (m)
|
||||
Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m)
|
||||
float delTime; // time interval that the measurement was accumulated over (sec)
|
||||
ftype delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
|
||||
ftype radius; // wheel radius (m)
|
||||
Vector3F hub_offset; // XYZ position of the wheel hub in body frame (m)
|
||||
ftype delTime; // time interval that the measurement was accumulated over (sec)
|
||||
};
|
||||
|
||||
// Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available
|
||||
|
@ -553,21 +559,21 @@ private:
|
|||
};
|
||||
|
||||
struct yaw_elements : EKF_obs_element_t {
|
||||
float yawAng; // yaw angle measurement (rad)
|
||||
float yawAngErr; // yaw angle 1SD measurement accuracy (rad)
|
||||
ftype yawAng; // yaw angle measurement (rad)
|
||||
ftype yawAngErr; // yaw angle 1SD measurement accuracy (rad)
|
||||
rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY)
|
||||
};
|
||||
|
||||
struct ext_nav_elements : EKF_obs_element_t {
|
||||
Vector3f pos; // XYZ position measured in a RH navigation frame (m)
|
||||
float posErr; // spherical position measurement error 1-std (m)
|
||||
Vector3F pos; // XYZ position measured in a RH navigation frame (m)
|
||||
ftype posErr; // spherical position measurement error 1-std (m)
|
||||
bool posReset; // true when the position measurement has been reset
|
||||
bool corrected; // true when the position has been corrected for sensor position
|
||||
};
|
||||
|
||||
struct ext_nav_vel_elements : EKF_obs_element_t {
|
||||
Vector3f vel; // velocity in NED (m/s)
|
||||
float err; // velocity measurement error (m/s)
|
||||
Vector3F vel; // velocity in NED (m/s)
|
||||
ftype err; // velocity measurement error (m/s)
|
||||
bool corrected; // true when the velocity has been corrected for sensor position
|
||||
};
|
||||
|
||||
|
@ -578,8 +584,8 @@ private:
|
|||
// bias estimates for the IMUs that are enabled but not being used
|
||||
// by this core.
|
||||
struct {
|
||||
Vector3f gyro_bias;
|
||||
Vector3f accel_bias;
|
||||
Vector3F gyro_bias;
|
||||
Vector3F accel_bias;
|
||||
} inactiveBias[INS_MAX_INSTANCES];
|
||||
|
||||
// Specify source of data to be used for a partial state reset
|
||||
|
@ -614,7 +620,7 @@ private:
|
|||
// calculate the predicted state covariance matrix
|
||||
// Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states
|
||||
// used to perform a reset of the quaternion state covariances only. Set to null for normal operation.
|
||||
void CovariancePrediction(Vector3f *rotVarVecPtr);
|
||||
void CovariancePrediction(Vector3F *rotVarVecPtr);
|
||||
|
||||
// force symmetry on the state covariance matrix
|
||||
void ForceSymmetry();
|
||||
|
@ -641,7 +647,7 @@ private:
|
|||
void FuseRngBcnStatic();
|
||||
|
||||
// calculate the offset from EKF vertical position datum to the range beacon system datum
|
||||
void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
|
||||
void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning);
|
||||
|
||||
// fuse magnetometer measurements
|
||||
void FuseMagnetometer();
|
||||
|
@ -665,21 +671,21 @@ private:
|
|||
void StoreQuatReset(void);
|
||||
|
||||
// Rotate the stored output quaternion history through a quaternion rotation
|
||||
void StoreQuatRotate(const Quaternion &deltaQuat);
|
||||
void StoreQuatRotate(const QuaternionF &deltaQuat);
|
||||
|
||||
// calculate the NED earth spin vector in rad/sec
|
||||
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
|
||||
void calcEarthRateNED(Vector3F &omega, int32_t latitude) const;
|
||||
|
||||
// initialise the covariance matrix
|
||||
void CovarianceInit();
|
||||
|
||||
// helper functions for readIMUData
|
||||
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
|
||||
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt);
|
||||
bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
|
||||
bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
|
||||
|
||||
// helper functions for correcting IMU data
|
||||
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
|
||||
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index);
|
||||
void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
|
||||
void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
|
||||
|
||||
// update IMU delta angle and delta velocity measurements
|
||||
void readIMUData();
|
||||
|
@ -744,10 +750,10 @@ private:
|
|||
void ResetPosition(resetDataSource posResetSource);
|
||||
|
||||
// reset the stateStruct's NE position to the specified position
|
||||
void ResetPositionNE(float posN, float posE);
|
||||
void ResetPositionNE(ftype posN, ftype posE);
|
||||
|
||||
// reset the stateStruct's D position
|
||||
void ResetPositionD(float posD);
|
||||
void ResetPositionD(ftype posD);
|
||||
|
||||
// reset velocity states using the last GPS measurement
|
||||
void ResetVelocity(resetDataSource velResetSource);
|
||||
|
@ -768,7 +774,7 @@ private:
|
|||
void checkDivergence(void);
|
||||
|
||||
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
|
||||
void calcIMU_Weighting(float K1, float K2);
|
||||
void calcIMU_Weighting(ftype K1, ftype K2);
|
||||
|
||||
// return true if the filter is ready to start using optical flow measurements for position and velocity estimation
|
||||
bool readyToUseOptFlow(void) const;
|
||||
|
@ -847,10 +853,10 @@ private:
|
|||
|
||||
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
|
||||
// Input is 1-sigma uncertainty in published declination
|
||||
void FuseDeclination(float declErr);
|
||||
void FuseDeclination(ftype declErr);
|
||||
|
||||
// return magnetic declination in radians
|
||||
float MagDeclination(void) const;
|
||||
ftype MagDeclination(void) const;
|
||||
|
||||
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
|
||||
// using a simple observer
|
||||
|
@ -905,7 +911,7 @@ private:
|
|||
// Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
|
||||
// Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
|
||||
// variances argument is updated with variances for each axis
|
||||
void CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const;
|
||||
void CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const;
|
||||
|
||||
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
|
||||
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
|
||||
|
@ -921,14 +927,14 @@ private:
|
|||
// yaw : new yaw angle (rad)
|
||||
// yaw_variance : variance of new yaw angle (rad^2)
|
||||
// order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle
|
||||
void resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order);
|
||||
void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
|
||||
|
||||
// attempt to reset the yaw to the EKF-GSF value
|
||||
// returns false if unsuccessful
|
||||
bool EKFGSF_resetMainFilterYaw();
|
||||
|
||||
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
|
||||
bool EKFGSF_getYaw(float &yaw, float &yawVariance) const;
|
||||
bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;
|
||||
|
||||
// Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
|
||||
void FuseDragForces();
|
||||
|
@ -948,7 +954,7 @@ private:
|
|||
bool badIMUdata; // boolean true if the bad IMU data is detected
|
||||
uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
|
||||
|
||||
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
|
||||
ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
|
||||
Matrix24 P; // covariance matrix
|
||||
EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
|
||||
EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
|
||||
|
@ -957,10 +963,10 @@ private:
|
|||
EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
|
||||
EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
|
||||
EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer
|
||||
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
|
||||
Matrix3F prevTnb; // previous nav to body transformation used for INS earth rotation compensation
|
||||
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
|
||||
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3F earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
ftype dtIMUavg; // expected time between IMU measurements (sec)
|
||||
ftype dtEkfAvg; // expected time between EKF updates (sec)
|
||||
ftype dt; // time lapsed since the last covariance prediction (sec)
|
||||
|
@ -976,12 +982,12 @@ private:
|
|||
bool fuseVelData; // this boolean causes the velNED measurements to be fused
|
||||
bool fusePosData; // this boolean causes the posNE measurements to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
|
||||
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
|
||||
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements
|
||||
Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
||||
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
|
||||
float defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
|
||||
ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
|
||||
ftype defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
|
||||
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
|
||||
MagCal effectiveMagCal; // the actual mag calibration being used as the default
|
||||
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
|
||||
|
@ -989,8 +995,8 @@ private:
|
|||
ftype innovBeta; // synthetic sideslip innovation (rad)
|
||||
uint32_t lastMagUpdate_us; // last time compass was updated in usec
|
||||
uint32_t lastMagRead_ms; // last time compass data was successfully read
|
||||
Vector3f velDotNED; // rate of change of velocity in NED frame
|
||||
Vector3f velDotNEDfilt; // low pass filtered velDotNED
|
||||
Vector3F velDotNED; // rate of change of velocity in NED frame
|
||||
Vector3F velDotNEDfilt; // low pass filtered velDotNED
|
||||
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
|
||||
bool tasDataToFuse; // true when new airspeed data is waiting to be fused
|
||||
uint32_t lastBaroReceived_ms; // time last time we received baro height data
|
||||
|
@ -1005,13 +1011,13 @@ private:
|
|||
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
|
||||
uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
|
||||
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
|
||||
Vector2f lastKnownPositionNE; // last known position
|
||||
Vector2F lastKnownPositionNE; // last known position
|
||||
uint32_t lastLaunchAccelTime_ms;
|
||||
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
|
||||
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
|
||||
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
|
||||
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
|
||||
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
|
||||
ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
|
||||
ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
|
||||
ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
|
||||
Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
|
||||
ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states are inactive
|
||||
bool lastInhibitMagStates; // previous inhibitMagStates
|
||||
|
@ -1021,15 +1027,15 @@ private:
|
|||
bool gpsNotAvailable; // bool true when valid GPS data is not available
|
||||
struct Location EKF_origin; // LLH origin of the NED axis system
|
||||
bool validOrigin; // true when the EKF origin is valid
|
||||
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
|
||||
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
|
||||
float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
|
||||
ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
|
||||
ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
|
||||
ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
|
||||
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
|
||||
uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
|
||||
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
|
||||
float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
|
||||
ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
|
||||
bool useGpsVertVel; // true if GPS vertical velocity should be used
|
||||
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
|
||||
ftype yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
|
||||
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
|
||||
bool tiltAlignComplete; // true when tilt alignment is complete
|
||||
bool yawAlignComplete; // true when yaw alignment is complete
|
||||
|
@ -1038,14 +1044,14 @@ private:
|
|||
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
|
||||
imu_elements imuDataNew; // IMU data at the current time horizon
|
||||
imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
|
||||
Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
|
||||
QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
|
||||
baro_elements baroDataNew; // Baro data at the current time horizon
|
||||
baro_elements baroDataDelayed; // Baro data at the fusion time horizon
|
||||
range_elements rangeDataNew; // Range finder data at the current time horizon
|
||||
range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
|
||||
tas_elements tasDataNew; // TAS data at the current time horizon
|
||||
tas_elements tasDataDelayed; // TAS data at the fusion time horizon
|
||||
float tasErrVar; // TAS error variance (m/s)**2
|
||||
ftype tasErrVar; // TAS error variance (m/s)**2
|
||||
bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value
|
||||
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
|
||||
gps_elements gpsDataNew; // GPS data at the current time horizon
|
||||
|
@ -1053,10 +1059,10 @@ private:
|
|||
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
|
||||
output_elements outputDataNew; // output state data at the current time step
|
||||
output_elements outputDataDelayed; // output state data at the current time step
|
||||
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
|
||||
Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m)
|
||||
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec)
|
||||
float innovYaw; // compass yaw angle innovation (rad)
|
||||
Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
|
||||
Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m)
|
||||
Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
|
||||
ftype innovYaw; // compass yaw angle innovation (rad)
|
||||
uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
|
||||
bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
|
||||
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
|
||||
|
@ -1068,94 +1074,94 @@ private:
|
|||
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
|
||||
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
|
||||
bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states
|
||||
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
|
||||
Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
|
||||
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
|
||||
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
|
||||
Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
|
||||
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
|
||||
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
|
||||
Vector2F velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
|
||||
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
|
||||
float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
|
||||
ftype posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
|
||||
uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset
|
||||
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
|
||||
Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
|
||||
float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
|
||||
ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
|
||||
QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
|
||||
ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
|
||||
uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
|
||||
bool runUpdates; // boolean true when the EKF updates can be run
|
||||
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
|
||||
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle
|
||||
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
|
||||
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
|
||||
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
|
||||
Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
|
||||
ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
|
||||
Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
|
||||
Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
|
||||
bool magFieldLearned; // true when the magnetic field has been learned
|
||||
uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
|
||||
Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
|
||||
Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
|
||||
Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
|
||||
Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
|
||||
bool delAngBiasLearned; // true when the gyro bias has been learned
|
||||
nav_filter_status filterStatus; // contains the status of various filter outputs
|
||||
float ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
|
||||
ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
|
||||
double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
|
||||
uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
|
||||
Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
|
||||
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
|
||||
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
|
||||
Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
|
||||
Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
|
||||
Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
|
||||
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
|
||||
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
|
||||
float tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
|
||||
ftype tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
|
||||
|
||||
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
|
||||
struct {
|
||||
float pos;
|
||||
float vel;
|
||||
float acc;
|
||||
ftype pos;
|
||||
ftype vel;
|
||||
ftype acc;
|
||||
} vertCompFiltState;
|
||||
|
||||
// variables used by the pre-initialisation GPS checks
|
||||
struct Location gpsloc_prev; // LLH location of previous GPS measurement
|
||||
uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
|
||||
float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
|
||||
float gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
|
||||
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
|
||||
ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
|
||||
ftype gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
|
||||
ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
|
||||
|
||||
// variable used by the in-flight GPS quality check
|
||||
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
|
||||
bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
|
||||
float sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
|
||||
float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
|
||||
ftype sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
|
||||
ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
|
||||
uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
|
||||
uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
|
||||
uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed
|
||||
bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
|
||||
Vector3f gpsVelInnov; // gps velocity innovations
|
||||
Vector3f gpsVelVarInnov; // gps velocity innovation variances
|
||||
Vector3F gpsVelInnov; // gps velocity innovations
|
||||
Vector3F gpsVelVarInnov; // gps velocity innovation variances
|
||||
uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts)
|
||||
|
||||
// variables added for optical flow fusion
|
||||
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
|
||||
of_elements ofDataNew; // OF data at the current time horizon
|
||||
bool flowDataValid; // true while optical flow data is still fresh
|
||||
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
|
||||
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
|
||||
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
|
||||
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
|
||||
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
|
||||
Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
|
||||
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
|
||||
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
|
||||
float Popt; // Optical flow terrain height state covariance (m^2)
|
||||
float terrainState; // terrain position state (m)
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
float varInnovRng; // range finder observation innovation variance (m^2)
|
||||
float innovRng; // range finder observation innovation (m)
|
||||
float hgtMea; // height measurement derived from either baro, gps or range finder data (m)
|
||||
ftype Popt; // Optical flow terrain height state covariance (m^2)
|
||||
ftype terrainState; // terrain position state (m)
|
||||
ftype prevPosN; // north position at last measurement
|
||||
ftype prevPosE; // east position at last measurement
|
||||
ftype varInnovRng; // range finder observation innovation variance (m^2)
|
||||
ftype innovRng; // range finder observation innovation (m)
|
||||
ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
|
||||
bool inhibitGndState; // true when the terrain position state is to remain constant
|
||||
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
|
||||
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2f auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
|
||||
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
|
||||
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
|
||||
Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
|
||||
ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2
|
||||
ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2F flowGyroBias; // bias error of optical flow sensor gyro output
|
||||
bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
|
||||
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
|
||||
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
|
||||
|
@ -1167,16 +1173,16 @@ private:
|
|||
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
|
||||
AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
|
||||
bool gndOffsetValid; // true when the ground offset state can still be considered valid
|
||||
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
|
||||
float delTimeOF; // time that delAngBodyOF is summed across
|
||||
Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
|
||||
ftype delTimeOF; // time that delAngBodyOF is summed across
|
||||
bool flowFusionActive; // true when optical flow fusion is active
|
||||
|
||||
Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m)
|
||||
Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
|
||||
|
||||
// Range finder
|
||||
float baroHgtOffset; // offset applied when when switching to use of Baro height
|
||||
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
|
||||
float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
|
||||
ftype baroHgtOffset; // offset applied when when switching to use of Baro height
|
||||
ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
|
||||
ftype storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors
|
||||
uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors
|
||||
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
|
||||
uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors
|
||||
|
@ -1214,49 +1220,49 @@ private:
|
|||
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
|
||||
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
|
||||
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
|
||||
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
|
||||
ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements
|
||||
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
|
||||
float varInnovRngBcn; // range beacon observation innovation variance (m^2)
|
||||
float innovRngBcn; // range beacon observation innovation (m)
|
||||
ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
|
||||
ftype innovRngBcn; // range beacon observation innovation (m)
|
||||
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec)
|
||||
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
|
||||
Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
|
||||
float beaconVehiclePosErr; // estimated position error from the beacon system (m)
|
||||
Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
|
||||
ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
|
||||
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
|
||||
bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
|
||||
uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
|
||||
Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter
|
||||
float receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
|
||||
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
|
||||
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
|
||||
bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started
|
||||
bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished
|
||||
uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter
|
||||
Vector3f rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
|
||||
Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
|
||||
uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
|
||||
float rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
|
||||
ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
|
||||
uint8_t N_beacons; // Number of range beacons in use
|
||||
float maxBcnPosD; // maximum position of all beacons in the down direction (m)
|
||||
float minBcnPosD; // minimum position of all beacons in the down direction (m)
|
||||
ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
|
||||
ftype minBcnPosD; // minimum position of all beacons in the down direction (m)
|
||||
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
|
||||
|
||||
float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
|
||||
float maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
|
||||
ftype bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
|
||||
ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
|
||||
|
||||
float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
|
||||
float minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
|
||||
ftype bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
|
||||
ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
|
||||
|
||||
Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
|
||||
Vector3F bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
|
||||
bool bcnOriginEstInit; // True when the beacon origin has been initialised
|
||||
|
||||
// Range Beacon Fusion Debug Reporting
|
||||
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
|
||||
struct rngBcnFusionReport_t {
|
||||
float rng; // measured range to beacon (m)
|
||||
float innov; // range innovation (m)
|
||||
float innovVar; // innovation variance (m^2)
|
||||
float testRatio; // innovation consistency test ratio
|
||||
Vector3f beaconPosNED; // beacon NED position
|
||||
ftype rng; // measured range to beacon (m)
|
||||
ftype innov; // range innovation (m)
|
||||
ftype innovVar; // innovation variance (m^2)
|
||||
ftype testRatio; // innovation consistency test ratio
|
||||
Vector3F beaconPosNED; // beacon NED position
|
||||
} *rngBcnFusionReport;
|
||||
|
||||
#if EK3_FEATURE_DRAG_FUSION
|
||||
|
@ -1265,10 +1271,10 @@ private:
|
|||
drag_elements dragSampleDelayed;
|
||||
drag_elements dragDownSampled; // down sampled from filter prediction rate to observation rate
|
||||
uint8_t dragSampleCount; // number of drag specific force samples accumulated at the filter prediction rate
|
||||
float dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
|
||||
Vector2f innovDrag; // multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2f innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
Vector2f dragTestRatio; // drag innovation consistency check ratio
|
||||
ftype dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
|
||||
Vector2F innovDrag; // multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
Vector2F dragTestRatio; // drag innovation consistency check ratio
|
||||
#endif
|
||||
bool dragFusionEnabled;
|
||||
|
||||
|
@ -1278,11 +1284,11 @@ private:
|
|||
|
||||
// Movement detector
|
||||
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
|
||||
float rngAtStartOfFlight; // range finder measurement at start of flight
|
||||
ftype rngAtStartOfFlight; // range finder measurement at start of flight
|
||||
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
|
||||
|
||||
// baro ground effect
|
||||
float meaHgtAtTakeOff; // height measured at commencement of takeoff
|
||||
ftype meaHgtAtTakeOff; // height measured at commencement of takeoff
|
||||
|
||||
// control of post takeoff magnetic field and heading resets
|
||||
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
|
||||
|
@ -1291,22 +1297,22 @@ private:
|
|||
bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements
|
||||
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
|
||||
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
|
||||
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
|
||||
float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
|
||||
Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset
|
||||
ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
|
||||
ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
|
||||
QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset
|
||||
|
||||
// Used by on ground movement check required when operating on ground without a yaw reference
|
||||
float gyro_diff; // filtered gyro difference (rad/s)
|
||||
float accel_diff; // filtered acceerometer difference (m/s/s)
|
||||
Vector3f gyro_prev; // gyro vector from previous time step (rad/s)
|
||||
Vector3f accel_prev; // accelerometer vector from previous time step (m/s/s)
|
||||
ftype gyro_diff; // filtered gyro difference (rad/s)
|
||||
ftype accel_diff; // filtered acceerometer difference (m/s/s)
|
||||
Vector3F gyro_prev; // gyro vector from previous time step (rad/s)
|
||||
Vector3F accel_prev; // accelerometer vector from previous time step (m/s/s)
|
||||
bool onGroundNotMoving; // true when on the ground and not moving
|
||||
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
|
||||
|
||||
// variables used to inhibit accel bias learning
|
||||
bool inhibitDelVelBiasStates; // true when all IMU delta velocity bias states are de-activated
|
||||
bool dvelBiasAxisInhibit[3] {}; // true when IMU delta velocity bias states for a specific axis is de-activated
|
||||
Vector3f dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2
|
||||
Vector3F dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2
|
||||
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
// external navigation fusion
|
||||
|
@ -1320,8 +1326,8 @@ private:
|
|||
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
|
||||
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
|
||||
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
|
||||
Vector3f extNavVelInnov; // external nav velocity innovations
|
||||
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
|
||||
Vector3F extNavVelInnov; // external nav velocity innovations
|
||||
Vector3F extNavVelVarInnov; // external nav velocity innovation variances
|
||||
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
|
||||
EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
|
||||
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
|
||||
|
@ -1384,8 +1390,8 @@ private:
|
|||
ftype magXbias;
|
||||
ftype magYbias;
|
||||
ftype magZbias;
|
||||
Matrix3f DCM;
|
||||
Vector3f MagPred;
|
||||
Matrix3F DCM;
|
||||
Vector3F MagPred;
|
||||
ftype R_MAG;
|
||||
Vector9 SH_MAG;
|
||||
} mag_state;
|
||||
|
@ -1395,8 +1401,8 @@ private:
|
|||
|
||||
// earth field from WMM tables
|
||||
bool have_table_earth_field; // true when we have initialised table_earth_field_ga
|
||||
Vector3f table_earth_field_ga; // earth field from WMM tables
|
||||
float table_declination; // declination in radians from the tables
|
||||
Vector3F table_earth_field_ga; // earth field from WMM tables
|
||||
ftype table_declination; // declination in radians from the tables
|
||||
|
||||
// timing statistics
|
||||
struct ekf_timing timing;
|
||||
|
@ -1408,7 +1414,7 @@ private:
|
|||
bool assume_zero_sideslip(void) const;
|
||||
|
||||
// vehicle specific initial gyro bias uncertainty
|
||||
float InitialGyroBiasUncertainty(void) const;
|
||||
ftype InitialGyroBiasUncertainty(void) const;
|
||||
|
||||
/*
|
||||
learn magnetometer biases from GPS yaw. Return true if the
|
||||
|
|
Loading…
Reference in New Issue