Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Jaaaky 2019-09-22 15:28:19 +03:00 committed by Andrew Tridgell
parent f735414bf1
commit fc5e1362a9
6 changed files with 90 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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
};

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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

16
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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;

28
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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();
}

33
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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

11
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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;