mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: Apply optimisations to airspeed fusion covariance update
These are the same optimisations that were successful with the magnetometer fusion
This commit is contained in:
parent
a29147d6d2
commit
5e4bc4e954
|
@ -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];
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue