From fc5e1362a9cfded73efcb82dee51d67cfef56b28 Mon Sep 17 00:00:00 2001 From: Jaaaky <43599380+Jaaaky@users.noreply.github.com> Date: Sun, 22 Sep 2019 15:28:19 +0300 Subject: [PATCH] AP_NavEKF3: added EK3_MAG_EF_LIM parameter Adapted from EKF2 implementation as of commits 3835d2613, e9ed3540f and df4fc0fff 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_NavEKF3/AP_NavEKF3.cpp | 8 +++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 16 ++++++--- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 28 ++++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 33 +++++++++++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 11 +++++++ 6 files changed, 90 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 71cf2f515e..c3033f171b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -596,6 +596,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @RebootRequired: False AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f), + // @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", 56, NavEKF3, _mag_ef_limit, 50), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 7be4bc8ccd..35de0716f9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -448,6 +448,7 @@ private: AP_Float _wencOdmVelErr; // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s) AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator. AP_Float _hrt_filt_freq; // frequency of output observer height rate complementary filter in Hz + AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field. // Possible values for _flowUse #define FLOW_USE_NONE 0 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 9651b3b6e3..245894c9ea 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -310,7 +310,9 @@ void NavEKF3_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 @@ -782,6 +784,12 @@ void NavEKF3_core::FuseMagnetometer() for (uint8_t j= 0; j<=stateIndexLim; j++) { statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex]; } + + // add table constraint here for faster convergence + if (have_table_earth_field && frontend->_mag_ef_limit > 0) { + MagTableConstrain(); + } + stateStruct.quat.normalize(); } else { @@ -937,7 +945,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) // Use the difference between the horizontal projection and declination to give the measured yaw // rotate measured mag components into earth frame Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); innovation = wrap_PI(yawAngPredicted - yawAngMeasured); } else { // use the external yaw sensor data @@ -1170,7 +1178,7 @@ void NavEKF3_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; @@ -1248,7 +1256,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index b358e0306e..17bdcff00b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -606,6 +606,17 @@ void NavEKF3_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 = EKF_origin.get_distance_NE(gpsloc); @@ -1007,3 +1018,20 @@ void NavEKF3_core::learnInactiveBiases(void) } } } + +/* + return declination in radians +*/ +float NavEKF3_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(); +} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 30acc09497..cf556e0761 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -252,6 +252,7 @@ void NavEKF3_core::InitialiseVariables() lastYawReset_ms = 0; tiltAlignComplete = false; yawAlignComplete = false; + have_table_earth_field = false; stateIndexLim = 23; baroStoreIndex = 0; rangeStoreIndex = 0; @@ -1539,6 +1540,22 @@ void NavEKF3_core::ConstrainVariances() } } +// constrain states using WMM tables and specified limit +void NavEKF3_core::MagTableConstrain(void) +{ + // 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); +} + // constrain states to prevent ill-conditioning void NavEKF3_core::ConstrainStates() { @@ -1555,7 +1572,13 @@ void NavEKF3_core::ConstrainStates() // the accelerometer bias limit is controlled by a user adjustable parameter for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*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 { + // use table constrain + MagTableConstrain(); + } // 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 @@ -1599,7 +1622,7 @@ Quaternion NavEKF3_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; @@ -1624,7 +1647,11 @@ Quaternion NavEKF3_core::calcQuatAndFieldStates(float roll, float pitch) // don't do this if the earth field has already been learned if (!magFieldLearned) { initQuat.rotation_matrix(Tbn); - stateStruct.earth_magfield = Tbn * magDataDelayed.mag; + if (have_table_earth_field && frontend->_mag_ef_limit > 0) { + stateStruct.earth_magfield = table_earth_field_ga; + } else { + stateStruct.earth_magfield = Tbn * magDataDelayed.mag; + } // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 783110841f..0a09d8f55d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -557,6 +557,9 @@ private: // constrain states void ConstrainStates(); + // constrain earth field using WMM tables + void MagTableConstrain(void); + // fuse selected position, velocity and height measurements void FuseVelPosNED(); @@ -801,6 +804,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(); @@ -1284,6 +1290,11 @@ private: AP_HAL::Util::perf_counter_t _perf_FuseBodyOdom; 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;