zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
Beat Küng 8 years ago
parent
1b59a89a18
commit
07c6aabd98
3 changed files with 70 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 27
      EKF/airspeed_fusion.cpp
  2. 81
      EKF/mag_fusion.cpp
  3. 31
      EKF/optflow_fusion.cpp

27
EKF/airspeed_fusion.cpp
Unescape View File

@ -161,23 +161,22 @@ void Ekf::fuseAirspeed() @@ -161,23 +161,22 @@ void Ekf::fuseAirspeed()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for 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 KH[5];
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++) {
float tmp = KH[row][4] * P[4][column];
tmp += KH[row][5] * P[5][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[4][column];
tmp += KH[1] * P[5][column];
tmp += KH[2] * P[6][column];
tmp += KH[3] * P[22][column];
tmp += KH[4] * P[23][column];
KHP[row][column] = tmp;
}
}

81
EKF/mag_fusion.cpp
Unescape View File

@ -309,31 +309,32 @@ void Ekf::fuseMag() @@ -309,31 +309,32 @@ void Ekf::fuseMag()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KH[_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 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];
}
}
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];
float KHP[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
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][16] * P[16][column];
tmp += KH[row][17] * P[17][column];
tmp += KH[row][18] * P[18][column];
tmp += KH[row][19] * P[19][column];
tmp += KH[row][20] * P[20][column];
tmp += KH[row][21] * P[21][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[16][column];
tmp += KH[5] * P[17][column];
tmp += KH[6] * P[18][column];
tmp += KH[7] * P[19][column];
tmp += KH[8] * P[20][column];
tmp += KH[9] * P[21][column];
KHP[row][column] = tmp;
}
}
@ -639,20 +640,20 @@ void Ekf::fuseHeading() @@ -639,20 +640,20 @@ void Ekf::fuseHeading()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for 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 KH[4];
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++) {
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];
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];
KHP[row][column] = tmp;
}
}
@ -777,18 +778,16 @@ void Ekf::fuseDeclination() @@ -777,18 +778,16 @@ void Ekf::fuseDeclination()
// first calculate expression for KHP
// then calculate P - KHP
// 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 KH[2];
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++) {
float tmp = KH[row][16] * P[16][column];
tmp += KH[row][17] * P[17][column];
float tmp = KH[0] * P[16][column];
tmp += KH[1] * P[17][column];
KHP[row][column] = tmp;
}
}

31
EKF/optflow_fusion.cpp
Unescape View File

@ -433,23 +433,26 @@ void Ekf::fuseOptFlow() @@ -433,23 +433,26 @@ void Ekf::fuseOptFlow()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for 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 KH[7];
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++) {
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][5] * P[5][column];
tmp += KH[row][6] * P[6][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];
KHP[row][column] = tmp;
}
}

Write Preview
Loading…
Cancel
Save