fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 16:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(ekf2): decorrelate vertical position and adjust baro bias during gnss altitude drift correction

Browse Source
This commit is contained in:
Marco Hauswirth
2026-04-08 18:20:21 +02:00
parent 7e4132a006
commit ca0bd4a8ac
3 changed files with 27 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+13 -1
View File
@@ -2429,6 +2429,7 @@ void EKF2::GpsAltDriftDetector::updateBaroLpf(float baro_alt, uint64_t timestamp
void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, uORB::PublicationMulti<gps_altitude_drift_correction_s> &pub)
{
altitude_offset = 0.f;
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)) {
@@ -2466,10 +2467,12 @@ void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, uORB::Publicatio
// hit pending to filter out single outliers
if (hit && hit_pending) {
const float offset = d1[newest] - d1[oldest];
gps_altitude_drift_correction_s correction{};
correction.timestamp = hrt_absolute_time();
correction.altitude_offset = d1[newest] - d1[oldest];
correction.altitude_offset = offset;
pub.publish(correction);
altitude_offset += offset;
hit_pending = false;
altitude_good_for_local_control = false;
wcount = 1;
@@ -2494,6 +2497,7 @@ void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, uORB::Publicatio
correction.timestamp = hrt_absolute_time();
correction.altitude_offset = residual;
pub.publish(correction);
altitude_offset += residual;
}
wcount = 1;
@@ -2594,6 +2598,14 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
if (_ekf.control_status_flags().in_air && _ekf.getHeightSensorRef() == HeightSensor::GNSS) {
_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);
}
} else {
_gps_alt_drift.reset();
}