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

ekf2-mag: do not limit the earth mag field estimate

Browse Source 
The EKF can recover from an initial bad earth mag field estimate.
Constraining the field is not necessary and can lead to an unpredicted
behavior of the filter.
Declination fusion is safe to run even when the horizontal field is 0
This commit is contained in:
bresch
2024-05-22 10:48:23 +02:00
committed by Daniel Agar
parent 774b6ed3b8
commit 6515935b52
2 changed files with 0 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
-76
View File
@@ -124,7 +124,6 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
fused[index] = true;
limitDeclination();
}
}
@@ -192,87 +191,12 @@ bool Ekf::fuseDeclination(float decl_sigma)
_fault_status.flags.bad_mag_decl = !is_fused;
if (is_fused) {
limitDeclination();
}
return is_fused;
}
return false;
}
void Ekf::limitDeclination()
{
const Vector3f mag_I_before = _state.mag_I;
// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
float decl_reference = NAN;
float h_field_min = 0.001f;
if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL
&& (PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps) && PX4_ISFINITE(_mag_inclination_gps))
) {
decl_reference = _mag_declination_gps;
// set to 50% of the horizontal strength from geo tables if location is known
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
// use parameter value if GPS isn't available
decl_reference = math::radians(_params.mag_declination_deg);
}
if (!PX4_ISFINITE(decl_reference)) {
return;
}
// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
// and can result in a reversal of the NE field states which the filter cannot recover from
// apply a circular limit
float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1));
if (h_field < h_field_min) {
if (h_field > 0.001f * h_field_min) {
const float h_scaler = h_field_min / h_field;
_state.mag_I(0) *= h_scaler;
_state.mag_I(1) *= h_scaler;
} else {
// too small to scale radially so set to expected value
_state.mag_I(0) = 2.0f * h_field_min * cosf(decl_reference);
_state.mag_I(1) = 2.0f * h_field_min * sinf(decl_reference);
}
h_field = h_field_min;
}
// do not allow the declination estimate to vary too much relative to the reference value
constexpr float decl_tolerance = 0.5f;
const float decl_max = decl_reference + decl_tolerance;
const float decl_min = decl_reference - decl_tolerance;
const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0));
if (decl_estimate > decl_max) {
_state.mag_I(0) = h_field * cosf(decl_max);
_state.mag_I(1) = h_field * sinf(decl_max);
} else if (decl_estimate < decl_min) {
_state.mag_I(0) = h_field * cosf(decl_min);
_state.mag_I(1) = h_field * sinf(decl_min);
}
if ((mag_I_before - _state.mag_I).longerThan(0.01f)) {
ECL_DEBUG("declination limited mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f] (decl: %.3f)",
(double)mag_I_before(0), (double)mag_I_before(1), (double)mag_I_before(2),
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)decl_reference
);
}
}
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
{
// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
src/modules/ekf2/EKF/ekf.h
-2
View File
@@ -763,8 +763,6 @@ private:
// argument passed in is the declination uncertainty in radians
bool fuseDeclination(float decl_sigma);
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_AIRSPEED)