From 7e4132a00678d4161947532895a72bdbac8d964d Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Wed, 25 Mar 2026 13:44:10 +0100 Subject: [PATCH] feat(ekf2): gnss altitude correction-drift detector --- msg/CMakeLists.txt | 1 + msg/GpsAltitudeDriftCorrection.msg | 4 + msg/versioned/VehicleLocalPosition.msg | 1 + src/modules/commander/Commander.cpp | 1 - src/modules/commander/HomePosition.cpp | 68 +++----------- src/modules/commander/HomePosition.hpp | 20 +---- src/modules/ekf2/EKF2.cpp | 117 +++++++++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 28 ++++++ 8 files changed, 166 insertions(+), 74 deletions(-) create mode 100644 msg/GpsAltitudeDriftCorrection.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 06feb9a335..b4068ddb9e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -114,6 +114,7 @@ set(msg_files GpioIn.msg GpioOut.msg GpioRequest.msg + GpsAltitudeDriftCorrection.msg GpsDump.msg GpsInjectData.msg Gripper.msg diff --git a/msg/GpsAltitudeDriftCorrection.msg b/msg/GpsAltitudeDriftCorrection.msg new file mode 100644 index 0000000000..234953d6fb --- /dev/null +++ b/msg/GpsAltitudeDriftCorrection.msg @@ -0,0 +1,4 @@ +uint64 timestamp # [us] time since system start + +float32 altitude_offset # [m] detected altitude offset to apply + diff --git a/msg/versioned/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg index 3178a00bb3..829ce32284 100644 --- a/msg/versioned/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -45,6 +45,7 @@ float32 unaided_heading # Same as heading but generated by integ float32 delta_heading # Heading delta caused by latest heading reset [rad] uint8 heading_reset_counter # Index of latest heading reset bool heading_good_for_control +bool altitude_good_for_local_control float32 tilt_var diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7a4ea1259d..438b38f1cf 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2210,7 +2210,6 @@ void Commander::landDetectorUpdate() events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected"); _vehicle_status.takeoff_time = hrt_absolute_time(); _have_taken_off_since_arming = true; - _home_position.setTakeoffTime(_vehicle_status.takeoff_time); } // automatically set or update home position diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 4eb0f068de..295bcebd9e 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -330,22 +330,6 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) _global_position_sub.update(); _attitude_sub.update(); - if (_vehicle_air_data_sub.updated()) { - vehicle_air_data_s baro_data; - _vehicle_air_data_sub.copy(&baro_data); - const float baro_alt = baro_data.baro_alt_meter; - - if (_last_baro_timestamp != 0) { - const float dt = baro_data.timestamp - _last_baro_timestamp; - _lpf_baro.update(baro_alt, dt); - - } else { - _lpf_baro.reset(baro_alt); - } - - _last_baro_timestamp = baro_data.timestamp; - } - if (_vehicle_gps_position_sub.updated()) { sensor_gps_s vehicle_gps_position; _vehicle_gps_position_sub.copy(&vehicle_gps_position); @@ -365,49 +349,21 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) _gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid && isGpsHorizontalFusionEnabled(); + } - if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0 - && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) { + // Apply home altitude correction from GPS altitude drift detection + if (_param_com_home_en.get() && _gps_alt_drift_sub.updated()) { + gps_altitude_drift_correction_s correction; + _gps_alt_drift_sub.copy(&correction); - const float gps_alt = static_cast(_gps_alt); + home_position_s home = _home_position_pub.get(); - if (!PX4_ISFINITE(_gps_vel_integral)) { - _gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos - _baro_gps_static_offset = gps_alt - _lpf_baro.getState(); - } - - _gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s); - - // correct baro_alt with offset from GPS alt from when the drift integral was initialized - const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset; - const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied; - - // Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude - // are more consistent with each other than either is with the GPS altitude (with home offset). - if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) && - fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) { - - const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied; - - if (fabsf(offset_new) > kAltitudeDifferenceThreshold) { - - home_position_s home = _home_position_pub.get(); - home.alt -= offset_new; - home.z += offset_new; - home.timestamp = now; - home.manual_home = false; - home.update_count = _home_position_pub.get().update_count + 1U; - - _home_position_pub.update(home); - _home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected - } - } - - } else { - _gps_vel_integral = NAN; - } - - _last_gps_timestamp = vehicle_gps_position.timestamp; + home.alt += correction.altitude_offset; + home.z -= correction.altitude_offset; + home.timestamp = hrt_absolute_time(); + home.manual_home = false; + home.update_count = _home_position_pub.get().update_count + 1U; + _home_position_pub.update(home); } const vehicle_local_position_s &lpos = _local_position_sub.get(); diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index fd5eb1e72d..f0daca98f0 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -40,9 +40,8 @@ #include #include #include +#include #include -#include -#include #include using namespace time_literals; @@ -53,9 +52,6 @@ static constexpr float kHomePositionGPSRequiredEPV = 10.f; static constexpr float kHomePositionGPSRequiredEVH = 1.f; static constexpr float kMinHomePositionChangeEPH = 1.f; static constexpr float kMinHomePositionChangeEPV = 1.5f; -static constexpr float kLpfBaroTimeConst = 5.f; -static constexpr float kAltitudeDifferenceThreshold = 1.f; // altitude difference after which home position gets updated -static constexpr uint64_t kHomePositionCorrectionTimeWindow = 120_s; class HomePosition: public ModuleParams { @@ -66,7 +62,6 @@ public: bool setHomePosition(bool force = false); void setInAirHomePosition(); bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw); - void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; } void update(bool set_automatically, bool check_if_changed); @@ -85,20 +80,11 @@ private: static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt); uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _gps_alt_drift_sub{ORB_ID(gps_altitude_drift_correction)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::SubscriptionData _attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - - uint64_t _last_gps_timestamp{0}; - uint64_t _last_baro_timestamp{0}; - AlphaFilter _lpf_baro{kLpfBaroTimeConst}; - float _gps_vel_integral{NAN}; - float _home_altitude_offset_applied{0.f}; - float _baro_gps_static_offset{0.f}; - uint64_t _takeoff_time{0}; - + uORB::SubscriptionData _attitude_sub{ORB_ID(vehicle_attitude)}; uORB::PublicationData _home_position_pub{ORB_ID(home_position)}; uint8_t _heading_reset_counter{0}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 84227fab6a..3eaf921115 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1618,6 +1618,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.heading_var = _ekf.getYawVar(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); + lpos.altitude_good_for_local_control = _gps_alt_drift.altitude_good_for_local_control; lpos.tilt_var = _ekf.getTiltVariance(); #if defined(CONFIG_EKF2_TERRAIN) @@ -2181,6 +2182,10 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset}); +#if defined(CONFIG_EKF2_GNSS) + _gps_alt_drift.updateBaroLpf(airdata.baro_alt_meter, airdata.timestamp_sample); +#endif // CONFIG_EKF2_GNSS + ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } @@ -2408,6 +2413,112 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) +void EKF2::GpsAltDriftDetector::updateBaroLpf(float baro_alt, uint64_t timestamp) +{ + const float dt = 1e-6f * (timestamp - last_baro_ts); + + if (last_baro_ts != 0 && dt < 1.f) { + baro_lpf.update(baro_alt, dt); + + } else { + baro_lpf.reset(baro_alt); + } + + last_baro_ts = timestamp; +} + +void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, uORB::PublicationMulti &pub) +{ + const bool gps_timeout = (last_gps_ts != 0) && (gps.timestamp - last_gps_ts > 500000); + + if (!gps_timeout && (last_gps_ts != 0) && (last_baro_ts != 0)) { + + vel_integral += 1e-6f * (gps.timestamp - last_gps_ts) * (-gps.vel_d_m_s); + + // sample at 1Hz normally, or immediately on pending hit + const bool sample_due = (last_sample_ts == 0) + || hit_pending + || (gps.timestamp >= last_sample_ts + 1000000); + + if (sample_due) { + const float gps_alt = static_cast(gps.altitude_msl_m); + d1[widx] = gps_alt - baro_lpf.getState(); + d2[widx] = gps_alt - vel_integral; + + widx = (widx + 1) % kWindowSize; + + if (wcount < kWindowSize) { + wcount++; + } + + last_sample_ts = gps.timestamp; + + if (wcount > 1) { + const int newest = (widx - 1 + kWindowSize) % kWindowSize; + const int oldest = (widx - wcount + kWindowSize) % kWindowSize; + + const float a = fabsf(d1[newest] - d1[oldest]); // change in (gps_alt - baro_alt) over window + const float b = fabsf(d2[newest] - d2[oldest]); // change in (gps_alt - vel_integral) over window + const float c = fabsf((d1[newest] - d2[newest]) - (d1[oldest] - d2[oldest])); // change in (vel_integral - baro_alt) over window + + // gps_alt drift has to have relevant magnitude and larger than vel-baro drift + const bool hit = (a > kDriftThreshold) && (b > kDriftThreshold) && (a > c) && (b > c); + + // hit pending to filter out single outliers + if (hit && hit_pending) { + gps_altitude_drift_correction_s correction{}; + correction.timestamp = hrt_absolute_time(); + correction.altitude_offset = d1[newest] - d1[oldest]; + pub.publish(correction); + hit_pending = false; + altitude_good_for_local_control = false; + wcount = 1; + + } else { + hit_pending = hit; + } + + // Re-enable when recent samples show stability + if (!altitude_good_for_local_control && wcount >= kStabilityWindow) { + const int recent = (widx - kStabilityWindow + kWindowSize) % kWindowSize; + const float a_recent = fabsf(d1[newest] - d1[recent]); + const float b_recent = fabsf(d2[newest] - d2[recent]); + + if ((a_recent < 0.2f * kDriftThreshold) && (b_recent < 0.2f * kDriftThreshold)) { + altitude_good_for_local_control = true; + + const float residual = d1[newest] - d1[oldest]; + + if (fabsf(residual) > 0.01f) { + gps_altitude_drift_correction_s correction{}; + correction.timestamp = hrt_absolute_time(); + correction.altitude_offset = residual; + pub.publish(correction); + } + + wcount = 1; + } + } + } + } + + } else { + reset(); + } + + last_gps_ts = gps.timestamp; +} + +void EKF2::GpsAltDriftDetector::reset() +{ + vel_integral = 0.f; + wcount = 0; + widx = 0; + last_sample_ts = 0; + hit_pending = false; + altitude_good_for_local_control = true; +} + void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF GPS message @@ -2480,6 +2591,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _last_geoid_height_update_us = gnss_sample.time_us; } + if (_ekf.control_status_flags().in_air && _ekf.getHeightSensorRef() == HeightSensor::GNSS) { + _gps_alt_drift.update(vehicle_gps_position, _gps_alt_drift_pub); + + } else { + _gps_alt_drift.reset(); + } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 36d7763995..0d5bdeb9fc 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -103,6 +103,7 @@ #if defined(CONFIG_EKF2_GNSS) # include +# include # include #endif // CONFIG_EKF2_GNSS @@ -470,6 +471,33 @@ private: hrt_abstime _status_gnss_yaw_pub_last {0}; uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; # endif // CONFIG_EKF2_GNSS_YAW + + struct GpsAltDriftDetector { + static constexpr int kWindowSize = 20; // roughly 20 seconds (at 1 Hz sample rate) + static constexpr int kStabilityWindow = 5; // samples to check for re-enable + static constexpr float kBaroLpfTimeConst = 3.f; + static constexpr float kDriftThreshold = 1.f; // [m] + + AlphaFilter baro_lpf; + uint64_t last_baro_ts{0}; + uint64_t last_gps_ts{0}; + float vel_integral{0.f}; + + float d1[kWindowSize] {}; // gps_alt - baro_alt + float d2[kWindowSize] {}; // gps_alt - vel_integral + int widx{0}; + int wcount{0}; + uint64_t last_sample_ts{0}; + bool hit_pending{false}; + bool altitude_good_for_local_control{true}; + + void updateBaroLpf(float baro_alt, uint64_t timestamp); + void update(const sensor_gps_s &gps, uORB::PublicationMulti &pub); + void reset(); + }; + + GpsAltDriftDetector _gps_alt_drift{}; + uORB::PublicationMulti _gps_alt_drift_pub{ORB_ID(gps_altitude_drift_correction)}; #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_GRAVITY_FUSION)