From 528e010c53f862aa2e890aaf5ad363b8da4e1681 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 2 May 2026 13:06:31 -0500 Subject: [PATCH] AP_NavEKF3: merge zeroRows and zeroCols into P-specific form These are always called together and always called with `P` as the first argument. `P` must remain symmetric, so merging them removes chances for bugs and saves flash. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 9 ++-- libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 3 +- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 18 +++----- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 46 ++++++------------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 7 +-- 6 files changed, 28 insertions(+), 58 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 9d459f9a855..6b83d67d74e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -89,16 +89,14 @@ void NavEKF3_core::setWindMagStateLearningMode() } // set the wind state variances to the measurement uncertainty - zeroCols(P, 22, 23); - zeroRows(P, 22, 23); + zeroStatesVarCov(22, 23); P[22][22] = P[23][23] = trueAirspeedVariance; windStatesAligned = true; } else { // set the variances using a typical max wind speed for small UAV operation - zeroCols(P, 22, 23); - zeroRows(P, 22, 23); + zeroStatesVarCov(22, 23); for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(WIND_VEL_VARIANCE_MAX); } @@ -276,8 +274,7 @@ void NavEKF3_core::setAidingMode() for (uint8_t row=0; row<6; row++) { oldBiasVariance[row] = P[row+10][row+10]; } - zeroCols(P,10,15); - zeroRows(P,10,15); + zeroStatesVarCov(10, 15); for (uint8_t row=0; row<6; row++) { P[row+10][row+10] = oldBiasVariance[row]; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp index 5e00a886100..53890bdf5d4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp @@ -7,8 +7,7 @@ void NavEKF3_core::resetGyroBias(void) { stateStruct.gyro_bias.zero(); - zeroRows(P,10,12); - zeroCols(P,10,12); + zeroStatesVarCov(10, 12); P[10][10] = sq(radians(0.5f * dtIMUavg)); P[11][11] = P[10][10]; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index e087b1d06dc..ea8bd3570a1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -1247,8 +1247,7 @@ void NavEKF3_core::alignMagStateDeclination() // zero the corresponding state covariances if magnetic field state learning is active ftype var_16 = P[16][16]; ftype var_17 = P[17][17]; - zeroRows(P,16,17); - zeroCols(P,16,17); + zeroStatesVarCov(16, 17); P[16][16] = var_16; P[17][17] = var_17; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index b7429438b35..515b8583b58 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -38,8 +38,7 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource) velResetNE.y = stateStruct.velocity.y; // reset the corresponding covariances - zeroRows(P,4,5); - zeroCols(P,4,5); + zeroStatesVarCov(4, 5); if (PV_AidingMode != AID_ABSOLUTE) { stateStruct.velocity.xy().zero(); @@ -118,8 +117,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) posResetNE.y = stateStruct.position.y; // reset the corresponding covariances - zeroRows(P,7,8); - zeroCols(P,7,8); + zeroStatesVarCov(7, 8); if (PV_AidingMode != AID_ABSOLUTE) { // reset all position state history to the last known position @@ -201,8 +199,7 @@ bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t ti posResetNE.y = stateStruct.position.y; // reset the corresponding covariances - zeroRows(P,7,8); - zeroCols(P,7,8); + zeroStatesVarCov(7, 8); // handle unknown accuracy if (isnan(posAccuracy)) { @@ -318,8 +315,7 @@ void NavEKF3_core::ResetHeight(void) lastHgtPassTime_ms = imuSampleTime_ms; // reset the corresponding covariances - zeroRows(P,9,9); - zeroCols(P,9,9); + zeroStatesVarCov(9, 9); // set the variances to the measurement variance P[9][9] = posDownObsNoise; @@ -347,8 +343,7 @@ void NavEKF3_core::ResetHeight(void) vertCompFiltState.vel = outputDataNew.velocity.z; // reset the corresponding covariances - zeroRows(P,6,6); - zeroCols(P,6,6); + zeroStatesVarCov(6, 6); // set the variances to the measurement variance #if EK3_FEATURE_EXTERNAL_NAV @@ -954,8 +949,7 @@ void NavEKF3_core::FuseVelPosNED() fusePosData = false; // Reset the position variances and corresponding covariances to a value that will pass the checks - zeroRows(P,7,8); - zeroCols(P,7,8); + zeroStatesVarCov(7, 8); P[7][7] = sq(ftype(0.5f*frontend->_gpsGlitchRadiusMax)); P[8][8] = P[7][7]; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8b7c023215c..f77ea66b27b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1083,8 +1083,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) if (needMagBodyVarReset) { // reset body mag variances needMagBodyVarReset = false; - zeroCols(P,19,21); - zeroRows(P,19,21); + zeroStatesVarCov(19, 21); P[19][19] = sq(frontend->_magNoise); P[20][20] = P[19][19]; P[21][21] = P[19][19]; @@ -1093,8 +1092,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) if (needEarthBodyVarReset) { // reset mag earth field variances needEarthBodyVarReset = false; - zeroCols(P,16,18); - zeroRows(P,16,18); + zeroStatesVarCov(16, 18); P[16][16] = sq(frontend->_magNoise); P[17][17] = P[16][16]; P[18][18] = P[16][16]; @@ -1170,8 +1168,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) dayVar = R_bf.b.y; dazVar = R_bf.c.z; quatCovResetOnly = true; - zeroRows(P,0,3); - zeroCols(P,0,3); + zeroStatesVarCov(0, 3); } else { ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f); daxVar = dayVar = dazVar = sq(dt*_gyrNoise); @@ -1803,8 +1800,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) for (uint8_t index=0; index<3; index++) { const uint8_t stateIndex = index + 13; if (dvelBiasAxisInhibit[index]) { - zeroRows(P, stateIndex, stateIndex); - zeroCols(P, stateIndex, stateIndex); + zeroStatesVarCov(stateIndex, stateIndex); P[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index]; } } @@ -1824,23 +1820,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) #endif } -// zero specified range of rows in the state covariance matrix -void NavEKF3_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last) +// zero specified state variances and covariances in state covariance matrix +void NavEKF3_core::zeroStatesVarCov(uint8_t first, uint8_t last) { uint8_t row; for (row=first; row<=last; row++) { - zero_range(&covMat[row][0], 0, 23); + zero_range(&P[row][0], 0, 23); } -} -// zero specified range of columns in the state covariance matrix -void NavEKF3_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last) -{ - uint8_t row; for (row=0; row<=23; row++) { - zero_range(&covMat[row][0], first, last); + zero_range(&P[row][0], first, last); } } @@ -1918,8 +1909,7 @@ void NavEKF3_core::ConstrainVariances() vertVelVarClipCounter += EKF_TARGET_RATE_HZ; if (vertVelVarClipCounter > VERT_VEL_VAR_CLIP_COUNT_LIM) { // reset the corresponding covariances - zeroRows(P,6,6); - zeroCols(P,6,6); + zeroStatesVarCov(6, 6); // set the variances to the measurement variance #if EK3_FEATURE_EXTERNAL_NAV @@ -1939,8 +1929,7 @@ void NavEKF3_core::ConstrainVariances() if (!inhibitDelAngBiasStates) { for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,sq(0.175 * dtEkfAvg)); } else { - zeroCols(P,10,12); - zeroRows(P,10,12); + zeroStatesVarCov(10, 12); } const ftype minSafeStateVar = 5E-9; @@ -1967,8 +1956,7 @@ void NavEKF3_core::ConstrainVariances() // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { // reset all delta velocity bias covariances - zeroCols(P,13,15); - zeroRows(P,13,15); + zeroStatesVarCov(13, 15); // set all delta velocity bias variances to initial values and zero bias states P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); P[14][14] = P[13][13]; @@ -1977,8 +1965,7 @@ void NavEKF3_core::ConstrainVariances() } } else { - zeroCols(P,13,15); - zeroRows(P,13,15); + zeroStatesVarCov(13, 15); // set all delta velocity bias variances to a margin above the minimum safe value for (uint8_t i=0; i<=2; i++) { const uint8_t stateIndex = i + 13; @@ -1990,8 +1977,7 @@ void NavEKF3_core::ConstrainVariances() for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // earth magnetic field for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // body magnetic field } else { - zeroCols(P,16,21); - zeroRows(P,16,21); + zeroStatesVarCov(16, 21); } if (!inhibitWindStates) { @@ -2001,8 +1987,7 @@ void NavEKF3_core::ConstrainVariances() for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); } } else { - zeroCols(P,22,23); - zeroRows(P,22,23); + zeroStatesVarCov(22, 23); } } @@ -2177,8 +2162,7 @@ void NavEKF3_core::resetMagFieldStates() alignMagStateDeclination(); // set the remaining variances and covariances - zeroRows(P,18,21); - zeroCols(P,18,21); + zeroStatesVarCov(18, 21); P[18][18] = sq(frontend->_magNoise); P[19][19] = P[18][18]; P[20][20] = P[18][18]; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 796d334b75c..c4398b594e4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -773,11 +773,8 @@ private: // fuse synthetic sideslip measurement of zero void FuseSideslip(); - // zero specified range of rows in the state covariance matrix - void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last); - - // zero specified range of columns in the state covariance matrix - void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last); + // zero specified state variances and covariances in state covariance matrix + void zeroStatesVarCov(uint8_t first, uint8_t last); // Reset the stored output history to current data void StoreOutputReset(void);