mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
parent
7206258587
commit
3835d2613e
|
@ -565,6 +565,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
|||
// @RebootRequired: True
|
||||
AP_GROUPINFO("FLOW_USE", 51, NavEKF2, _flowUse, FLOW_USE_DEFAULT),
|
||||
|
||||
// @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
|
||||
};
|
||||
|
||||
|
|
|
@ -402,6 +402,7 @@ private:
|
|||
AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
|
||||
AP_Int8 _extnavDelay_ms; // effective average delay of external nav system measurements relative to inertial measurements (msec)
|
||||
AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
|
||||
AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field.
|
||||
|
||||
// Possible values for _flowUse
|
||||
#define FLOW_USE_NONE 0
|
||||
|
|
|
@ -267,7 +267,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
|
||||
|
@ -811,7 +813,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);
|
||||
|
@ -857,7 +859,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();
|
||||
|
@ -1043,7 +1045,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;
|
||||
|
@ -1127,7 +1129,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;
|
||||
|
|
|
@ -529,6 +529,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 = EKF_origin.get_distance_NE(gpsloc);
|
||||
|
@ -856,3 +867,19 @@ 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();
|
||||
}
|
||||
|
|
|
@ -316,6 +316,7 @@ void NavEKF2_core::InitialiseVariables()
|
|||
|
||||
// now init mag variables
|
||||
yawAlignComplete = false;
|
||||
have_table_earth_field = false;
|
||||
|
||||
InitialiseVariablesMag();
|
||||
}
|
||||
|
@ -1458,8 +1459,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
|
||||
|
@ -1503,7 +1521,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;
|
||||
|
|
|
@ -724,6 +724,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();
|
||||
|
@ -1171,6 +1174,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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue