From 3accab1ac531eaf4f06bc861a0246872752f2834 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 28 Jan 2019 16:33:13 +1100 Subject: [PATCH] EKF: protect declination fusion from badly conditioned earth field estimates Fusion with large initial magnetometer biases errors can result in the the NE earth field states reducing in magnitude and effectively flipping sign. EKF: Move declination state limiting into a separate function EKF: Limit NE mag states after each 3-axis mag fusion EKF: Fix bug in mag field strength look-up scale factor --- EKF/ekf.h | 3 ++ EKF/estimator_interface.h | 2 + EKF/gps_checks.cpp | 6 ++- EKF/mag_fusion.cpp | 60 +++++++++++++++++++++++++++++- geo_lookup/geo_mag_declination.cpp | 2 +- geo_lookup/geo_mag_declination.h | 2 +- 6 files changed, 71 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 3de91dc6ba..93f3507a7f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -494,6 +494,9 @@ private: // argument passed in is the declination uncertainty in radians void fuseDeclination(float decl_sigma); + // apply sensible limits to the declination and length of the NE mag field states estimates + void limitDeclination(); + // fuse airspeed measurement void fuseAirspeed(); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index e7e26c103c..e37755886e 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -552,6 +552,8 @@ protected: float _mag_declination_gps{0.0f}; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_declination_to_save_deg{0.0f}; // magnetic declination to save to EKF2_MAG_DECL (deg) + float _mag_inclination_gps{0.0f}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) + float _mag_strength_gps{0.0f}; // magnetic strength returned by the geo library using the last valid GPS position (T) // this is the current status of the filter control modes filter_control_status_u _control_status{}; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index b46096392f..7adc9bc9a4 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -77,8 +77,12 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); _NED_origin_initialised = true; _last_gps_origin_time_us = _time_last_imu; - // set the magnetic declination returned by the geo library using the current GPS position + + // set the magnetic field data returned by the geo library using the current GPS position _mag_declination_gps = math::radians(get_mag_declination(lat, lon)); + _mag_inclination_gps = math::radians(get_mag_inclination(lat, lon)); + _mag_strength_gps = 0.01f * get_mag_strength(lat, lon); + // request a reset of the yaw using the new declination _mag_yaw_reset_req = true; // save the horizontal and vertical position uncertainty of the origin diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index cfd33122ce..0c1e43bffc 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -419,6 +419,8 @@ void Ekf::fuseMag() // apply the state corrections fuse(Kfusion, _mag_innov[index]); + // constrain the declination of the earth field states + limitDeclination(); } } } @@ -797,6 +799,10 @@ void Ekf::fuseDeclination(float decl_sigma) float magN = _state.mag_I(0); float magE = _state.mag_I(1); + // minimum horizontal field strength before calculation becomes badly conditioned (T) + float h_field_min = 0.001f; + + // observation variance (rad**2) float R_DECL = sq(decl_sigma); // Calculate intermediate variables @@ -804,7 +810,7 @@ void Ekf::fuseDeclination(float decl_sigma) float t3 = magN*magN; float t4 = t2+t3; // if the horizontal magnetic field is too small, this calculation will be badly conditioned - if (t4 < 1e-4f) { + if (t4 < h_field_min*h_field_min) { return; } float t5 = P[16][16]*t2; @@ -922,5 +928,57 @@ void Ekf::fuseDeclination(float decl_sigma) // apply the state corrections fuse(Kfusion, innovation); + // constrain the declination of the earth field states + limitDeclination(); + } +} + +void Ekf::limitDeclination() +{ + // get a reference value for the earth field declinaton and minimum plausible horizontal field strength + // set to 50% of the horizontal strength from geo tables if location is known + float decl_reference; + float h_field_min = 0.001f; + if (_params.mag_declination_source & MASK_USE_GEO_DECL) { + // use parameter value until GPS is available, then use value returned by geo library + if (_NED_origin_initialised) { + decl_reference = _mag_declination_gps; + h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); + } else { + decl_reference = math::radians(_params.mag_declination_deg); + } + } else { + // always use the parameter value + decl_reference = math::radians(_params.mag_declination_deg); + } + + // 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) { + 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(_mag_declination); + _state.mag_I(1) = 2.0f * h_field_min * sinf(_mag_declination); + } + h_field = h_field_min; + } + + // do not allow the declination estimate to vary too much relative to the reference value + const 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 = atan2(_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); } } diff --git a/geo_lookup/geo_mag_declination.cpp b/geo_lookup/geo_mag_declination.cpp index 8a80b4b0d4..6055809267 100644 --- a/geo_lookup/geo_mag_declination.cpp +++ b/geo_lookup/geo_mag_declination.cpp @@ -93,7 +93,7 @@ static constexpr const int8_t inclination_table[13][37] = \ { 71,71,72,73,75,77,78,80,81,81,80,79,77,76,74,73,73,73,73,73,73,74,74,75,76,77,78,78,78,78,77,75,73,72,71,71,71 }, }; -// strength data in mTesla +// strength data in centi-Tesla static constexpr const int8_t strength_table[13][37] = \ { { 62,60,58,56,54,52,49,46,43,41,38,36,34,32,31,31,30,30,30,31,33,35,38,42,46,51,55,59,62,64,66,67,67,66,65,64,62 }, diff --git a/geo_lookup/geo_mag_declination.h b/geo_lookup/geo_mag_declination.h index a48f5e4397..e26a8416a0 100644 --- a/geo_lookup/geo_mag_declination.h +++ b/geo_lookup/geo_mag_declination.h @@ -47,5 +47,5 @@ float get_mag_declination(float lat, float lon); // Return magnetic field inclination in degrees float get_mag_inclination(float lat, float lon); -// return magnetic field strength in mTesla +// return magnetic field strength in centi-Tesla float get_mag_strength(float lat, float lon);