From 6f5d0fa3f0eb485cfc3a4124a499504d5b16bf9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 6 Jul 2019 08:49:00 +1000 Subject: [PATCH] AP_NavEKF2: added EK2_MAG_EF_LIM parameter this sets a limit on the difference between the earth field from the WMM tables and the learned earth field inside the EKF. Setting it to zero disables the feature. A positive value sets the limit in mGauss. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 8 ++++++ libraries/AP_NavEKF2/AP_NavEKF2.h | 1 + libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 12 ++++---- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 28 +++++++++++++++++++ libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 22 +++++++++++++-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 8 ++++++ 6 files changed, 72 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 138998d0c8..4c5b309487 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -552,6 +552,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @RebootRequired: True AP_GROUPINFO("OGN_HGT_MASK", 49, NavEKF2, _originHgtMode, 0), + // @Param: MAG_EF_LIM + // @DisplayName: EarthField error limit + // @Description: This limits the difference between the learned earth magnetic field and the earth field from the world magnetic model tables. A value of zero means to disable the use of the WMM tables. + // @User: Advanced + // @Range: 0 500 + // @Units: mGauss + AP_GROUPINFO("MAG_EF_LIM", 52, NavEKF2, _mag_ef_limit, 0), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 9f6b2d1b5e..338fc90439 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -396,6 +396,7 @@ private: AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s) AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion. AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height. + AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field. // Tuning parameters const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 9c5d3f9319..227ecfd893 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -269,7 +269,9 @@ void NavEKF2_core::SelectMagFusion() } else { // if we are not doing aiding with earth relative observations (eg GPS) then the declination is // maintained by fusing declination as a synthesised observation - if (PV_AidingMode != AID_ABSOLUTE) { + // We also fuse declination if we are using the WMM tables + if (PV_AidingMode != AID_ABSOLUTE || + (frontend->_mag_ef_limit > 0 && have_table_earth_field)) { FuseDeclination(0.34f); } // fuse the three magnetometer componenents sequentially @@ -813,7 +815,7 @@ void NavEKF2_core::fuseEulerYaw() // Use measured mag components rotated into earth frame to measure yaw Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z); @@ -859,7 +861,7 @@ void NavEKF2_core::fuseEulerYaw() // Use measured mag components rotated into earth frame to measure yaw Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data euler312 = extNavDataDelayed.quat.to_vector312(); @@ -1045,7 +1047,7 @@ void NavEKF2_core::FuseDeclination(float declErr) } // get the magnetic declination - float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; + float magDecAng = MagDeclination(); // Calculate the innovation float innovation = atan2f(magE , magN) - magDecAng; @@ -1129,7 +1131,7 @@ void NavEKF2_core::alignMagStateDeclination() } // get the magnetic declination - float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; + float magDecAng = MagDeclination(); // rotate the NE values so that the declination matches the published value Vector3f initMagNED = stateStruct.earth_magfield; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 501ff3261a..20c737bdfe 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -553,6 +553,17 @@ void NavEKF2_core::readGpsData() } + if (gpsGoodToAlign && !have_table_earth_field) { + table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc); + table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, + gpsloc.lng*1.0e-7)); + have_table_earth_field = true; + if (frontend->_mag_ef_limit > 0) { + // initialise earth field from tables + stateStruct.earth_magfield = table_earth_field_ga; + } + } + // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin if (validOrigin) { gpsDataNew.pos = location_diff(EKF_origin, gpsloc); @@ -871,6 +882,23 @@ void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &p } +/* + return declination in radians +*/ +float NavEKF2_core::MagDeclination(void) const +{ + // if we are using the WMM tables then use the table declination + // to ensure consistency with the table mag field. Otherwise use + // the declination from the compass library + if (have_table_earth_field && frontend->_mag_ef_limit > 0) { + return table_declination; + } + if (!use_compass()) { + return 0; + } + return _ahrs->get_compass()->get_declination(); +} + /* update estimates of inactive bias states. This keeps inactive IMUs as hot-spares so we can switch to them without causing a jump in the diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 839921ebdd..96cb1a3f9d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -339,6 +339,7 @@ void NavEKF2_core::InitialiseVariables() storedOutput.reset(); storedRangeBeacon.reset(); storedExtNav.reset(); + have_table_earth_field = false; } // Initialise the states from accelerometer and magnetometer data (if present) @@ -1434,8 +1435,25 @@ void NavEKF2_core::ConstrainStates() for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f); // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*dtEkfAvg); + // earth magnetic field limit - for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); + if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) { + // constrain to +/-1Ga + for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); + } else { + // constrain to error from table earth field + float limit_ga = frontend->_mag_ef_limit * 0.001f; + stateStruct.earth_magfield.x = constrain_float(stateStruct.earth_magfield.x, + table_earth_field_ga.x-limit_ga, + table_earth_field_ga.x+limit_ga); + stateStruct.earth_magfield.y = constrain_float(stateStruct.earth_magfield.y, + table_earth_field_ga.y-limit_ga, + table_earth_field_ga.y+limit_ga); + stateStruct.earth_magfield.z = constrain_float(stateStruct.earth_magfield.z, + table_earth_field_ga.z-limit_ga, + table_earth_field_ga.z+limit_ga); + } + // body magnetic field limit for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f); // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit @@ -1479,7 +1497,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) float magHeading = atan2f(initMagNED.y, initMagNED.x); // get the magnetic declination - float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; + float magDecAng = MagDeclination(); // calculate yaw angle rel to true north yaw = magDecAng - magHeading; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 162f9a2a8f..8b7d0cd71f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -734,6 +734,9 @@ private: // Input is 1-sigma uncertainty in published declination void FuseDeclination(float declErr); + // return magnetic declination in radians + float MagDeclination(void) const; + // Propagate PVA solution forward from the fusion time horizon to the current time horizon // using a simple observer void calcOutputStates(); @@ -1181,6 +1184,11 @@ private: AP_HAL::Util::perf_counter_t _perf_FuseOptFlow; AP_HAL::Util::perf_counter_t _perf_test[10]; + // earth field from WMM tables + bool have_table_earth_field; // true when we have initialised table_earth_field_ga + Vector3f table_earth_field_ga; // earth field from WMM tables + float table_declination; // declination in radians from the tables + // timing statistics struct ekf_timing timing;