fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 17:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: fix bug in sideslip fusion and reduce RAM usage

Browse Source 
The bug meant the  Y velocity (state index 5)  covariance was not being updated correctly when sideslip was being used to constrain velocity drift (extended GPS loss).
The rework of the covariance update to reduce RAM follows the same pattern as adopted for other fusion processes
This commit is contained in:
Paul Riseborough
2016-11-02 14:53:47 +11:00
committed by Lorenz Meier
parent 951c8b77af
commit f76a2a5ff7
1 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/sideslip_fusion.cpp
+22 -20
View File
@@ -189,7 +189,7 @@ void Ekf::fuseSideslip()
_innov_check_fail_status.flags.reject_sideslip = true;
return;
} else {
} else {
_innov_check_fail_status.flags.reject_sideslip = false;
}
@@ -200,28 +200,30 @@ void Ekf::fuseSideslip()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[9];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[row][0] = Kfusion[row] * H_BETA[0];
KH[row][1] = Kfusion[row] * H_BETA[1];
KH[row][2] = Kfusion[row] * H_BETA[2];
KH[row][3] = Kfusion[row] * H_BETA[3];
KH[row][4] = Kfusion[row] * H_BETA[4];
KH[row][5] = Kfusion[row] * H_BETA[5];
KH[row][6] = Kfusion[row] * H_BETA[6];
KH[row][22] = Kfusion[row] * H_BETA[22];
KH[row][23] = Kfusion[row] * H_BETA[23];
}
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_BETA[0];
KH[1] = Kfusion[row] * H_BETA[1];
KH[2] = Kfusion[row] * H_BETA[2];
KH[3] = Kfusion[row] * H_BETA[3];
KH[4] = Kfusion[row] * H_BETA[4];
KH[5] = Kfusion[row] * H_BETA[5];
KH[6] = Kfusion[row] * H_BETA[6];
KH[7] = Kfusion[row] * H_BETA[22];
KH[8] = Kfusion[row] * H_BETA[23];
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column];
tmp += KH[row][1] * P[1][column];
tmp += KH[row][2] * P[2][column];
tmp += KH[row][3] * P[3][column];
tmp += KH[row][4] * P[4][column];
tmp += KH[row][6] * P[6][column];
tmp += KH[row][22] * P[22][column];
tmp += KH[row][23] * P[23][column];
float tmp = KH[0] * P[0][column];
tmp += KH[1] * P[1][column];
tmp += KH[2] * P[2][column];
tmp += KH[3] * P[3][column];
tmp += KH[4] * P[4][column];
tmp += KH[5] * P[5][column];
tmp += KH[6] * P[6][column];
tmp += KH[7] * P[22][column];
tmp += KH[8] * P[23][column];
KHP[row][column] = tmp;
}
}