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

EKF: Don't discard declination certainty information when resuming 3-axis fusion.

Browse Source 
In the case where the EKF is switching between 3-axis and heading fusion, off-diagonal elements containing the correlation between N,E  components of the earth field were being lost on each switch event. These elements contained information about the declination uncertainty and should be preserved.
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent 25148e1b45
commit 8839e4e1f4
2 changed files with 23 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+21 -6
View File
@@ -1463,9 +1463,16 @@ void Ekf::controlMagFusion()
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
// re-instate the last used variances
for (uint8_t index = 0; index <= 5; index ++) {
P[index + 16][index + 16] = _saved_mag_variance[index];
// re-instate variances for the D earth axis and XYZ body axis field
for (uint8_t index = 0; index <= 3; index ++) {
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];
}
}
}
}
@@ -1475,10 +1482,18 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
} else {
// save magnetic field state variances for next time
// save magnetic field state covariance data for next time
if (_control_status.flags.mag_3D) {
for (uint8_t index = 0; index <= 5; index ++) {
_saved_mag_variance[index] = P[index + 16][index + 16];
// save variances for the D earth axis and XYZ body axis field
for (uint8_t index = 0; index <= 3; index ++) {
_saved_mag_bf_variance[index] = P[index + 18][index + 18];
}
// 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];
}
}
_control_status.flags.mag_3D = false;