From 5e4bc4e9542bf67a09a5f88735f828dfb5594841 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 20 Oct 2015 13:05:46 +1100 Subject: [PATCH] AP_NavEKF2: Apply optimisations to airspeed fusion covariance update These are the same optimisations that were successful with the magnetometer fusion --- .../AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 850eeeafe3..40afd74f32 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -149,33 +149,33 @@ void NavEKF2_core::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i<=stateIndexLim; i++) { - for (uint8_t j = 0; j<=2; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=2; j++) { KH[i][j] = 0.0f; } - for (uint8_t j = 3; j<=5; j++) { + for (unsigned j = 3; j<=5; j++) { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 6; j<=21; j++) { + for (unsigned j = 6; j<=21; j++) { KH[i][j] = 0.0f; } - for (uint8_t j = 22; j<=23; j++) { + for (unsigned j = 22; j<=23; j++) { KH[i][j] = Kfusion[i] * H_TAS[j]; } } - for (uint8_t i = 0; i<=stateIndexLim; i++) { - for (uint8_t j = 0; j<=stateIndexLim; j++) { - KHP[i][j] = 0; - for (uint8_t k = 3; k<=5; k++) { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 22; k<=23; k++) { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][3] * P[3][j]; + res += KH[i][4] * P[4][j]; + res += KH[i][5] * P[5][j]; + res += KH[i][22] * P[22][j]; + res += KH[i][23] * P[23][j]; + KHP[i][j] = res; } } - for (uint8_t i = 0; i<=stateIndexLim; i++) { - for (uint8_t j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=stateIndexLim; j++) { P[i][j] = P[i][j] - KHP[i][j]; } }