diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 2fef831ffe..c0ea29f1dd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -94,9 +94,14 @@ void NavEKF3_core::FuseAirspeed() } if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) { - Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = SK_TAS[0]*(P[stateIndex][4]*SH_TAS[2] - P[stateIndex][22]*SH_TAS[2] + P[stateIndex][5]*SK_TAS[1] - P[stateIndex][23]*SK_TAS[1] + P[stateIndex][6]*vd*SH_TAS[0]); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -388,9 +393,14 @@ void NavEKF3_core::FuseSideslip() } if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) { - Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); - Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); - Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = SK_BETA[0]*(P[stateIndex][0]*SK_BETA[5] + P[stateIndex][1]*SK_BETA[4] - P[stateIndex][4]*SK_BETA[1] + P[stateIndex][5]*SK_BETA[2] + P[stateIndex][2]*SK_BETA[6] + P[stateIndex][6]*SK_BETA[3] - P[stateIndex][3]*SK_BETA[7] + P[stateIndex][22]*SK_BETA[1] - P[stateIndex][23]*SK_BETA[2]); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index ace96cd72e..8659348d1b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -602,9 +602,14 @@ void NavEKF3_core::FuseMagnetometer() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = SK_MX[0]*(P[stateIndex][19] + P[stateIndex][1]*SH_MAG[0] - P[stateIndex][2]*SH_MAG[1] + P[stateIndex][3]*SH_MAG[2] + P[stateIndex][0]*SK_MX[2] - P[stateIndex][16]*SK_MX[1] + P[stateIndex][17]*SK_MX[4] - P[stateIndex][18]*SK_MX[3]); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -677,9 +682,14 @@ void NavEKF3_core::FuseMagnetometer() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = SK_MY[0]*(P[stateIndex][20] + P[stateIndex][0]*SH_MAG[2] + P[stateIndex][1]*SH_MAG[1] + P[stateIndex][2]*SH_MAG[0] - P[stateIndex][3]*SK_MY[2] - P[stateIndex][17]*SK_MY[1] - P[stateIndex][16]*SK_MY[3] + P[stateIndex][18]*SK_MY[4]); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -754,9 +764,14 @@ void NavEKF3_core::FuseMagnetometer() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = SK_MZ[0]*(P[stateIndex][21] + P[stateIndex][0]*SH_MAG[1] - P[stateIndex][1]*SH_MAG[2] + P[stateIndex][3]*SH_MAG[0] + P[stateIndex][2]*SK_MZ[2] + P[stateIndex][18]*SK_MZ[1] + P[stateIndex][16]*SK_MZ[4] - P[stateIndex][17]*SK_MZ[3]); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -1287,9 +1302,14 @@ void NavEKF3_core::FuseDeclination(float declErr) } if (!inhibitDelVelBiasStates) { - Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); - Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); - Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = -t4*t13*(P[stateIndex][16]*magE-P[stateIndex][17]*magN); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index b09e737e05..8c55f8028a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -466,9 +466,14 @@ void NavEKF3_core::FuseOptFlow() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); - Kfusion[14] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); - Kfusion[15] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = t78*(P[stateIndex][0]*t2*t5-P[stateIndex][4]*t2*t7+P[stateIndex][1]*t2*t15+P[stateIndex][6]*t2*t10+P[stateIndex][2]*t2*t19-P[stateIndex][3]*t2*t22+P[stateIndex][5]*t2*t27); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -637,9 +642,14 @@ void NavEKF3_core::FuseOptFlow() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); - Kfusion[14] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); - Kfusion[15] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = -t78*(P[stateIndex][0]*t2*t5+P[stateIndex][5]*t2*t8-P[stateIndex][6]*t2*t10+P[stateIndex][1]*t2*t16-P[stateIndex][2]*t2*t19+P[stateIndex][3]*t2*t22+P[stateIndex][4]*t2*t27); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index ea6e749ff4..8041688188 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1363,9 +1363,14 @@ void NavEKF3_core::FuseBodyVel() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24); - Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24); - Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = t77*(P[stateIndex][5]*t4+P[stateIndex][4]*t9+P[stateIndex][0]*t14-P[stateIndex][6]*t11+P[stateIndex][1]*t18-P[stateIndex][2]*t21+P[stateIndex][3]*t24); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -1535,9 +1540,14 @@ void NavEKF3_core::FuseBodyVel() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25); - Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25); - Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = t77*(-P[stateIndex][4]*t3+P[stateIndex][5]*t8+P[stateIndex][0]*t15+P[stateIndex][6]*t12+P[stateIndex][1]*t18+P[stateIndex][2]*t22-P[stateIndex][3]*t25); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); @@ -1708,9 +1718,14 @@ void NavEKF3_core::FuseBodyVel() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24); - Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24); - Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = t77*(P[stateIndex][4]*t4+P[stateIndex][0]*t14+P[stateIndex][6]*t9-P[stateIndex][5]*t11-P[stateIndex][1]*t17+P[stateIndex][2]*t20+P[stateIndex][3]*t24); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 4bb037032d..4abc2ef5a2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -149,9 +149,14 @@ void NavEKF3_core::FuseRngBcn() } if (!inhibitDelVelBiasStates) { - Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9); - Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9); - Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9); + for (uint8_t index = 0; index < 3; index++) { + uint8_t stateIndex = index + 13; + if (!dvelBiasAxisInhibit[index]) { + Kfusion[stateIndex] = -t26*(P[stateIndex][7]*t4*t9+P[stateIndex][8]*t3*t9+P[stateIndex][9]*t2*t9); + } else { + Kfusion[stateIndex] = 0.0f; + } + } } else { // zero indexes 13 to 15 = 3*4 bytes memset(&Kfusion[13], 0, 12); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 68a849a325..e341b8cd79 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -226,6 +226,8 @@ void NavEKF3_core::InitialiseVariables() lastRngMeasTime_ms = 0; // initialise other variables + memset(&dvelBiasAxisInhibit, 0, sizeof(dvelBiasAxisInhibit)); + dvelBiasAxisVarPrev.zero(); gpsNoiseScaler = 1.0f; hgtTimeout = true; tasTimeout = true; @@ -1131,6 +1133,24 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f); dvxVar = dvyVar = dvzVar = sq(dt*_accNoise); + if (!inhibitDelVelBiasStates) { + for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) { + const unsigned index = stateIndex - 13; + + // Don't attempt learning of IMU delta velocty bias if on ground and not aligned with the gravity vector + const bool is_bias_observable = (fabsf(prevTnb[index][2]) > 0.8f) && onGround; + + if (!is_bias_observable && !dvelBiasAxisInhibit[index]) { + // store variances to be reinstated wben learnign can commence later + dvelBiasAxisVarPrev[index] = P[stateIndex][stateIndex]; + dvelBiasAxisInhibit[index] = true; + } else if (is_bias_observable && dvelBiasAxisInhibit[index]) { + P[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index]; + dvelBiasAxisInhibit[index] = false; + } + } + } + // calculate the predicted covariance due to inertial sensor error propagation // we calculate the lower diagonal and copy to take advantage of symmetry @@ -1659,6 +1679,18 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) } } + // inactive delta velocity bias states have all covariances zeroed to prevent + // interacton with other states + if (!inhibitDelVelBiasStates) { + for (uint8_t index=0; index<3; index++) { + uint8_t stateIndex = index + 13; + if (dvelBiasAxisInhibit[index]) { + zeroCols(nextP,stateIndex,stateIndex); + nextP[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index]; + } + } + } + // if the total position variance exceeds 1e4 (100m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -1783,6 +1815,7 @@ void NavEKF3_core::ConstrainVariances() zeroRows(P,10,12); } + const float minStateVarTarget = 1E-8f; if (!inhibitDelVelBiasStates) { // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum @@ -1799,7 +1832,6 @@ void NavEKF3_core::ConstrainVariances() // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must // not exceed 100 and the minimum variance must not fall below the target minimum - const float minStateVarTarget = 1E-8f; float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { P[stateIndex][stateIndex] = constrain_float(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg)); @@ -1823,6 +1855,10 @@ void NavEKF3_core::ConstrainVariances() } else { zeroCols(P,13,15); zeroRows(P,13,15); + for (uint8_t i=0; i<=2; i++) { + const uint8_t stateIndex = 1 + 13; + P[stateIndex][stateIndex] = fmaxf(P[stateIndex][stateIndex], minStateVarTarget); + } } if (!inhibitMagStates) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b4f7f0a58f..3b31ef0699 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1029,7 +1029,6 @@ private: bool lastInhibitMagStates; // previous inhibitMagStates bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction bool needEarthBodyVarReset; // we need to reset mag earth variances at next CovariancePrediction - bool inhibitDelVelBiasStates; // true when IMU delta velocity bias states are inactive bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive bool gpsNotAvailable; // bool true when valid GPS data is not available struct Location EKF_origin; // LLH origin of the NED axis system @@ -1325,6 +1324,11 @@ private: 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 + #if EK3_FEATURE_EXTERNAL_NAV // external navigation fusion EKF_obs_buffer_t storedExtNav; // external navigation data buffer