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

EKF: Ensure mag field state covariance data is always available for re-use

Browse Source 
Prevents use of  _saved_mag_ef_covmat and _saved_mag_bf_variance before being initialised or when stale.
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent 8839e4e1f4
commit 708c79eb8f
4 changed files with 35 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+25
View File
@@ -494,6 +494,9 @@ bool Ekf::realignYawGPS()
// 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();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -532,6 +535,9 @@ bool Ekf::realignYawGPS()
// 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();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -694,6 +700,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// 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();
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -1708,3 +1717,19 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
P[3][1] -= yaw_variance*SQ[1]*SQ[3];
P[3][2] -= yaw_variance*SQ[0]*SQ[3];
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
void Ekf::save_mag_cov_data()
{
// 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];
}
}
}