From f29c302e08d32bf8ad10126b1cb6d88469da7db8 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 9 Apr 2026 18:37:20 +0200 Subject: [PATCH] feat(ekf2): absorb GPS altitude drift into origin instead of velocity fallback --- msg/versioned/VehicleLocalPosition.msg | 1 - src/modules/ekf2/EKF/ekf.h | 14 ++++---------- src/modules/ekf2/EKF2.cpp | 7 +------ .../tasks/FlightTask/FlightTask.cpp | 1 - .../tasks/FlightTask/FlightTask.hpp | 1 - .../ManualAltitude/FlightTaskManualAltitude.cpp | 16 ---------------- .../ManualAltitude/FlightTaskManualAltitude.hpp | 1 - 7 files changed, 5 insertions(+), 36 deletions(-) diff --git a/msg/versioned/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg index 829ce32284..3178a00bb3 100644 --- a/msg/versioned/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -45,7 +45,6 @@ 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/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8d7fa2af30..9c41a67b68 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -179,17 +179,11 @@ public: // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } - template - void uncorrelateCovariance(size_t first) { P.uncorrelateCovariance(first); } - - // adjust baro bias for a GPS altitude drift correction so baro fusion - // doesn't slowly fight the corrected GPS reference - void adjustBaroBiasForDriftCorrection(float altitude_offset) + void shiftAltOrigin(float offset) { -#if defined(CONFIG_EKF2_BAROMETER) - const float delta_z = -altitude_offset; - _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); -#endif + if (PX4_ISFINITE(_local_origin_alt)) { + _local_origin_alt += offset; + } } matrix::Vector3f getRotVarBody() const; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b087bd84ba..97f099817c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1618,7 +1618,6 @@ 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) @@ -2599,11 +2598,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _gps_alt_drift.update(vehicle_gps_position, _gps_alt_drift_pub); if (fabsf(_gps_alt_drift.altitude_offset) > 0.f) { - _ekf.adjustBaroBiasForDriftCorrection(_gps_alt_drift.altitude_offset); - } - - if (!_gps_alt_drift.altitude_good_for_local_control) { - _ekf.uncorrelateCovariance<1>(estimator::State::pos.idx + 2); + _ekf.shiftAltOrigin(_gps_alt_drift.altitude_offset); } } else { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index a9561d5cd4..58d0253012 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -132,7 +132,6 @@ void FlightTask::_evaluateVehicleLocalPosition() _yaw = _sub_vehicle_local_position.get().heading; _unaided_yaw = _sub_vehicle_local_position.get().unaided_heading; _is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control; - _is_altitude_good_for_local_control = _sub_vehicle_local_position.get().altitude_good_for_local_control; // position if (_sub_vehicle_local_position.get().xy_valid) { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 16f0e18273..080c594d1a 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -206,7 +206,6 @@ protected: float _yaw{}; /**< current vehicle yaw heading */ float _unaided_yaw{}; bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */ - bool _is_altitude_good_for_local_control{true}; /**< true if the altitude estimate can be used for altitude control */ float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */ float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 244abb0030..778cea55f2 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -305,22 +305,6 @@ bool FlightTaskManualAltitude::update() _updateConstraintsFromEstimator(); _scaleSticks(); _updateSetpoints(); - - // When altitude estimate is degraded, fall back to velocity-only control - if (!_is_altitude_good_for_local_control) { - if (_altitude_was_good_for_local_control) { - } - - _position_setpoint(2) = NAN; - _dist_to_ground_lock = NAN; - _altitude_was_good_for_local_control = false; - - } else if (!_altitude_was_good_for_local_control) { - // Altitude recovered: reset reference to current position - _position_setpoint(2) = _position(2); - _altitude_was_good_for_local_control = true; - } - _constraints.want_takeoff = _checkTakeoff(); _max_distance_to_ground = INFINITY; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 819c000d68..92648daf7c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -126,7 +126,6 @@ private: bool _updateYawCorrection(); uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - bool _altitude_was_good_for_local_control{true}; /**< tracks previous state for edge detection */ float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ float _max_distance_to_ground{(float)INFINITY}; /**< max distance to ground constraint */