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

EKF2: improve resilience against incorrect mag data

Browse Source 
- when GNSS is used require low mag heading innovations during
  horizontal acceleration (yaw observable) to validate the mag
- only fuse mag heading just enough to constrain the yaw estimate
  variance to a sane value. Leave enough uncertainty to allow for a
  correction when the yaw is observable through GNSS fusion
This commit is contained in:
Mathieu Bresciani
2023-08-17 09:55:15 -04:00
committed by GitHub
parent eaad11bb74
commit 74a54b3b12
15 changed files with 685 additions and 636 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_heading_control.cpp
+20 -3
View File
@@ -68,18 +68,23 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
if (_control_status.flags.yaw_align) {
// mag heading
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
_mag_heading_innov_lpf.update(aid_src.innovation);
} else {
// mag heading delta (logging only)
aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
- wrap_pi(measured_hdg - _mag_heading_prev));
_mag_heading_innov_lpf.reset(0.f);
}
// determine if we should use mag heading aiding
const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING)
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
&& _control_status.flags.tilt_align
&& (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
@@ -99,6 +104,7 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
const bool declination_changed = _control_status_prev.flags.mag_hdg
&& (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
bool skip_timeout_check = false;
if (mag_sample.reset || declination_changed) {
if (declination_changed) {
@@ -113,10 +119,21 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
aid_src.time_last_fuse = _time_delayed_us;
} else {
fuseYaw(aid_src);
Vector24f H_YAW;
computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW);
if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) {
// Only fuse mag to constrain the yaw variance to a safe value
fuseYaw(aid_src, H_YAW);
} else {
// Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check
skip_timeout_check = true;
aid_src.test_ratio = 0.f; // required for preflight checks to pass
}
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check;
if (is_fusion_failing) {
if (_nb_mag_heading_reset_available > 0) {