aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: down to 218us for test[9]

Browse Source
This commit is contained in:
Andrew Tridgell 2015-10-20 08:57:39 +11:00
parent fc23be8025
commit a017ae7e00
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -525,22 +525,21 @@ void NavEKF2_core::FuseMagnetometer()
perf_begin(_perf_test[9]);
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
KHP[i][j] = 0;
for (unsigned k=0; k<=2; k++) {
KHP[i][j] += KH[i][k] * P[k][j];
}
if (!inhibitMagStates) {
KHP[i][j] += KH[i][16] * P[16][j];
KHP[i][j] += KH[i][17] * P[17][j];
KHP[i][j] += KH[i][18] * P[18][j];
KHP[i][j] += KH[i][19] * P[19][j];
KHP[i][j] += KH[i][20] * P[20][j];
KHP[i][j] += KH[i][21] * P[21][j];
}
ftype res = 0;
res += KH[i][0] * P[0][j];
res += KH[i][1] * P[1][j];
res += KH[i][2] * P[2][j];
res += KH[i][16] * P[16][j];
res += KH[i][17] * P[17][j];
res += KH[i][18] * P[18][j];
res += KH[i][19] * P[19][j];
res += KH[i][20] * P[20][j];
res += KH[i][21] * P[21][j];
KHP[i][j] = res;
}
}
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}