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

src/modules/ekf2/EKF/ekf.h

@@ -179,6 +179,19 @@ public:
 	// get the diagonal elements of the covariance matrix
 	matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
 
+	template <size_t Width>
+	void uncorrelateCovariance(size_t first) { P.uncorrelateCovariance<Width>(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)
+	{
+#if defined(CONFIG_EKF2_BAROMETER)
+		const float delta_z = -altitude_offset;
+		_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
+#endif
+	}
+
 	matrix::Vector3f getRotVarBody() const;
 	matrix::Vector3f getRotVarNed() const;
 	float getYawVar() const;

src/modules/ekf2/EKF2.cpp

@@ -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();
 		}

src/modules/ekf2/EKF2.hpp

@@ -490,6 +490,7 @@ private:
 		uint64_t last_sample_ts{0};
 		bool hit_pending{false};
 		bool altitude_good_for_local_control{true};
+		float altitude_offset{0.f};
 
 		void updateBaroLpf(float baro_alt, uint64_t timestamp);
 		void update(const sensor_gps_s &gps, uORB::PublicationMulti<gps_altitude_drift_correction_s> &pub);