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

ekf2: return saved mag bias variance when not in 3d magnetometer fusion

Browse Source 
- the estimated mag bias was requiring > 30 seconds of continuous 3d
mag fusion to be reported stable (and saved back to mag cal), this
restores the original intent requiring 30 seconds of accumulated valid
3d mag fusion
This commit is contained in:
Daniel Agar 2022-03-11 13:36:32 -05:00
parent 5717434e93
commit 7ef38112d2
2 changed files with 29 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/ekf2/EKF/ekf.h
View File

@ -246,7 +246,15 @@ public:
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag_3D) {
return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
}
return _saved_mag_bf_variance;
}
bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
@ -518,8 +526,9 @@ private:
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
Matrix2f _saved_mag_ef_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
gps_check_fail_status_u _gps_check_fail_status{};

31
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -879,10 +879,7 @@ void Ekf::resetMagBias()
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
// _saved_mag_bf_variance[0] is the the D earth axis
_saved_mag_bf_variance[1] = 0;
_saved_mag_bf_variance[2] = 0;
_saved_mag_bf_variance[3] = 0;
_saved_mag_bf_variance.zero();
}
// get EKF innovation consistency check status information comprising of:
@ -1487,24 +1484,30 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
void Ekf::saveMagCovData()
{
// 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 variances for XYZ body axis field
_saved_mag_bf_variance(0) = P(19, 19);
_saved_mag_bf_variance(1) = P(20, 20);
_saved_mag_bf_variance(2) = P(21, 21);
// save the NE axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<2, 2>(16, 16);
_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
// save variance for the D earth axis
_saved_mag_ef_d_variance = P(18, 18);
}
void Ekf::loadMagCovData()
{
// 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 variances for the XYZ body axis field
P(19, 19) = _saved_mag_bf_variance(0);
P(20, 20) = _saved_mag_bf_variance(1);
P(21, 21) = _saved_mag_bf_variance(2);
// re-instate the NE axis covariance sub-matrix
P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
// re-instate the D earth axis variance
P(18, 18) = _saved_mag_ef_d_variance;
}
void Ekf::startAirspeedFusion()