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

EKF: Remove unnecessary matrix operations from optical flow fusion

Browse Source 
The updated formulation means that H_LOS[][8] is always zero, so these operations are no longer required.
This commit is contained in:
Paul Riseborough
2016-03-11 11:03:43 +11:00
parent b3b0f1347a
commit dd1d58bab5
1 changed files with 0 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
-3
View File
@@ -456,8 +456,6 @@ void Ekf::fuseOptFlow()
for (unsigned column = 0; column <= 5; column++) { for (unsigned column = 0; column <= 5; column++) {
KH[row][column] = gain[row] * H_LOS[obs_index][column]; KH[row][column] = gain[row] * H_LOS[obs_index][column];
} }
KH[row][8] = gain[row] * H_LOS[obs_index][8];
} }
for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned row = 0; row < _k_num_states; row++) {
@@ -468,7 +466,6 @@ void Ekf::fuseOptFlow()
tmp += KH[row][3] * P[3][column]; tmp += KH[row][3] * P[3][column];
tmp += KH[row][4] * P[4][column]; tmp += KH[row][4] * P[4][column];
tmp += KH[row][5] * P[5][column]; tmp += KH[row][5] * P[5][column];
tmp += KH[row][8] * P[8][column];
KHP[row][column] = tmp; KHP[row][column] = tmp;
} }
} }