EKF: add baro bias estimator using GNSS altitude

msg/CMakeLists.txt

@@ -44,6 +44,7 @@ set(msg_files
 	airspeed.msg
 	airspeed_validated.msg
 	airspeed_wind.msg
+	baro_bias_estimate.msg
 	battery_status.msg
 	camera_capture.msg
 	camera_trigger.msg

msg/baro_bias_estimate.msg

@@ -0,0 +1,10 @@
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+uint32 baro_device_id		# unique device ID for the sensor that does not change between power cycles
+float32 bias			# estimated barometric altitude bias (m)
+float32 bias_var		# estimated barometric altitude bias variance (m^2)
+
+float32 innov			# innovation of the last measurement fusion (m)
+float32 innov_var		# innovation variance of the last measurement fusion (m^2)
+float32 innov_test_ratio	# normalized innovation squared test ratio

src/modules/ekf2/CMakeLists.txt

@@ -45,6 +45,7 @@ px4_add_module(
 		3500
 	SRCS
 		EKF/airspeed_fusion.cpp
+		EKF/baro_bias_estimator.cpp
 		EKF/control.cpp
 		EKF/covariance.cpp
 		EKF/drag_fusion.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_library(ecl_EKF
 	airspeed_fusion.cpp
+	baro_bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	drag_fusion.cpp

src/modules/ekf2/EKF/baro_bias_estimator.cpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file baro_bias_estimator.cpp
+ *
+ * @author Mathieu Bresciani 	<mathieu@auterion.com>
+ */
+
+#include "baro_bias_estimator.hpp"
+
+void BaroBiasEstimator::predict(const float dt)
+{
+	// State is constant
+	// Predict state covariance only
+	_state_var += _process_var * dt * dt;
+	constrainStateVar();
+
+	if (dt > FLT_EPSILON && fabsf(_dt - dt) > 0.001f) {
+		_signed_innov_test_ratio_lpf.setParameters(dt, _lpf_time_constant);
+		_dt = dt;
+	}
+}
+
+void BaroBiasEstimator::constrainStateVar()
+{
+	_state_var = math::constrain(_state_var, 1e-8f, _state_var_max);
+}
+
+void BaroBiasEstimator::fuseBias(const float measurement, const float measurement_var)
+{
+	const float innov_var = _state_var + measurement_var;
+	const float innov = measurement - _state;
+	const float K = _state_var / innov_var;
+	const float innov_test_ratio = computeInnovTestRatio(innov, innov_var);
+
+	if (isTestRatioPassing(innov_test_ratio)) {
+		updateState(K, innov);
+		updateStateCovariance(K);
+
+	}
+
+	if (isLargeOffsetDetected()) {
+		// A bias in the state has been detected by the innovation
+		// sequence check.
+		bumpStateVariance();
+	}
+
+	const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio;
+	_signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f));
+
+	_status = packStatus(innov, innov_var, innov_test_ratio);
+}
+
+inline float BaroBiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const
+{
+	return innov * innov / (_gate_size * _gate_size * innov_var);
+}
+
+inline bool BaroBiasEstimator::isTestRatioPassing(const float innov_test_ratio) const
+{
+	return innov_test_ratio < 1.f;
+}
+
+inline void BaroBiasEstimator::updateState(const float K, const float innov)
+{
+	_state = math::constrain(_state + K * innov, -_state_max, _state_max);
+}
+
+inline void BaroBiasEstimator::updateStateCovariance(const float K)
+{
+	_state_var -= K * _state_var;
+	constrainStateVar();
+}
+
+inline bool BaroBiasEstimator::isLargeOffsetDetected() const
+{
+	return fabsf(_signed_innov_test_ratio_lpf.getState()) > 0.2f;
+}
+
+inline void BaroBiasEstimator::bumpStateVariance()
+{
+	_state_var += _process_var_boost_gain * _process_var * _dt * _dt;
+}
+
+inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var,
+		const float innov_test_ratio) const
+{
+	// Send back status for logging
+	status ret{};
+	ret.bias = _state;
+	ret.bias_var = _state_var;
+	ret.innov = innov;
+	ret.innov_var = innov_var;
+	ret.innov_test_ratio = innov_test_ratio;
+
+	return ret;
+}

src/modules/ekf2/EKF/baro_bias_estimator.hpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file baro_bias_estimator.hpp
+ * @brief Implementation of a single-state baro bias estimator
+ *
+ * state: baro bias (in meters)
+ *
+ * The state is noise driven: Transition matrix A = 1
+ * x[k+1] = Ax[k] + v with v ~ N(0, Q)
+ * y[k] = x[k] + w with w ~ N(0, R)
+ *
+ * The difference between the current barometric altitude and another absolute
+ * reference (e.g.: GNSS altitude) is used as a bias measurement.
+ *
+ * During the measurment update step, the Normalized Innovation Squared (NIS) is checked
+ * and the measurement is rejected if larger than the gate size.
+ *
+ * @author Mathieu Bresciani 	<mathieu@auterion.com>
+ */
+
+#ifndef EKF_BARO_BIAS_ESTIMATOR_HPP
+#define EKF_BARO_BIAS_ESTIMATOR_HPP
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+class BaroBiasEstimator
+{
+public:
+	struct status {
+		float bias;
+		float bias_var;
+		float innov;
+		float innov_var;
+		float innov_test_ratio;
+	};
+
+	BaroBiasEstimator() = default;
+	~BaroBiasEstimator() = default;
+
+	void predict(float dt);
+	void fuseBias(float measurement, float measurement_var);
+
+	void setBias(float bias) { _state = bias; }
+	void setProcessNoiseStdDev(float process_noise)
+	{
+		_process_var = process_noise * process_noise;
+		_state_drift_rate = 3.f * process_noise;
+	}
+	void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
+	void setInnovGate(float gate_size) { _gate_size = gate_size; }
+
+	void setMaxStateNoise(float max_noise) { _state_var_max = max_noise * max_noise; }
+
+	float getBias() const { return _state; }
+	float getBiasVar() const { return _state_var; }
+	const status &getStatus() const { return _status; }
+
+private:
+	float _state{0.f};
+	float _state_max{100.f};
+	float _state_drift_rate{0.005f}; ///< in m/s
+	float _dt{0.01f};
+
+	float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio)
+	float _state_var{0.1f}; ///< Initial state uncertainty variance (m^2)
+	float _process_var{25.0e-6f}; ///< State process noise variance (m^2/s^2)
+	float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2)
+
+	AlphaFilter<float> _signed_innov_test_ratio_lpf; ///< innovation sequence monitoring; used to detect a bias in the state
+
+	status _status;
+
+	void constrainStateVar();
+	float computeKalmanGain(float innov_var) const;
+
+	/*
+	 * Compute the ratio between the Normalized Innovation Squared (NIS)
+	 * and its maximum gate size. Use isTestRatioPassing to know if the
+	 * measurement should be fused or not.
+	 */
+	float computeInnovTestRatio(float innov, float innov_var) const;
+	bool isTestRatioPassing(float innov_test_ratio) const;
+
+	void updateState(float K, float innov);
+	void updateStateCovariance(float K);
+
+	bool isLargeOffsetDetected() const;
+	void bumpStateVariance();
+
+	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
+
+	static constexpr float _lpf_time_constant{10.f}; ///< in seconds
+	static constexpr float _process_var_boost_gain{1.0e3f};
+};
+#endif // !EKF_BARO_BIAS_ESTIMATOR_HPP

src/modules/ekf2/EKF/common.h

@@ -257,6 +257,7 @@ struct parameters {
 	float gps_pos_noise{0.5f};		///< minimum allowed observation noise for gps position fusion (m)
 	float pos_noaid_noise{10.0f};		///< observation noise for non-aiding position fusion (m)
 	float baro_noise{2.0f};			///< observation noise for barometric height fusion (m)
+	float baro_drift_rate{0.005f}; ///< process noise for barometric height bias estimation (m/s)
 	float baro_innov_gate{5.0f};		///< barometric and GPS height innovation consistency gate size (STD)
 	float gps_pos_innov_gate{5.0f};		///< GPS horizontal position innovation consistency gate size (STD)
 	float gps_vel_innov_gate{5.0f};		///< GPS velocity innovation consistency gate size (STD)

src/modules/ekf2/EKF/control.cpp

@@ -1161,6 +1161,7 @@ void Ekf::controlHeightFusion()
 		break;
 	}
 
+	updateBaroHgtBias();
 	updateBaroHgtOffset();
 
 	if (_control_status.flags.rng_hgt
@@ -1184,7 +1185,8 @@ void Ekf::controlHeightFusion()
 			Vector3f baro_hgt_obs_var;
 
 			// vertical position innovation - baro measurement has opposite sign to earth z axis
-			_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
+			const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+			_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
 			// observation variance - user parameter defined
 			baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
 			// innovation gate size

src/modules/ekf2/EKF/ekf.h

@@ -46,6 +46,7 @@
 #include "estimator_interface.h"
 
 #include "EKFGSF_yaw.h"
+#include "baro_bias_estimator.hpp"
 
 class Ekf final : public EstimatorInterface
 {
@@ -297,6 +298,7 @@ public:
 	// returns false when data is not available
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+	void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio);
 
 private:
 
@@ -500,6 +502,8 @@ private:
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
 	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+	float _baro_hgt_bias{0.0f};
+	float _baro_hgt_bias_var{1.f};
 
 	// Variables used to control activation of post takeoff functionality
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
@@ -890,6 +894,7 @@ private:
 	void startGpsHgtFusion();
 
 	void updateBaroHgtOffset();
+	void updateBaroHgtBias();
 
 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();
@@ -983,6 +988,8 @@ private:
 	// yaw estimator instance
 	EKFGSF_yaw _yawEstimator{};
 
+	BaroBiasEstimator _baro_b_est{};
+
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested
 	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1319,7 +1319,10 @@ void Ekf::updateBaroHgtOffset()
 		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
 
 		// apply a 10 second first order low pass filter to baro offset
-		const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset);
+		const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+;
+		const float offset_rate_correction =  0.1f * (unbiased_baro + _state.pos(2) -
+								_baro_hgt_offset);
 		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
 	}
 }
@@ -1334,6 +1337,37 @@ float Ekf::getGpsHeightVariance()
 	return gps_alt_var;
 }
 
+void Ekf::updateBaroHgtBias()
+{
+	// Baro bias estimation using GPS altitude
+	if (_baro_data_ready) {
+		const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
+		_baro_b_est.setMaxStateNoise(_params.baro_noise);
+		_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
+		_baro_b_est.predict(dt);
+	}
+
+	if (_gps_data_ready && !_gps_hgt_intermittent
+	    && _gps_checks_passed &&_NED_origin_initialised
+	    && !_baro_hgt_faulty) {
+		// Use GPS altitude as a reference to compute the baro bias measurement
+		const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
+					- (_gps_sample_delayed.hgt - _gps_alt_ref);
+		const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise);
+		_baro_b_est.fuseBias(baro_bias, baro_bias_var);
+	}
+}
+
+void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio)
+{
+	BaroBiasEstimator::status status = _baro_b_est.getStatus();
+	bias = status.bias;
+	bias_var = status.bias_var;
+	innov = status.innov;
+	innov_var = status.innov_var;
+	innov_test_ratio = status.innov_test_ratio;
+}
+
 Vector3f Ekf::getVisionVelocityInEkfFrame() const
 {
 	Vector3f vel;

src/modules/ekf2/EKF2.cpp

@@ -202,6 +202,7 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_status_flags_pub.advertise();
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
+	_baro_bias_estimate_pub.advertise();
 
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
 
@@ -505,6 +506,7 @@ void EKF2::Run()
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
 			PublishYawEstimatorStatus(now);
+			PublishBaroBiasEstimate(now);
 
 			UpdateMagCalibration(now);
 
@@ -548,6 +550,21 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 	}
 }
 
+void EKF2::PublishBaroBiasEstimate(const hrt_abstime &timestamp)
+{
+
+	baro_bias_estimate_s bbe{};
+	bbe.timestamp = timestamp;
+	bbe.timestamp_sample = timestamp;
+	bbe.baro_device_id = _device_id_baro;
+	_ekf.getBaroBiasEstimatorStatus(bbe.bias,
+					bbe.bias_var,
+					bbe.innov,
+					bbe.innov_var,
+					bbe.innov_test_ratio);
+	_baro_bias_estimate_pub.publish(bbe);
+}
+
 void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
 {
 	// publish GPS drift data only when updated to minimise overhead

src/modules/ekf2/EKF2.hpp

@@ -62,6 +62,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/baro_bias_estimate.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_drift.h>
@@ -129,6 +130,7 @@ private:
 	void Run() override;
 
 	void PublishAttitude(const hrt_abstime &timestamp);
+	void PublishBaroBiasEstimate(const hrt_abstime &timestamp);
 	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishEventFlags(const hrt_abstime &timestamp);
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
@@ -256,6 +258,7 @@ private:
 	uint32_t _filter_warning_event_changes{0};
 	uint32_t _filter_information_event_changes{0};
 
+	uORB::PublicationMulti<baro_bias_estimate_s>         _baro_bias_estimate_pub{ORB_ID(baro_bias_estimate)};
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};

src/modules/logger/logged_topics.cpp

@@ -137,6 +137,7 @@ void LoggedTopics::add_default_topics()
 #else
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
 	add_topic("estimator_selector_status");
+	add_topic_multi("baro_bias_estimate", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);