From dbf51da99e8ae7bc62c4e347e5c3868229d96314 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 30 Sep 2024 13:30:57 +0200 Subject: [PATCH] ekf2: rework amsl to ellipsoid altitude conversion --- src/modules/ekf2/EKF2.cpp | 55 ++++++++++++++++++++------------------- src/modules/ekf2/EKF2.hpp | 15 +++++------ 2 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 46a28e469f..8d66fcf52a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1176,7 +1176,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters #if defined(CONFIG_EKF2_GNSS) - global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); + global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt); #else global_pos.alt_ellipsoid = global_pos.alt; #endif @@ -2051,29 +2051,6 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) } #endif // CONFIG_EKF2_OPTICAL_FLOW -#if defined(CONFIG_EKF2_GNSS) -float EKF2::filter_altitude_ellipsoid(float amsl_hgt) -{ - float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; - - if (_gps_alttitude_ellipsoid_previous_timestamp == 0) { - - _wgs84_hgt_offset = height_diff; - _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; - - } else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) { - - // apply a 10 second first order low pass filter to baro offset - float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp); - _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; - float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset); - _wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f); - } - - return amsl_hgt + _wgs84_hgt_offset; -} -#endif // CONFIG_EKF2_GNSS - #if defined(CONFIG_EKF2_AIRSPEED) void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) { @@ -2433,11 +2410,14 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) return; //TODO: change and set to NAN } + const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); + const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m); + gnssSample gnss_sample{ .time_us = vehicle_gps_position.timestamp, .lat = vehicle_gps_position.latitude_deg, .lon = vehicle_gps_position.longitude_deg, - .alt = static_cast(vehicle_gps_position.altitude_msl_m), + .alt = altitude_amsl, .vel = vel_ned, .hacc = vehicle_gps_position.eph, .vacc = vehicle_gps_position.epv, @@ -2454,10 +2434,31 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.setGpsData(gnss_sample); - _gps_time_usec = gnss_sample.time_us; - _gps_alttitude_ellipsoid = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)); + const float geoid_height = altitude_ellipsoid - altitude_amsl; + + if (_last_geoid_height_update_us == 0) { + _geoid_height_lpf.reset(geoid_height); + _last_geoid_height_update_us = gnss_sample.time_us; + + } else if (gnss_sample.time_us > _last_geoid_height_update_us) { + const float dt = 1e-6f * (gnss_sample.time_us - _last_geoid_height_update_us); + _geoid_height_lpf.setParameters(dt, kGeoidHeightLpfTimeConstant); + _geoid_height_lpf.update(geoid_height); + _last_geoid_height_update_us = gnss_sample.time_us; + } + } } + +float EKF2::altEllipsoidToAmsl(float ellipsoid_alt) const +{ + return ellipsoid_alt - _geoid_height_lpf.getState(); +} + +float EKF2::altAmslToEllipsoid(float amsl_alt) const +{ + return amsl_alt + _geoid_height_lpf.getState(); +} #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 6b6366557b..57b3cc749c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -44,6 +44,7 @@ #include "EKF/ekf.h" #include "EKF2Selector.hpp" +#include "mathlib/math/filter/AlphaFilter.hpp" #include @@ -210,10 +211,8 @@ private: bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - /* - * Calculate filtered WGS84 height from estimated AMSL height - */ - float filter_altitude_ellipsoid(float amsl_hgt); + float altEllipsoidToAmsl(float ellipsoid_alt) const; + float altAmslToEllipsoid(float amsl_alt) const; void PublishGpsStatus(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); @@ -445,10 +444,10 @@ private: #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_GNSS) - uint64_t _gps_time_usec {0}; - int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid - uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt - float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + + uint64_t _last_geoid_height_update_us{0}; + static constexpr float kGeoidHeightLpfTimeConstant = 10.f; + AlphaFilter _geoid_height_lpf; ///< height offset between AMSL and ellipsoid hrt_abstime _last_gps_status_published{0};