fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-05 13:40:05 +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-17 14:51:24 +11: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;
EKF/ekf.h
+2 -1
View File
@@ -414,7 +414,8 @@ private:
bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
float _saved_mag_variance[6] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_covmat[2][2] {}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
gps_check_fail_status_u _gps_check_fail_status{};