ekf2: estimator aid source status (baro height)

src/modules/ekf2/EKF/control.cpp

@@ -779,16 +779,16 @@ void Ekf::checkVerticalAccelerationHealth()
 	bool are_vertical_pos_and_vel_independant = false;
 
 	if (_control_status.flags.gps) {
-		// velocity
-		auto &gps_vel = _aid_src_gnss_vel;
+		// GNSS velocity
+		const auto &gps_vel = _aid_src_gnss_vel;
 
 		if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) {
 			_vert_vel_fuse_time_us = gps_vel.time_last_fuse[2];
 			_vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]);
 		}
 
-		// position
-		auto &gps_pos = _aid_src_gnss_pos;
+		// GNSS position
+		const auto &gps_pos = _aid_src_gnss_pos;
 
 		if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) {
 			_vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2];
@@ -796,6 +796,16 @@ void Ekf::checkVerticalAccelerationHealth()
 		}
 	}
 
+	if (_control_status.flags.baro_hgt) {
+		// baro height
+		const auto &baro_hgt = _aid_src_baro_hgt;
+
+		if (baro_hgt.time_last_fuse > _vert_pos_fuse_attempt_time_us) {
+			_vert_pos_fuse_attempt_time_us = baro_hgt.time_last_fuse;
+			_vert_pos_innov_ratio = baro_hgt.innovation / sqrtf(baro_hgt.innovation_variance);
+		}
+	}
+
 	if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
 		if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
 			// If vertical position and velocity come from independent sensors then we can
@@ -941,15 +951,15 @@ void Ekf::controlHeightFusion()
 	updateBaroHgtOffset();
 	updateGroundEffect();
 
-	if (_control_status.flags.baro_hgt) {
+	if (_baro_data_ready) {
+		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
 
-		if (_baro_data_ready && !_baro_hgt_faulty) {
-			fuseBaroHgt();
+		if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
+			fuseBaroHgt(_aid_src_baro_hgt);
 		}
+	}
 
-	} else if (_control_status.flags.gps_hgt) {
-		// handled by gps fusion
-	} else if (_control_status.flags.rng_hgt) {
+	if (_control_status.flags.rng_hgt) {
 
 		if (_range_sensor.isDataHealthy()) {
 			fuseRngHgt();

src/modules/ekf2/EKF/covariance.cpp

@@ -991,7 +991,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f);
 
 			if (bad_vz_gps || bad_vz_ev) {
-				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _baro_hgt_innov < 0.0f);
+				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation    < 0.0f);
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
 				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _rng_hgt_innov < 0.0f);
 				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);

src/modules/ekf2/EKF/ekf.h

@@ -86,9 +86,9 @@ public:
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
 
 	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
 	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
@@ -340,11 +340,13 @@ public:
 
 	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 
-	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
-	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
+	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
 
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
+	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
+	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
+
 private:
 
 	// set the internal states and status to their default value
@@ -452,9 +454,6 @@ private:
 	Vector3f _ev_pos_innov{};	///< external vision position innovations (m)
 	Vector3f _ev_pos_innov_var{};	///< external vision position innovation variances (m**2)
 
-	float _baro_hgt_innov{};		///< baro hgt innovations (m)
-	float _baro_hgt_innov_var{};	///< baro hgt innovation variances (m**2)
-
 	float _rng_hgt_innov{};	///< range hgt innovations (m)
 	float _rng_hgt_innov_var{};	///< range hgt innovation variances (m**2)
 
@@ -493,11 +492,13 @@ private:
 	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
-	estimator_aid_source_3d_s _aid_src_gnss_vel{};
-	estimator_aid_source_3d_s _aid_src_gnss_pos{};
+	estimator_aid_source_1d_s _aid_src_baro_hgt{};
 
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
 
+	estimator_aid_source_3d_s _aid_src_gnss_vel{};
+	estimator_aid_source_3d_s _aid_src_gnss_pos{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -644,10 +645,13 @@ private:
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);
 
-	void fuseBaroHgt();
+	void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
+
 	void fuseRngHgt();
 	void fuseEvHgt();
 
+	void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
+
 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
@@ -1063,7 +1067,7 @@ private:
 
 	void resetGpsDriftCheckFilters();
 
-	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status)
+	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
 	{
 		if (status.timestamp_sample != 0) {
 			status.timestamp_sample = 0;
@@ -1077,7 +1081,7 @@ private:
 		return false;
 	}
 
-	void resetEstimatorAidStatus(estimator_aid_source_1d_s &status)
+	void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
 	{
 		if (resetEstimatorAidStatusFlags(status)) {
 			status.observation = 0;
@@ -1090,7 +1094,7 @@ private:
 	}
 
 	template <typename T>
-	bool resetEstimatorAidStatusFlags(T &status)
+	bool resetEstimatorAidStatusFlags(T &status) const
 	{
 		if (status.timestamp_sample != 0) {
 			status.timestamp_sample = 0;
@@ -1108,7 +1112,7 @@ private:
 	}
 
 	template <typename T>
-	void resetEstimatorAidStatus(T &status)
+	void resetEstimatorAidStatus(T &status) const
 	{
 		if (resetEstimatorAidStatusFlags(status)) {
 			for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
@@ -1121,6 +1125,21 @@ private:
 			}
 		}
 	}
+
+	void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
+	{
+		status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
+		status.innovation_rejected = (status.test_ratio > 1.f);
+	}
+
+	template <typename T>
+	void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
+	{
+		for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
+			status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
+			status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
+		}
+	}
 };
 
 #endif // !EKF_EKF_H

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -941,7 +941,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	// return the vertical position innovation test ratio
 	if (_control_status.flags.baro_hgt) {
-		hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_baro_hgt.test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.gps_hgt) {
 		hgt = math::max(sqrtf(_aid_src_gnss_pos.test_ratio[2]), FLT_MIN);

src/modules/ekf2/EKF/estimator_interface.h

@@ -334,7 +334,6 @@ protected:
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio
-	float _baro_hgt_test_ratio{};	// baro height innovation consistency check ratios
 	float _rng_hgt_test_ratio{};		// range finder height innovation consistency check ratios
 	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _tas_test_ratio{};		// tas innovation consistency check ratio

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -139,10 +139,12 @@ void Ekf::fuseFakePosition()
 
 		fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i);
 		fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
-		fake_pos.test_ratio[i] = sq(fake_pos.innovation[i]) / (sq(innov_gate) * fake_pos.innovation_variance[i]);
+	}
 
-		fake_pos.innovation_rejected[i] = (fake_pos.test_ratio[i] > 1.f);
+	setEstimatorAidStatusTestRatio(fake_pos, innov_gate);
 
+	// fuse
+	for (int i = 0; i < 2; i++) {
 		// always protect against extreme values that could result in a NaN
 		fake_pos.fusion_enabled[i] = fake_pos.test_ratio[i] < sq(100.0f / innov_gate);
 

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -55,11 +55,10 @@ void Ekf::updateGpsVel(const gpsSample &gps_sample)
 
 		gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
 		gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
-		gps_vel.test_ratio[i] = sq(gps_vel.innovation[i]) / (sq(innov_gate) * gps_vel.innovation_variance[i]);
-
-		gps_vel.innovation_rejected[i] = (gps_vel.test_ratio[i] > 1.f);
 	}
 
+	setEstimatorAidStatusTestRatio(gps_vel, innov_gate);
+
 	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	if (_fault_status.flags.bad_acc_vertical && gps_vel.innovation_rejected[2]) {
@@ -109,11 +108,10 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 
 		gps_pos.innovation[i] = _state.pos(i) - position(i);
 		gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
-		gps_pos.test_ratio[i] = sq(gps_pos.innovation[i]) / (sq(innov_gate) * gps_pos.innovation_variance[i]);
-
-		gps_pos.innovation_rejected[i] = (gps_pos.test_ratio[i] > 1.f);
 	}
 
+	setEstimatorAidStatusTestRatio(gps_pos, innov_gate);
+
 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	if (_fault_status.flags.bad_acc_vertical && gps_pos.innovation_rejected[2]) {

src/modules/ekf2/EKF/height_fusion.cpp

@@ -38,12 +38,23 @@
 
 #include "ekf.h"
 
-void Ekf::fuseBaroHgt()
+void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt)
 {
-	// vertical position innovation - baro measurement has opposite sign to earth z axis
-	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+	// reset flags
+	resetEstimatorAidStatusFlags(baro_hgt);
 
-	_baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
+	// innovation gate size
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+
+	// observation variance - user parameter defined
+	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+
+	// vertical position innovation - baro measurement has opposite sign to earth z axis
+	baro_hgt.observation = -(_baro_sample_delayed.hgt - _baro_b_est.getBias() - _baro_hgt_offset);
+	baro_hgt.observation_variance = obs_var;
+
+	baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
+	baro_hgt.innovation_variance = P(9, 9) + obs_var;
 
 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -52,24 +63,40 @@ void Ekf::fuseBaroHgt()
 		const float deadzone_start = 0.0f;
 		const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
 
-		if (_baro_hgt_innov < -deadzone_start) {
-			if (_baro_hgt_innov <= -deadzone_end) {
-				_baro_hgt_innov += deadzone_end;
+		if (baro_hgt.innovation < -deadzone_start) {
+			if (baro_hgt.innovation <= -deadzone_end) {
+				baro_hgt.innovation += deadzone_end;
 
 			} else {
-				_baro_hgt_innov = -deadzone_start;
+				baro_hgt.innovation = -deadzone_start;
 			}
 		}
 	}
 
-	// innovation gate size
-	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(baro_hgt, innov_gate);
 
-	// observation variance - user parameter defined
-	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	// special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && baro_hgt.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(baro_hgt.innovation_variance);
+		baro_hgt.innovation = math::constrain(baro_hgt.innovation, -innov_limit, innov_limit);
+		baro_hgt.innovation_rejected = false;
+	}
 
-	fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var,
-			     _baro_hgt_innov_var, _baro_hgt_test_ratio);
+	baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
+
+	baro_hgt.timestamp_sample = baro_sample.time_us;
+}
+
+void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
+{
+	if (baro_hgt.fusion_enabled
+	    && !baro_hgt.innovation_rejected
+	    && fuseVelPosHeight(baro_hgt.innovation, baro_hgt.innovation_variance, 5)) {
+
+		baro_hgt.fused = true;
+		baro_hgt.time_last_fuse = _time_last_imu;
+	}
 }
 
 void Ekf::fuseRngHgt()