Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Apply optimisations to declination fusion covariance update

Browse Source 
These are the same type of optimisations that were successful with the magnetometer fusion
This commit is contained in:
Paul Riseborough 2015-10-20 14:23:32 +11:00 committed by Andrew Tridgell
parent 0054291cf4
commit 60c2e81d19
1 changed files with 10 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -754,31 +754,23 @@ void NavEKF2_core::FuseDeclination()
// 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<=15; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=15; j++) {
KH[i][j] = 0.0f;
}
for (uint8_t j = 16; j<=17; j++) {
if (!inhibitMagStates) {
KH[i][j] = Kfusion[i] * H_MAG[j];
} else {
KH[i][j] = 0.0f;
}
}
for (uint8_t j = 18; j<=23; j++) {
KH[i][16] = Kfusion[i] * H_MAG[16];
KH[i][17] = Kfusion[i] * H_MAG[17];
for (unsigned j = 18; j<=23; j++) {
KH[i][j] = 0.0f;
}
}
for (uint8_t i = 0; i<=stateIndexLim; i++) {
for (uint8_t j = 0; j<=stateIndexLim; j++) {
KHP[i][j] = 0;
for (uint8_t k = 16; k<=17; 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++) {
KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j];
}
}
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];
}
}