fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 08:19:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: mag field disturbed only care about NE for heading

Browse Source 
- mag_field_disturbed look at full mag field strength (after mag bias applied)
 - mag_field_ne_disturbed (NEW) only consider north east component of
mag field (what's actually used for mag heading)
This commit is contained in:
Daniel Agar 2022-11-07 10:53:21 -05:00
parent 4190353192
commit be453c098f
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
4 changed files with 41 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorStatusFlags.msg
View File

@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on win
bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing
bool cs_fake_pos # 32 - true when fake position measurements are being fused
bool cs_fake_hgt # 33 - true when fake height measurements are being fused
bool cs_mag_field_ne_disturbed # 34 - true when the NE mag field does not match the expected strength
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

1
src/modules/ekf2/EKF/common.h
View File

@ -535,6 +535,7 @@ union filter_control_status_u {
uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused
uint64_t mag_field_ne_disturbed : 1; ///< 34 - true when the NE mag field does not match the expected strength
} flags;
uint64_t value;
};

54
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -65,17 +65,39 @@ void Ekf::controlMagFusion()
_control_status.flags.synthetic_mag_z = false;
}
_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
const Vector3f mag_earth_pred = R_to_earth * mag_observation;
// mag field disturbed
if (_params.check_mag_strength) {
_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_observation);
// mag field NE disturbed
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
// expected mag field from the world magnetic model in earth frame
const Vector3f wmm = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
const float wmm_ne_norm = Vector2f(wmm(0), wmm(1)).norm();
// norm magnetometer NE components
const float mag_ne_norm = Vector2f(mag_earth_pred(0), mag_earth_pred(1)).norm();
// magnetometer NE strength must be within ~20% of the expected value
const float gate_size = wmm_ne_norm * 0.2f;
_control_status.flags.mag_field_ne_disturbed = !isMeasuredMatchingExpected(mag_ne_norm, wmm_ne_norm, gate_size);
}
} else {
_control_status.flags.mag_field_disturbed = false;
_control_status.flags.mag_field_ne_disturbed = false;
}
resetEstimatorAidStatus(_aid_src_mag_heading);
_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();;
_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
// compute magnetometer innovations (for estimator_aid_src_mag logging)
@ -193,7 +215,7 @@ void Ekf::runOnGroundYawReset()
bool Ekf::canResetMagHeading() const
{
return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE);
return !_control_status.flags.mag_field_ne_disturbed && (_params.mag_fusion_type != MagFuseType::NONE);
}
void Ekf::runInAirYawReset()
@ -233,6 +255,8 @@ void Ekf::runInAirYawReset()
);
resetMagCov();
has_realigned_yaw = true;
}
if (!has_realigned_yaw && canResetMagHeading()) {
@ -351,23 +375,19 @@ bool Ekf::shouldInhibitMag() const
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel;
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_ne_disturbed;
}
bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const
{
if (_params.check_mag_strength
&& ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
if (PX4_ISFINITE(_mag_strength_gps)) {
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
if (PX4_ISFINITE(_mag_strength_gps)) {
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
} else {
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
}
} else {
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
}
return false;

3
src/modules/ekf2/EKF2.cpp
View File

@ -1370,7 +1370,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_ne_disturbed;
status.accel_device_id = _device_id_accel;
status.baro_device_id = _device_id_baro;
@ -1446,6 +1446,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent;
status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos;
status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt;
status_flags.cs_mag_field_ne_disturbed = _ekf.control_status_flags().mag_field_ne_disturbed;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;