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

Make Kfusion a Vector24f

Browse Source
This commit is contained in:
kamilritz
2020-07-02 09:02:24 +02:00
committed by Mathieu Bresciani
parent 356b2f6ffe
commit 16a00eae0b
11 changed files with 619 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+9 -9
View File
@@ -431,10 +431,10 @@ void Ekf::fuseOptFlow()
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// copy the Kalman gain vector for the axis we are fusing
float gain[24];
Vector24f gain;
for (unsigned row = 0; row <= 23; row++) {
gain[row] = Kfusion[row][obs_index];
gain(row) = Kfusion[row][obs_index];
}
// apply covariance correction via P_new = (I -K*H)*P
@@ -445,13 +445,13 @@ void Ekf::fuseOptFlow()
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];
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[0] * P(0,column);