From bbcf741e9ea8743458df8be9a9317e2913f5b695 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 31 May 2024 15:37:05 -0400 Subject: [PATCH] ekf2: make mag control responsible for WMM - this further untangles mag control (which requires the WMM) from GPS --- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 20 ++- .../aid_sources/magnetometer/mag_control.cpp | 134 ++++++++++++++---- .../aid_sources/magnetometer/mag_fusion.cpp | 57 +++----- src/modules/ekf2/EKF/ekf.h | 8 +- src/modules/ekf2/EKF/ekf_helper.cpp | 37 +---- src/modules/ekf2/EKF/estimator_interface.h | 21 +-- src/modules/ekf2/test/test_EKF_airspeed.cpp | 2 +- src/modules/ekf2/test/test_EKF_basics.cpp | 1 - 8 files changed, 157 insertions(+), 123 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 94fa6f04f3..4467ff9f7e 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -41,10 +41,6 @@ #include "ekf.h" -#if defined(CONFIG_EKF2_MAGNETOMETER) -# include -#endif // CONFIG_EKF2_MAGNETOMETER - #include // GPS pre-flight check bit locations @@ -89,20 +85,20 @@ void Ekf::collect_gps(const gnssSample &gps) _gpos_origin_eph = gps.hacc; _gpos_origin_epv = gps.vacc; - _information_events.flags.gps_checks_passed = true; - ECL_INFO("GPS checks passed"); - } + _earth_rate_NED = calcEarthRateNED(math::radians(static_cast(gps.lat))); - if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) { - // a rough 2D fix is sufficient to lookup declination + _information_events.flags.gps_checks_passed = true; + + ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f", + _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon()); + + } else { + // a rough 2D fix is sufficient to lookup earth spin rate const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) { - updateWmm(gps.lat, gps.lon); _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } - - _wmm_gps_time_last_checked = _time_delayed_us; } } diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 0e42006b24..f36eba1dc6 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -39,6 +39,8 @@ #include "ekf.h" #include +#include + #include void Ekf::controlMagFusion(const imuSample &imu_sample) @@ -76,15 +78,43 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) _mag_counter++; } + // check for WMM update periodically or if global origin has changed + bool wmm_updated = false; + + if (global_origin().isInitialized()) { + + bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse); + + if (global_origin_valid() + && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) + ) { + // position of local NED origin in GPS / WGS84 frame + double latitude_deg; + double longitude_deg; + global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg); + + if (updateWorldMagneticModel(latitude_deg, longitude_deg)) { + wmm_updated = true; + } + + _wmm_mag_time_last_checked = _time_delayed_us; + + } else if (origin_newer_than_last_mag) { + // use global origin to update WMM + if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(), + global_origin().getProjectionReferenceLon()) + ) { + wmm_updated = true; + } + } + } + // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // this is useful if there is a lot of interference on the sensor measurement. if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) - && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) + && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f)) ) { - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) - * Vector3f(_mag_strength_gps, 0, 0); - - mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred); + mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss); _control_status.flags.synthetic_mag_z = true; @@ -140,8 +170,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) checkMagHeadingConsistency(mag_sample); - // 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; @@ -203,7 +231,21 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) } if (_control_status.flags.mag_dec) { - fuseDeclination(0.5f); + + if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) + && PX4_ISFINITE(_wmm_declination_rad) + ) { + fuseDeclination(_wmm_declination_rad, 0.5f); + + } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) + && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) + ) { + + fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f); + + } else { + _control_status.flags.mag_dec = false; + } } } @@ -333,24 +375,21 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) resetMagCov(); // if world magnetic model (inclination, declination, strength) available then use it to reset mag states - if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { - + if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) { // use expected earth field to reset states - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) - * Vector3f(_mag_strength_gps, 0, 0); // mag_B: reset if (!reset_heading && _control_status.flags.yaw_align) { // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); - _state.mag_B = mag - (R_to_body * mag_earth_pred); + _state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss); } else { _state.mag_B.zero(); } // mag_I: reset, skipped if no change in state and variance good - _state.mag_I = mag_earth_pred; + _state.mag_I = _wmm_earth_field_gauss; if (reset_heading) { resetMagHeading(mag); @@ -451,8 +490,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample) _mag_strength = mag_sample.length(); if (_params.mag_check & static_cast(MagCheckMask::STRENGTH)) { - if (PX4_ISFINITE(_mag_strength_gps)) { - if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) { + if (PX4_ISFINITE(_wmm_field_strength_gauss)) { + if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) { _control_status.flags.mag_field_disturbed = true; is_check_failing = true; } @@ -475,9 +514,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample) _mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); if (_params.mag_check & static_cast(MagCheckMask::INCLINATION)) { - if (PX4_ISFINITE(_mag_inclination_gps)) { + if (PX4_ISFINITE(_wmm_inclination_rad)) { const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg); - const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps); + const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad); if (fabsf(inc_error_rad) > inc_tol_rad) { _control_status.flags.mag_field_disturbed = true; @@ -549,13 +588,60 @@ float Ekf::getMagDeclination() // Use value consistent with earth field state return atan2f(_state.mag_I(1), _state.mag_I(0)); - } else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) { - // use parameter value until GPS is available, then use value returned by geo library - if (PX4_ISFINITE(_mag_declination_gps)) { - return _mag_declination_gps; + } else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) + && PX4_ISFINITE(_wmm_declination_rad) + ) { + // if available use value returned by geo library + return _wmm_declination_rad; + + } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) + && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) + ) { + // using saved mag declination + return math::radians(_params.mag_declination_deg); + } + + // otherwise unavailable + return 0.f; +} + +bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg) +{ + // set the magnetic field data returned by the geo library using the current GPS position + const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg)); + const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg); + + if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) { + + const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f)); + const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f)); + const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f); + + if (!PX4_ISFINITE(_wmm_declination_rad) + || !PX4_ISFINITE(_wmm_inclination_rad) + || !PX4_ISFINITE(_wmm_field_strength_gauss) + || !_wmm_earth_field_gauss.longerThan(0.f) + || !_wmm_earth_field_gauss.isAllFinite() + || declination_changed + || inclination_changed + || strength_changed + ) { + + ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)", + (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad), + (double)latitude_deg, (double)longitude_deg + ); + + _wmm_declination_rad = declination_rad; + _wmm_inclination_rad = inclination_rad; + _wmm_field_strength_gauss = strength_gauss; + + _wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0); + + return true; } } - // otherwise use the parameter value - return math::radians(_params.mag_declination_deg); + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index acd16578c5..887caf20dd 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -150,51 +150,34 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima return false; } -bool Ekf::fuseDeclination(float decl_sigma) +bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma) { - float decl_measurement = NAN; + // observation variance (rad**2) + const float R_DECL = sq(decl_sigma); - if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) - && PX4_ISFINITE(_mag_declination_gps) - ) { - decl_measurement = _mag_declination_gps; + VectorState H; + float decl_pred; + float innovation_variance; - } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) - && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) - ) { - decl_measurement = math::radians(_params.mag_declination_deg); + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, + &decl_pred, &innovation_variance, &H); + + const float innovation = wrap_pi(decl_pred - decl_measurement_rad); + + if (innovation_variance < R_DECL) { + // variance calculation is badly conditioned + _fault_status.flags.bad_mag_decl = true; + return false; } - if (PX4_ISFINITE(decl_measurement)) { + // Calculate the Kalman gains + VectorState Kfusion = P * H / innovation_variance; - // observation variance (rad**2) - const float R_DECL = sq(decl_sigma); + const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); - VectorState H; - float decl_pred; - float innovation_variance; + _fault_status.flags.bad_mag_decl = !is_fused; - sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, - &H); - - const float innovation = wrap_pi(decl_pred - decl_measurement); - - if (innovation_variance < R_DECL) { - // variance calculation is badly conditioned - return false; - } - - // Calculate the Kalman gains - VectorState Kfusion = P * H / innovation_variance; - - const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); - - _fault_status.flags.bad_mag_decl = !is_fused; - - return is_fused; - } - - return false; + return is_fused; } float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e6c182171a..edf50329bb 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -259,7 +259,6 @@ public: // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); - void updateWmm(double lat, double lon); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -466,6 +465,9 @@ public: #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) + // set the magnetic field data returned by the geo library using position + bool updateWorldMagneticModel(const double latitude_deg, const double longitude_deg); + const auto &aid_src_mag() const { return _aid_src_mag; } #endif // CONFIG_EKF2_MAGNETOMETER @@ -770,8 +772,8 @@ private: bool update_all_states = false, bool update_tilt = false); // fuse magnetometer declination measurement - // argument passed in is the declination uncertainty in radians - bool fuseDeclination(float decl_sigma); + // declination uncertainty in radians + bool fuseDeclination(float decl_measurement_rad, float decl_sigma); #endif // CONFIG_EKF2_MAGNETOMETER diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9df8fe1da2..01821fd97b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -96,13 +96,13 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; - updateWmm(current_lat, current_lon); - _gpos_origin_eph = eph; _gpos_origin_epv = epv; _NED_origin_initialised = true; + _earth_rate_NED = calcEarthRateNED(math::radians(static_cast(latitude))); + if (current_pos_available) { // reset horizontal position if we already have a global origin Vector2f position = _pos_ref.project(current_lat, current_lon); @@ -131,39 +131,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons return false; } -void Ekf::updateWmm(const double lat, const double lon) -{ -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // set the magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon)); - const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon)); - const float mag_strength_gps = get_mag_strength_gauss(lat, lon); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - - const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); - const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); - - if ((_wmm_gps_time_last_set == 0) - || !PX4_ISFINITE(_mag_declination_gps) - || !PX4_ISFINITE(_mag_inclination_gps) - || !PX4_ISFINITE(_mag_strength_gps) - || mag_declination_changed - || mag_inclination_changed - ) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - } - -#endif // CONFIG_EKF2_MAGNETOMETER -} - - void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { float eph = INFINITY; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 57e6fc06b0..302d113f8d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -250,7 +250,7 @@ public: bool get_mag_decl_deg(float &val) const { if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { - val = math::degrees(_mag_declination_gps); + val = math::degrees(_wmm_declination_rad); return true; } else { @@ -261,7 +261,7 @@ public: bool get_mag_inc_deg(float &val) const { if (_NED_origin_initialised) { - val = math::degrees(_mag_inclination_gps); + val = math::degrees(_wmm_inclination_rad); return true; } else { @@ -272,9 +272,9 @@ public: void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const { inc_deg = math::degrees(_mag_inclination); - inc_ref_deg = math::degrees(_mag_inclination_gps); + inc_ref_deg = math::degrees(_wmm_inclination_rad); strength_gs = _mag_strength; - strength_ref_gs = _mag_strength_gps; + strength_ref_gs = _wmm_field_strength_gauss; } #endif // CONFIG_EKF2_MAGNETOMETER @@ -451,13 +451,14 @@ protected: // allocate data buffers and initialize interface variables bool initialise_interface(uint64_t timestamp); - uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked - uint64_t _wmm_gps_time_last_set{0}; // time WMM last set - #if defined(CONFIG_EKF2_MAGNETOMETER) - float _mag_declination_gps {NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) - float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) - float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) + uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control + + float _wmm_declination_rad{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) + float _wmm_inclination_rad{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) + float _wmm_field_strength_gauss{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (Gauss) + + Vector3f _wmm_earth_field_gauss{}; // expected magnetic field vector from the last valid GPS position (Gauss) float _mag_inclination{NAN}; float _mag_strength{NAN}; diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 61865dfc7a..d1bd54a13b 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -199,7 +199,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); const Vector3f vel = _ekf->getVelocity(); - EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f); + EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f); const Vector2f vel_wind_earth = _ekf->getWindVelocity(); EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index e954433442..0d34e676cb 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -108,7 +108,6 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag); - EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec); EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);