From 579e4566c60b6a42f95bd879f3c40ae6929d25bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Jul 2021 15:17:11 +1000 Subject: [PATCH] AP_NavEKF3: implement moving origin this shifts EKF_origin to the current location at 1Hz, while leaving public_origin alone. All output APIs and logging are relative to public_origin. The effect of this change is to remove the distortion caused by a spherical earth, which allows the EKF to operate without errors at very long distances from the public_origin. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 9 +++-- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 18 ++------- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 15 +++---- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 26 +++++++++---- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 39 +++++++++++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 13 +++++-- 6 files changed, 79 insertions(+), 41 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 82c7476653..aff6b2ff6e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -611,9 +611,12 @@ bool NavEKF3_core::setOrigin(const Location &loc) validOrigin = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index); - // put origin in frontend as well to ensure it stays in sync between lanes - frontend->common_EKF_origin = EKF_origin; - frontend->common_origin_valid = true; + if (!frontend->common_origin_valid) { + frontend->common_origin_valid = true; + // put origin in frontend as well to ensure it stays in sync between lanes + public_origin = EKF_origin; + } + return true; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 91969e174d..7a3b8c41ce 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -648,16 +648,6 @@ void NavEKF3_core::readGpsData() // Post-alignment checks calcGpsGoodForFlight(); - // see if we can get an origin from the frontend - if (!validOrigin && frontend->common_origin_valid) { - - if (!setOrigin(frontend->common_EKF_origin)) { - // set an error as an attempt was made to set the origin more than once - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return; - } - } - // Read the GPS location in WGS-84 lat,long,height coordinates const struct Location &gpsloc = gps.location(selected_gps); @@ -700,7 +690,8 @@ 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 = EKF_origin.get_distance_NE_ftype(gpsloc); + gpsDataNew.lat = gpsloc.lat; + gpsDataNew.lng = gpsloc.lng; if ((frontend->_originHgtMode & (1<<2)) == 0) { gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); } else { @@ -1463,15 +1454,14 @@ void NavEKF3_core::getEarthFieldTable(const Location &loc) table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7, loc.lng*1.0e-7)); have_table_earth_field = true; - last_field_update_ms = imuSampleTime_ms; } /* - check if we should update the earth field, we update it at 1Hz + update earth field, called at 1Hz */ void NavEKF3_core::checkUpdateEarthField(void) { - if (have_table_earth_field && imuSampleTime_ms - last_field_update_ms > 1000) { + if (have_table_earth_field && filterStatus.flags.using_gps) { Location loc = EKF_origin; loc.offset(stateStruct.position.x, stateStruct.position.y); getEarthFieldTable(loc); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index c2ca8c5a4b..10d8b2a18b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -207,8 +207,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const if (PV_AidingMode != AID_NONE) { // This is the normal mode of operation where we can use the EKF position states // correct for the IMU offset (EKF calculations are at the IMU) - posNE.x = outputDataNew.position.x + posOffsetNED.x; - posNE.y = outputDataNew.position.y + posOffsetNED.y; + posNE = (outputDataNew.position.xy() + posOffsetNED.xy() + public_origin.get_distance_NE_ftype(EKF_origin)).tofloat(); return true; } else { @@ -218,9 +217,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const struct Location &gpsloc = gps.location(selected_gps); - const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc); - posNE.x = tempPosNE.x; - posNE.y = tempPosNE.y; + posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat(); return false; } else if (rngBcnAlignmentStarted) { // If we are attempting alignment using range beacon data, then report the position @@ -229,14 +226,12 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const return false; } else { // If no GPS fix is available, all we can do is provide the last known position - posNE.x = outputDataNew.position.x; - posNE.y = outputDataNew.position.y; + posNE = outputDataNew.position.xy().tofloat(); return false; } } else { // If the origin has not been set, then we have no means of providing a relative position - posNE.x = 0.0f; - posNE.y = 0.0f; + posNE.zero(); return false; } } @@ -357,7 +352,7 @@ void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa bool NavEKF3_core::getOriginLLH(struct Location &loc) const { if (validOrigin) { - loc = EKF_origin; + loc = public_origin; // report internally corrected reference height if enabled if ((frontend->_originHgtMode & (1<<2)) == 0) { loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 1fa8dd990d..b0c4307b64 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -95,9 +95,13 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) CorrectGPSForAntennaOffset(gps_corrected); // record the ID of the GPS for the data we are using for the reset last_gps_idx = gps_corrected.sensor_idx; - // write to state vector and compensate for offset between last GPS measurement and the EKF time horizon - stateStruct.position.x = gps_corrected.pos.x + 0.001*gps_corrected.vel.x*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms)); - stateStruct.position.y = gps_corrected.pos.y + 0.001*gps_corrected.vel.y*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms)); + // calculate position + const Location gpsloc{gps_corrected.lat, gps_corrected.lng, 0, Location::AltFrame::ABSOLUTE}; + stateStruct.position.xy() = EKF_origin.get_distance_NE_ftype(gpsloc); + // compensate for offset between last GPS measurement and the EKF time horizon. Note that this is an unusual + // time delta in that it can be both -ve and +ve + const int32_t tdiff = imuDataDelayed.time_ms - gps_corrected.time_ms; + stateStruct.position.xy() += gps_corrected.vel.xy()*0.001*tdiff; // set the variances using the position measurement noise parameter P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) { @@ -335,8 +339,7 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const gps_data.vel -= velOffsetEarth; Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); - gps_data.pos.x -= posOffsetEarth.x; - gps_data.pos.y -= posOffsetEarth.y; + Location::offset_latlng(gps_data.lat, gps_data.lng, -posOffsetEarth.x, -posOffsetEarth.y); gps_data.hgt += posOffsetEarth.z; } @@ -349,6 +352,9 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data) } ext_nav_data.corrected = true; + // external nav data is against the public_origin, so convert to offset from EKF_origin + ext_nav_data.pos.xy() += EKF_origin.get_distance_NE_ftype(public_origin); + #if HAL_VISUALODOM_ENABLED const auto *visual_odom = dal.visualodom(); if (visual_odom == nullptr) { @@ -477,8 +483,10 @@ void NavEKF3_core::SelectVelPosFusion() velPosObs[1] = gpsDataDelayed.vel.y; velPosObs[2] = gpsDataDelayed.vel.z; } - velPosObs[3] = gpsDataDelayed.pos.x; - velPosObs[4] = gpsDataDelayed.pos.y; + const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE}; + const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc); + velPosObs[3] = posxy.x; + velPosObs[4] = posxy.y; #if EK3_FEATURE_EXTERNAL_NAV } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) { // use external nav system for horizontal position @@ -517,7 +525,9 @@ void NavEKF3_core::SelectVelPosFusion() last_gps_idx = gpsDataDelayed.sensor_idx; // reset the position to the GPS position - ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y); + const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE}; + const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc); + ResetPositionNE(posxy.x, posxy.y); // If we are also using GPS as the height reference, reset the height if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d96d6bfc05..0405046b57 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -10,7 +10,8 @@ // constructor NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) : frontend(_frontend), - dal(AP::dal()) + dal(AP::dal()), + public_origin(frontend->common_EKF_origin) { firstInitTime_ms = 0; lastInitFailReport_ms = 0; @@ -701,8 +702,12 @@ void NavEKF3_core::UpdateFilter(bool predict) // Update the filter status updateFilterStatus(); - // check for update of earth field - checkUpdateEarthField(); + if (imuSampleTime_ms - last_oneHz_ms >= 1000) { + // 1Hz tasks + last_oneHz_ms = imuSampleTime_ms; + moveEKFOrigin(); + checkUpdateEarthField(); + } } // Wind output forward from the fusion to output time horizon @@ -2146,3 +2151,31 @@ void NavEKF3_core::verifyTiltErrorVariance() } } #endif + +/* + move the EKF origin to the current position at 1Hz. The public_origin doesn't move. + By moving the EKF origin we keep the distortion due to spherical + shape of the earth to a minimum. + */ +void NavEKF3_core::moveEKFOrigin(void) +{ + // only move origin when we have a origin and we're using GPS + if (!frontend->common_origin_valid || !filterStatus.flags.using_gps) { + return; + } + + // move the origin to the current state location + Location loc = EKF_origin; + loc.offset(stateStruct.position.x, stateStruct.position.y); + const Vector2F diffNE = loc.get_distance_NE_ftype(EKF_origin); + EKF_origin = loc; + + // now fix all output states + stateStruct.position.xy() += diffNE; + outputDataNew.position.xy() += diffNE; + outputDataDelayed.position.xy() += diffNE; + + for (unsigned index=0; index < imu_buffer_length; index++) { + storedOutput[index].position.xy() += diffNE; + } +} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index bf22be313b..d5be194406 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -499,7 +499,7 @@ private: }; struct gps_elements : EKF_obs_element_t { - Vector2F pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) + int32_t lat, lng; // latitude and longitude in 1e7 degrees ftype hgt; // height of the GPS antenna in local NED earth frame (m) Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec) uint8_t sensor_idx; // unique integer identifying the GPS sensor @@ -1025,7 +1025,8 @@ private: bool needEarthBodyVarReset; // we need to reset mag earth variances at next CovariancePrediction bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive bool gpsNotAvailable; // bool true when valid GPS data is not available - struct Location EKF_origin; // LLH origin of the NED axis system + struct Location EKF_origin; // LLH origin of the NED axis system, internal only + struct Location &public_origin; // LLH origin of the NED axis system, public functions bool validOrigin; // true when the EKF origin is valid ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver @@ -1403,7 +1404,13 @@ private: bool have_table_earth_field; // true when we have initialised table_earth_field_ga Vector3F table_earth_field_ga; // earth field from WMM tables ftype table_declination; // declination in radians from the tables - uint32_t last_field_update_ms; + + // 1Hz update + uint32_t last_oneHz_ms; + void oneHzUpdate(void); + + // move EKF origin at 1Hz + void moveEKFOrigin(void); // handle earth field updates void getEarthFieldTable(const Location &loc);