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

saved_mag_ef_cov to Squarematrix2f

Browse Source
This commit is contained in:
kamilritz
2020-08-04 09:57:00 +02:00
committed by Mathieu Bresciani
parent 7609bc69b2
commit 15d360f446
2 changed files with 4 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2 -10
View File
@@ -1517,11 +1517,7 @@ void Ekf::saveMagCovData()
}
// save the NE axis covariance sub-matrix
for (uint8_t row = 0; row <= 1; row ++) {
for (uint8_t col = 0; col <= 1; col ++) {
_saved_mag_ef_covmat[row][col] = P(row + 16,col + 16);
}
}
_saved_mag_ef_covmat = P.slice<2, 2>(16, 16);
}
void Ekf::loadMagCovData()
@@ -1531,11 +1527,7 @@ void Ekf::loadMagCovData()
P(index + 18,index + 18) = _saved_mag_bf_variance[index];
}
// re-instate the NE axis covariance sub-matrix
for (uint8_t row = 0; row <= 1; row ++) {
for (uint8_t col = 0; col <= 1; col ++) {
P(row + 16,col + 16) = _saved_mag_ef_covmat[row][col];
}
}
P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
}
void Ekf::stopGpsFusion()