From 36e7dfb5f66542caeff2ae75bd7b8b32de0f222c Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 9 May 2017 16:30:58 +1000 Subject: [PATCH] AP_NavEKF2: Improve GPS reference height estimator 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. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 7 +++++ libraries/AP_NavEKF2/AP_NavEKF2.h | 1 + libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 ++ .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 28 ++++++++----------- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 6 +++- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 13 ++++++--- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 1 + libraries/AP_NavEKF2/AP_NavEKF2_core.h | 7 +++-- 8 files changed, 40 insertions(+), 25 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index e854804c67..92a2944b6a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -535,6 +535,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @User: Advanced AP_GROUPINFO("MAG_MASK", 48, NavEKF2, _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", 49, NavEKF2, _originHgtMode, 7), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 70e6a73255..1d0630ba29 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -374,6 +374,7 @@ private: AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec) 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. // Tuning parameters const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 997c5f94c2..858b4a9579 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -409,6 +409,7 @@ bool NavEKF2_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; @@ -420,6 +421,7 @@ void NavEKF2_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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 49976723a7..9eaf8018d4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -499,8 +499,8 @@ void NavEKF2_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 uncertinty of the GPS origin height ekfOriginHgtVar = sq(gpsHgtAccuracy); @@ -510,7 +510,7 @@ void NavEKF2_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; @@ -585,10 +585,8 @@ void NavEKF2_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 NavEKF2_core::calcFiltGpsHgtOffset() +// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter. +void NavEKF2_core::correctEkfOriginHeight() { // Estimate the WGS-84 height of the EKF's origin using a Bayes filter @@ -602,9 +600,8 @@ void NavEKF2_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; @@ -622,10 +619,10 @@ void NavEKF2_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); - ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f); + // 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); } } @@ -742,9 +739,6 @@ void NavEKF2_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); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 338d4b0b64..3a7d730827 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -308,7 +308,7 @@ bool NavEKF2_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; @@ -370,6 +370,10 @@ bool NavEKF2_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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 21da89e684..ffdca9d46b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -208,7 +208,7 @@ bool NavEKF2_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; @@ -822,9 +822,14 @@ void NavEKF2_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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index adceb79672..83f7d34364 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -274,6 +274,7 @@ void NavEKF2_core::InitialiseVariables() memset(&storedRngMeas, 0, sizeof(storedRngMeas)); terrainHgtStable = true; ekfOriginHgtVar = 0.0f; + ekfGpsRefHgt = 0.0; velOffsetNED.zero(); posOffsetNED.zero(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 47ee53870b..eaca8b7cc4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -691,8 +691,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(); @@ -805,7 +805,7 @@ private: bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant bool gpsNotAvailable; // bool true when valid GPS data is not available uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset - 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 @@ -885,6 +885,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)