EKF: Miscellaneous updates for EKF2 and EKF3

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -380,15 +380,15 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
         return AP_AHRS_DCM::get_position(loc);
 
     case EKF_TYPE2:
-        if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(origin)) {
+        if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(-1,origin)) {
             // fixup altitude using relative position from EKF origin
             loc.alt = origin.alt - ned_pos.z*100;
             return true;
         }
         break;
 
     case EKF_TYPE3:
-        if (EKF3.getLLH(loc) && EKF3.getPosD(-1,ned_pos.z) && EKF3.getOriginLLH(origin)) {
+        if (EKF3.getLLH(loc) && EKF3.getPosD(-1,ned_pos.z) && EKF3.getOriginLLH(-1,origin)) {
             // fixup altitude using relative position from EKF origin
             loc.alt = origin.alt - ned_pos.z*100;
             return true;
@@ -1406,13 +1406,13 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const
 
     case EKF_TYPE2:
     default:
-        if (!EKF2.getOriginLLH(ret)) {
+        if (!EKF2.getOriginLLH(-1,ret)) {
             return false;
         }
         return true;
 
     case EKF_TYPE3:
-        if (!EKF3.getOriginLLH(ret)) {
+        if (!EKF3.getOriginLLH(-1,ret)) {
             return false;
         }
         return true;

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -123,6 +123,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Description: This enables EKF2. Enabling EKF2 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=2. A reboot or restart will need to be performed after changing the value of EK2_ENABLE for it to take effect.
     // @Values: 0:Disabled, 1:Enabled
     // @User: Advanced
+    // @RebootRequired: True
     AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 1, AP_PARAM_FLAG_ENABLE),
 
     // GPS measurement parameters
@@ -193,6 +194,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Increment: 10
     // @User: Advanced
     // @Units: ms
+    // @RebootRequired: True
     AP_GROUPINFO("GPS_DELAY", 8, NavEKF2, _gpsDelay_ms, 220),
 
     // Height measurement parameters
@@ -228,6 +230,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Increment: 10
     // @User: Advanced
     // @Units: ms
+    // @RebootRequired: True
     AP_GROUPINFO("HGT_DELAY", 12, NavEKF2, _hgtDelay_ms, 60),
 
     // Magnetometer measurement parameters
@@ -329,6 +332,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Increment: 10
     // @User: Advanced
     // @Units: ms
+    // @RebootRequired: True
     AP_GROUPINFO("FLOW_DELAY", 23, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
 
     // State and Covariance Predition Parameters
@@ -406,6 +410,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF2 will fail to start.
     // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
     // @User: Advanced
+    // @RebootRequired: True
     AP_GROUPINFO("IMU_MASK",     33, NavEKF2, _imuMask, 3),
 
     // @Param: CHECK_SCALE
@@ -429,6 +434,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Description: This sets the IMU mask of sensors to do full logging for
     // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
     // @User: Advanced
+    // @RebootRequired: True
     AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),
 
     // control of magentic yaw angle fusion
@@ -516,6 +522,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Increment: 10
     // @User: Advanced
     // @Units: ms
+    // @RebootRequired: True
     AP_GROUPINFO("BCN_DELAY", 46, NavEKF2, _rngBcnDelay_ms, 50),
 
     // @Param: RNG_USE_SPD
@@ -532,8 +539,20 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
     // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
     // @User: Advanced
+    // @RebootRequired: True
     AP_GROUPINFO("MAG_MASK", 48, NavEKF2, _magMask, 0),
 
+    // @Param: OGN_HGT_MASK
+    // @DisplayName: Bitmask control of EKF reference height correction
+    // @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 corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time.
+    // The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits.
+    // The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position (default) to the reported EKF origin height.
+    // @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to origin height
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("OGN_HGT_MASK", 49, NavEKF2, _originHgtMode, 0),
+
     AP_GROUPEND
 };
 
@@ -990,15 +1009,17 @@ bool NavEKF2::getLLH(struct Location &loc) const
     return core[primary].getLLH(loc);
 }
 
-// return the latitude and longitude and height used to set the NED origin
+// Return the latitude and longitude and height used to set the NED origin for the specified instance
+// An out of range instance (eg -1) returns data for the the primary instance
 // All NED positions calculated by the filter are relative to this location
 // Returns false if the origin has not been set
-bool NavEKF2::getOriginLLH(struct Location &loc) const
+bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const
 {
+    if (instance < 0 || instance >= num_cores) instance = primary;
     if (!core) {
         return false;
     }
-    return core[primary].getOriginLLH(loc);
+    return core[instance].getOriginLLH(loc);
 }
 
 // set the latitude and longitude and height used to set the NED origin

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -156,10 +156,11 @@ class NavEKF2
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(struct Location &loc) const;
 
-    // return the latitude and longitude and height used to set the NED origin
+    // Return the latitude and longitude and height used to set the NED origin for the specified instance
+    // An out of range instance (eg -1) returns data for the the primary instance
     // All NED positions calculated by the filter are relative to this location
     // Returns false if the origin has not been set
-    bool getOriginLLH(struct Location &loc) const;
+    bool getOriginLLH(int8_t instance, struct Location &loc) const;
 
     // set the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter will be relative to this location
@@ -374,6 +375,7 @@ class NavEKF2
     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

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;

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

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -279,18 +279,27 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
     return false;
 }
 
-// Write the last calculated D position of the body frame origin relative to the reference point (m).
+// Write the last calculated D position of the body frame origin relative to the EKF origin (m).
 // Return true if the estimate is valid
 bool NavEKF2_core::getPosD(float &posD) const
 {
     // The EKF always has a height estimate regardless of mode of operation
-    // correct for the IMU offset (EKF calculations are at the IMU)
-    posD = outputDataNew.position.z + posOffsetNED.z;
+    // Correct for the IMU offset (EKF calculations are at the IMU)
+    // Correct for
+    if (frontend->_originHgtMode & (1<<2)) {
+        // Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
+        posD = outputDataNew.position.z + posOffsetNED.z;
+    } else {
+        // The origin height is static and corrections are applied to the local vertical position
+        // so that height returned by getLLH() = height returned by getOriginLLH - posD
+        posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
+    }
 
     // Return the current height solution status
     return filterStatus.flags.vert_pos;
 
 }
+
 // return the estimated height of body frame origin above ground level
 bool NavEKF2_core::getHAGL(float &HAGL) const
 {
@@ -308,7 +317,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 +379,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;
 }

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

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

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -691,8 +691,8 @@ class NavEKF2_core
     // 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 @@ class NavEKF2_core
     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 @@ class NavEKF2_core
     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)