Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve GPS reference height estimator

Browse Source 
Fix rounding error bug preventing state from updating after initial convergence.
Decouple GPS reference height from published EKf origin height.
Add bitmask parameter to control update and publishing of GPS reference height.
This commit is contained in:
priseborough 2017-05-09 16:23:58 +10:00 committed by Francisco Ferreira
parent e7a5dd6a69
commit 9da3caca47
8 changed files with 39 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -531,6 +531,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MAG_MASK", 49, NavEKF3, _magMask, 0),
// @Param: OGN_HGT_MASK
// @DisplayName: Bitmask control of EKF origin height adjustment
// @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be adjusted so that the height returned by the getLLH() function is corrected for primary height sensor drift and change in datum over time. This parameter controls when the WGS-84 reference height used by the EKF to convert GPS height to local height will be adjusted. Adjustment is performed using a Bayes filter and only operates when GPS quality permits. The parameter also controls whether the adjustments to the GPS reference datum also update the reported height of the EKF origin.
// @Bitmask: 0:When using Baro hgt,1:When using range height,2:Update Origin
// @User: Advanced
AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 7),
AP_GROUPEND
};

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -390,6 +390,7 @@ private:
AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
AP_Float _accBiasLim; // Accelerometer bias limit (m/s/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.
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -435,6 +435,7 @@ bool NavEKF3_core::setOriginLLH(const Location &loc)
return false;
}
EKF_origin = loc;
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
@ -446,6 +447,7 @@ void NavEKF3_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;

26
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -521,8 +521,8 @@ void NavEKF3_core::readGpsData()
// and set the corresponding variances and covariances
alignMagStateDeclination();
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
// Set the height of the NED origin
ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;
// Set the uncertainty of the GPS origin height
ekfOriginHgtVar = sq(gpsHgtAccuracy);
@ -532,7 +532,7 @@ void NavEKF3_core::readGpsData()
// 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);
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
storedGPS.push(gpsDataNew);
// declare GPS available for use
gpsNotAvailable = false;
@ -607,10 +607,8 @@ void NavEKF3_core::calcFiltBaroOffset()
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
}
// calculate filtered offset between GPS height measurement and EKF height estimate
// offset should be subtracted from GPS measurement to match filter estimate
// offset is used to switch reversion to GPS from alternate height data source
void NavEKF3_core::calcFiltGpsHgtOffset()
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void NavEKF3_core::correctEkfOriginHeight()
{
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
@ -624,9 +622,8 @@ void NavEKF3_core::calcFiltGpsHgtOffset()
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_GPS) {
// by definition we are using GPS height as the EKF datum in this mode
// so cannot run this filter
} else {
// by definition our height source is absolute so cannot run this filter
return;
}
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
@ -644,9 +641,9 @@ void NavEKF3_core::calcFiltGpsHgtOffset()
// check the innovation variance ratio
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
// correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
if (ratio < 5.0f) {
EKF_origin.alt -= (int)(100.0f * gain * innovation);
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if (ratio < 25.0f && gpsAccuracyGood) {
ekfGpsRefHgt -= (double)(gain * innovation);
ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
}
}
@ -764,9 +761,6 @@ void NavEKF3_core::readRngBcnData()
// and set the corresponding variances and covariances
alignMagStateDeclination();
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
EKF_origin.alt = origin_loc.alt - baroDataNew.hgt;
// Set the uncertainty of the origin height
ekfOriginHgtVar = sq(beaconVehiclePosErr);
}

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -306,7 +306,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
{
if(validOrigin) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.alt = EKF_origin.alt - outputDataNew.position.z*100;
loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;
@ -372,6 +372,10 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
{
if (validOrigin) {
loc = EKF_origin;
// report internally corrected reference height if enabled
if (frontend->_originHgtMode & (1<<2)) {
loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
}
}
return validOrigin;
}

13
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -212,7 +212,7 @@ bool NavEKF3_core::resetHeightDatum(void)
stateStruct.position.z = 0.0f;
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
if (validOrigin) {
EKF_origin.alt += oldHgt*100;
ekfGpsRefHgt += (double)oldHgt;
}
// adjust the terrain state
terrainState += oldHgt;
@ -820,9 +820,14 @@ void NavEKF3_core::selectHeightForFusion()
}
}
// calculate offset to GPS height data that enables us to switch to GPS height during operation
if (gpsDataToFuse && (activeHgtSource != HGT_SOURCE_GPS)) {
calcFiltGpsHgtOffset();
// If we are not using GPS as the primary height sensor, correct EKF origin height so that
// combined local NED position height and origin height remains consistent with the GPS altitude
// This also enables the GPS height to be used as a backup height source
if (gpsDataToFuse &&
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
) {
correctEkfOriginHeight();
}
// Select the height measurement source

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -315,6 +315,7 @@ void NavEKF3_core::InitialiseVariables()
memset(&storedRngMeas, 0, sizeof(storedRngMeas));
terrainHgtStable = true;
ekfOriginHgtVar = 0.0f;
ekfGpsRefHgt = 0.0;
velOffsetNED.zero();
posOffsetNED.zero();
posResetSource = DEFAULT;

7
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -743,8 +743,8 @@ private:
// calculate a filtered offset between baro height measurement and EKF height estimate
void calcFiltBaroOffset();
// calculate a filtered offset between GPS height measurement and EKF height estimate
void calcFiltGpsHgtOffset();
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void correctEkfOriginHeight();
// Select height data to be fused from the available baro, range finder and GPS sources
void selectHeightForFusion();
@ -860,7 +860,7 @@ private:
bool inhibitDelVelBiasStates; // true when delta velocity bias states are inactive
bool inhibitDelAngBiasStates;
bool gpsNotAvailable; // bool true when valid GPS data is not available
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
@ -938,6 +938,7 @@ private:
bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs
float ekfOriginHgtVar; // Variance of the the EKF WGS-84 origin height estimate (m^2)
double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)