Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Down to 12 from 14 usec for perf test[8]

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-20 13:02:28 +11:00 committed by Andrew Tridgell
parent 0f530bb5a0
commit b5e43288d4
1 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -496,21 +496,17 @@ void NavEKF2_core::FuseMagnetometer()
// 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] = Kfusion[i] * H_MAG[j];
}
for (uint8_t j = 3; j<=15; j++) {
for (unsigned j = 3; j<=15; j++) {
KH[i][j] = 0.0f;
}
for (uint8_t j = 16; j<=21; j++) {
if (!inhibitMagStates) {
KH[i][j] = Kfusion[i] * H_MAG[j];
} else {
KH[i][j] = 0.0f;
}
for (unsigned j = 16; j<=21; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
for (uint8_t j = 22; j<=23; j++) {
for (unsigned j = 22; j<=23; j++) {
KH[i][j] = 0.0f;
}
}