fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag: remove immediate declination fusion after reset

Browse Source
This commit is contained in:
bresch
2024-05-31 10:38:17 -04:00
committed by Daniel Agar
parent a6007e4b93
commit c3d984703c
3 changed files with 20 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+19 -40
View File
@@ -59,13 +59,6 @@ void Ekf::controlMagFusion()
return;
}
// stop mag (require a reset before using again) if there was an external yaw reset (yaw estimator, GPS yaw, etc)
if (_mag_decl_cov_reset && (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat)) {
ECL_INFO("yaw reset, stopping mag fusion to force reinitialization");
stopMagFusion();
resetMagCov();
}
magSample mag_sample;
if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
@@ -154,11 +147,13 @@ void Ekf::controlMagFusion()
// WMM update can occur on the last epoch, just after mag fusion
const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
{
const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
const bool common_conditions_passing = _control_status.flags.mag
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
@@ -186,9 +181,8 @@ void Ekf::controlMagFusion()
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional.
const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
_control_status.flags.mag_dec = _control_status.flags.mag
&& (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
&& (!using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
if (_control_status.flags.mag) {
@@ -200,33 +194,22 @@ void Ekf::controlMagFusion()
aid_src.time_last_fuse = _time_delayed_us;
} else {
if (!_mag_decl_cov_reset) {
// After any magnetic field covariance reset event the earth field state
// covariances need to be corrected to incorporate knowledge of the declination
// before fusing magnetometer data to prevent rapid rotation of the earth field
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag_sample.mag, R_MAG, H, aid_src);
// The normal sequence is to fuse the magnetometer data first before fusing
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
const bool update_tilt = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
} else {
// The normal sequence is to fuse the magnetometer data first before fusing
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
const bool update_tilt = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
if (update_all_states && update_tilt) {
_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
}
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
if (update_all_states && update_tilt) {
_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
}
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
}
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
}
}
@@ -272,7 +255,6 @@ void Ekf::controlMagFusion()
// activate fusion, reset mag states and initialize variance if first init or in flight reset
if (!_control_status.flags.yaw_align
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
|| (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
@@ -399,9 +381,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
resetMagHeading(mag);
}
// earth field was reset to WMM, skip initial declination fusion
_mag_decl_cov_reset = true;
} else {
// mag_B: reset
_state.mag_B.zero();
src/modules/ekf2/EKF/covariance.cpp
+1 -4
View File
@@ -308,10 +308,7 @@ void Ekf::resetAccelBiasCov()
#if defined(CONFIG_EKF2_MAGNETOMETER)
void Ekf::resetMagCov()
{
if (_mag_decl_cov_reset) {
ECL_INFO("reset mag covariance");
_mag_decl_cov_reset = false;
}
ECL_INFO("reset mag covariance");
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
src/modules/ekf2/EKF/ekf.h
-1
View File
@@ -705,7 +705,6 @@ private:
// used by magnetometer fusion mode selection
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
uint8_t _nb_mag_3d_reset_available{0};
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time