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

EKF: Rework use of fuseDeclination()

Browse Source 
Simplify calling so that it is only called in two ways:
1) Immediately before 3-axis mag fusion if not called since the last earth field covariance reset so that the earth field declination information can be formed.
2) Immediately after 3-axis mag fusion otherwise.
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent d52f53635b
commit bd1647a7fb
4 changed files with 28 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+4 -9
View File
@@ -486,15 +486,13 @@ bool Ekf::realignYawGPS()
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_mag_decl_cov_reset = false;
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
@@ -529,15 +527,13 @@ bool Ekf::realignYawGPS()
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_mag_decl_cov_reset = false;
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
@@ -572,6 +568,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
}
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_mag_decl_cov_reset = false;
_control_status.flags.mag_hdg = false;
return false;
@@ -701,15 +698,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_mag_decl_cov_reset = false;
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}