fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 21:24:08 +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-17 15:51:37 +11:00 committed by Daniel Agar
parent 8839e4e1f4
commit 708c79eb8f
4 changed files with 35 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
EKF/control.cpp
View File

@ -1484,18 +1484,7 @@ void Ekf::controlMagFusion()
} else {
// save magnetic field state covariance data for next time
if (_control_status.flags.mag_3D) {
// 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];
}
}
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
@ -1528,6 +1517,9 @@ void Ekf::controlMagFusion()
// 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();
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {

3
EKF/covariance.cpp
View File

@ -892,6 +892,9 @@ void Ekf::resetMagCovariance()
// 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();
}
void Ekf::resetWindCovariance()

3
EKF/ekf.h
View File

@ -674,6 +674,9 @@ private:
// Argument is additional yaw variance in rad**2
void increaseQuatYawErrVariance(float yaw_variance);
// save mag field state covariance data for re-use
void save_mag_cov_data();
// uncorrelate quaternion states from other states
void uncorrelateQuatStates();

25
EKF/ekf_helper.cpp
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];
}
}
}