dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: reduce KH to only the elements that are really needed, and merge the loops

This commit is contained in:
Beat Küng 2016-11-02 10:26:40 +01:00
parent 1b59a89a18
commit 07c6aabd98
3 changed files with 71 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
EKF/airspeed_fusion.cpp
View File

@ -161,23 +161,22 @@ void Ekf::fuseAirspeed()
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[row][4] = Kfusion[row] * H_TAS[4];
KH[row][5] = Kfusion[row] * H_TAS[5];
KH[row][6] = Kfusion[row] * H_TAS[6];
KH[row][22] = Kfusion[row] * H_TAS[22];
KH[row][23] = Kfusion[row] * H_TAS[23];
}
float KHP[_k_num_states][_k_num_states]; float KHP[_k_num_states][_k_num_states];
float KH[5];
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_TAS[4];
KH[1] = Kfusion[row] * H_TAS[5];
KH[2] = Kfusion[row] * H_TAS[6];
KH[3] = Kfusion[row] * H_TAS[22];
KH[4] = Kfusion[row] * H_TAS[23];
for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][4] * P[4][column]; float tmp = KH[0] * P[4][column];
tmp += KH[row][5] * P[5][column]; tmp += KH[1] * P[5][column];
tmp += KH[row][6] * P[6][column]; tmp += KH[2] * P[6][column];
tmp += KH[row][22] * P[22][column]; tmp += KH[3] * P[22][column];
tmp += KH[row][23] * P[23][column]; tmp += KH[4] * P[23][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }

83
EKF/mag_fusion.cpp
View File

@ -309,31 +309,32 @@ void Ekf::fuseMag()
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 3; column++) {
KH[row][column] = Kfusion[row] * H_MAG[column];
}
for (unsigned column = 16; column <= 21; column++) {
KH[row][column] = Kfusion[row] * H_MAG[column];
}
}
float KHP[_k_num_states][_k_num_states]; float KHP[_k_num_states][_k_num_states];
float KH[10];
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_MAG[0];
KH[1] = Kfusion[row] * H_MAG[1];
KH[2] = Kfusion[row] * H_MAG[2];
KH[3] = Kfusion[row] * H_MAG[3];
KH[4] = Kfusion[row] * H_MAG[16];
KH[5] = Kfusion[row] * H_MAG[17];
KH[6] = Kfusion[row] * H_MAG[18];
KH[7] = Kfusion[row] * H_MAG[19];
KH[8] = Kfusion[row] * H_MAG[20];
KH[9] = Kfusion[row] * H_MAG[21];
for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column]; float tmp = KH[0] * P[0][column];
tmp += KH[row][1] * P[1][column]; tmp += KH[1] * P[1][column];
tmp += KH[row][2] * P[2][column]; tmp += KH[2] * P[2][column];
tmp += KH[row][3] * P[3][column]; tmp += KH[3] * P[3][column];
tmp += KH[row][16] * P[16][column]; tmp += KH[4] * P[16][column];
tmp += KH[row][17] * P[17][column]; tmp += KH[5] * P[17][column];
tmp += KH[row][18] * P[18][column]; tmp += KH[6] * P[18][column];
tmp += KH[row][19] * P[19][column]; tmp += KH[7] * P[19][column];
tmp += KH[row][20] * P[20][column]; tmp += KH[8] * P[20][column];
tmp += KH[row][21] * P[21][column]; tmp += KH[9] * P[21][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }
@ -639,20 +640,20 @@ void Ekf::fuseHeading()
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 3; column++) {
KH[row][column] = Kfusion[row] * H_YAW[column];
}
}
float KHP[_k_num_states][_k_num_states]; float KHP[_k_num_states][_k_num_states];
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_YAW[0];
KH[1] = Kfusion[row] * H_YAW[1];
KH[2] = Kfusion[row] * H_YAW[2];
KH[3] = Kfusion[row] * H_YAW[3];
for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column]; float tmp = KH[0] * P[0][column];
tmp += KH[row][1] * P[1][column]; tmp += KH[1] * P[1][column];
tmp += KH[row][2] * P[2][column]; tmp += KH[2] * P[2][column];
tmp += KH[row][3] * P[3][column]; tmp += KH[3] * P[3][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }
@ -777,18 +778,16 @@ void Ekf::fuseDeclination()
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP
// take advantage of the empty columns in KH to reduce the number of operations // take advantage of the empty columns in KH to reduce the number of operations
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 16; column <= 17; column++) {
KH[row][column] = Kfusion[row] * H_DECL[column];
}
}
float KHP[_k_num_states][_k_num_states]; float KHP[_k_num_states][_k_num_states];
float KH[2];
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_DECL[16];
KH[1] = Kfusion[row] * H_DECL[17];
for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][16] * P[16][column]; float tmp = KH[0] * P[16][column];
tmp += KH[row][17] * P[17][column]; tmp += KH[1] * P[17][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }

31
EKF/optflow_fusion.cpp
View File

@ -433,23 +433,26 @@ void Ekf::fuseOptFlow()
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 6; column++) {
KH[row][column] = gain[row] * H_LOS[obs_index][column];
}
}
float KHP[_k_num_states][_k_num_states]; float KHP[_k_num_states][_k_num_states];
float KH[7];
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = gain[row] * H_LOS[obs_index][0];
KH[1] = gain[row] * H_LOS[obs_index][1];
KH[2] = gain[row] * H_LOS[obs_index][2];
KH[3] = gain[row] * H_LOS[obs_index][3];
KH[4] = gain[row] * H_LOS[obs_index][4];
KH[5] = gain[row] * H_LOS[obs_index][5];
KH[6] = gain[row] * H_LOS[obs_index][6];
for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column]; float tmp = KH[0] * P[0][column];
tmp += KH[row][1] * P[1][column]; tmp += KH[1] * P[1][column];
tmp += KH[row][2] * P[2][column]; tmp += KH[2] * P[2][column];
tmp += KH[row][3] * P[3][column]; tmp += KH[3] * P[3][column];
tmp += KH[row][4] * P[4][column]; tmp += KH[4] * P[4][column];
tmp += KH[row][5] * P[5][column]; tmp += KH[5] * P[5][column];
tmp += KH[row][6] * P[6][column]; tmp += KH[6] * P[6][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }