From 1980b5c5e8c475b18caeabf4fabb730f39adbd45 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 16 Jun 2022 00:59:54 -0400 Subject: [PATCH] ekf2: setEkfGlobalOrigin() reset baro and hgt sensor offsets if necessary - handle uninitalized _gps_alt_ref - add basic lat/lon/alt sanity checks --- src/modules/ekf2/EKF/ekf.h | 6 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 76 +++++++++++++++++++---------- src/modules/ekf2/EKF/gps_checks.cpp | 5 +- src/modules/ekf2/EKF/gps_fusion.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 20 ++++---- 5 files changed, 68 insertions(+), 41 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index df86bdadf1..a0d39576b2 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -169,9 +169,9 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; - void setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); + bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); - float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } bool setEkfGlobalOriginAltitude(const float altitude); @@ -521,7 +521,7 @@ private: // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) - float _gps_alt_ref{0.0f}; ///< WGS-84 height (m) + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index fc89d182b3..a731bc8047 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -256,7 +256,7 @@ void Ekf::resetHeightToGps() _information_events.flags.reset_hgt_to_gps = true; const float z_pos_before_reset = _state.pos(2); - resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref); + resetVerticalPositionTo(-_gps_sample_delayed.hgt + getEkfGlobalOriginAltitude()); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance()); @@ -695,40 +695,62 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo origin_time = _last_gps_origin_time_us; latitude = _pos_ref.getProjectionReferenceLat(); longitude = _pos_ref.getProjectionReferenceLon(); - origin_alt = _gps_alt_ref; + origin_alt = getEkfGlobalOriginAltitude(); return _NED_origin_initialised; } -void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude) { - bool current_pos_available = false; - double current_lat = static_cast(NAN); - double current_lon = static_cast(NAN); - float current_alt = 0.f; + // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space + if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) + && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) + && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) + ) { + bool current_pos_available = false; + double current_lat = static_cast(NAN); + double current_lon = static_cast(NAN); - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && isHorizontalAidingActive()) { - _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); - current_alt = -_state.pos(2) + _gps_alt_ref; - current_pos_available = true; - } - - // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_last_imu); + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (_pos_ref.isInitialized() && isHorizontalAidingActive()) { + _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); + current_pos_available = true; + } - if (current_pos_available) { - // reset horizontal position - Vector2f position = _pos_ref.project(current_lat, current_lon); - resetHorizontalPositionTo(position); + const float gps_alt_ref_prev = getEkfGlobalOriginAltitude(); - // reset altitude + // reinitialize map projection to latitude, longitude, altitude, and reset position + _pos_ref.initReference(latitude, longitude, _time_last_imu); _gps_alt_ref = altitude; - resetVerticalPositionTo(_gps_alt_ref - current_alt); - } else { - // reset altitude - _gps_alt_ref = altitude; + // minimum change in position or height that triggers a reset + static constexpr float MIN_RESET_DIST_M = 0.01f; + + if (current_pos_available) { + // reset horizontal position + Vector2f position = _pos_ref.project(current_lat, current_lon); + + if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) { + resetHorizontalPositionTo(position); + } + } + + // reset vertical position (if there's any change) + if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) { + // determine current z + float current_alt = -_state.pos(2) + gps_alt_ref_prev; + + resetVerticalPositionTo(_gps_alt_ref - current_alt); + + _baro_hgt_offset -= _state_reset_status.posD_change; + + _rng_hgt_offset -= _state_reset_status.posD_change; + _ev_hgt_offset -= _state_reset_status.posD_change; + } + + return true; } + + return false; } // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position @@ -1283,7 +1305,7 @@ void Ekf::startGpsHgtFusion() // switch out of range aid // calculate height sensor offset such that current // measurement matches our current height estimate - _gps_hgt_offset = (_gps_sample_delayed.hgt - _gps_alt_ref) + _state.pos(2); + _gps_hgt_offset = _gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() + _state.pos(2); } else { _gps_hgt_offset = 0.f; @@ -1381,7 +1403,7 @@ void Ekf::updateBaroHgtBias() && !_baro_hgt_faulty) { // Use GPS altitude as a reference to compute the baro bias measurement const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset) - - (_gps_sample_delayed.hgt - _gps_alt_ref); + - (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude()); const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise); _baro_b_est.fuseBias(baro_bias, baro_bias_var); } diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 6701eda17d..8b69f2f71b 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -78,7 +78,10 @@ bool Ekf::collect_gps(const gpsMessage &gps) } // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - _gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2); + if (!PX4_ISFINITE(_gps_alt_ref)) { + _gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2); + } + _NED_origin_initialised = true; _earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat())); diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 86948f957c..940ce3f151 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -77,7 +77,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) position(1) = gps_sample.pos(1); // vertical position - gps measurement has opposite sign to earth z axis - position(2) = -(gps_sample.hgt - _gps_alt_ref - _gps_hgt_offset); + position(2) = -(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_offset); const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 82255b40eb..e8f72196d1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -343,18 +343,20 @@ void EKF2::Run() if (_vehicle_command_sub.update(&vehicle_command)) { if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { - if (!_ekf.control_status_flags().in_air) { - - uint64_t origin_time {}; - double latitude = vehicle_command.param5; - double longitude = vehicle_command.param6; - float altitude = vehicle_command.param7; - - _ekf.setEkfGlobalOrigin(latitude, longitude, altitude); + double latitude = vehicle_command.param5; + double longitude = vehicle_command.param6; + float altitude = vehicle_command.param7; + if (_ekf.setEkfGlobalOrigin(latitude, longitude, altitude)) { // Validate the ekf origin status. + uint64_t origin_time {}; _ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude); - PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast(altitude)); + PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", + _instance, latitude, longitude, static_cast(altitude)); + + } else { + PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n", + _instance, latitude, longitude, static_cast(altitude)); } } }