ekf2: add vertical dead-reckoning timeout check for height validity

src/modules/ekf2/EKF/control.cpp

@@ -181,7 +181,7 @@ void Ekf::controlFusionModes()
 	controlFakeHgtFusion();
 
 	// check if we are no longer fusing measurements that directly constrain velocity drift
-	update_deadreckoning_status();
+	updateDeadReckoningStatus();
 }
 
 void Ekf::controlExternalVisionFusion()

src/modules/ekf2/EKF/ekf.h

@@ -272,7 +272,17 @@ public:
 	// return true if the local position estimate is valid
 	bool local_position_is_valid() const
 	{
-		return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos);
+		return (!_horizontal_deadreckon_time_exceeded && !_control_status.flags.fake_pos);
+	}
+
+	bool isLocalVerticalPositionValid() const
+	{
+		return !_vertical_position_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
+	}
+
+	bool isLocalVerticalVelocityValid() const
+	{
+		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
 	}
 
 	bool isTerrainEstimateValid() const { return _hagl_valid; };
@@ -430,7 +440,9 @@ private:
 	bool initialiseTilt();
 
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
-	void update_deadreckoning_status();
+	void updateDeadReckoningStatus();
+	void updateHorizontalDeadReckoningstatus();
+	void updateVerticalDeadReckoningStatus();
 
 	void updateTerrainValidity();
 
@@ -470,7 +482,9 @@ private:
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
-	uint64_t _time_last_aiding{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
+	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
+	uint64_t _time_last_v_pos_aiding{0};
+	uint64_t _time_last_v_vel_aiding{0};
 
 	uint64_t _time_last_hor_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
 	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of vertical position measurements was performed (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -731,7 +731,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
-	if (_deadreckon_time_exceeded && _control_status.flags.gps) {
+	if (_horizontal_deadreckon_time_exceeded && _control_status.flags.gps) {
 		hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 	}
 
@@ -747,7 +747,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
-	if (_deadreckon_time_exceeded) {
+	if (_horizontal_deadreckon_time_exceeded) {
 		float vel_err_conservative = 0.0f;
 
 		if (_control_status.flags.opt_flow) {
@@ -1016,8 +1016,13 @@ void Ekf::uncorrelateQuatFromOtherStates()
 	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
 }
 
-// return true if we are totally reliant on inertial dead-reckoning for position
-void Ekf::update_deadreckoning_status()
+void Ekf::updateDeadReckoningStatus()
+{
+	updateHorizontalDeadReckoningstatus();
+	updateVerticalDeadReckoningStatus();
+}
+
+void Ekf::updateHorizontalDeadReckoningstatus()
 {
 	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
@@ -1034,20 +1039,40 @@ void Ekf::update_deadreckoning_status()
 
 	if (!_control_status.flags.inertial_dead_reckoning) {
 		if (_imu_sample_delayed.time_us > _params.no_aid_timeout_max) {
-			_time_last_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max;
+			_time_last_horizontal_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max;
 		}
 	}
 
 	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	bool deadreckon_time_exceeded = (_time_last_aiding == 0)
-				    || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max);
+	bool deadreckon_time_exceeded = (_time_last_horizontal_aiding == 0)
+				    || isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
 
-	if (!_deadreckon_time_exceeded && deadreckon_time_exceeded) {
+	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
 		// deadreckon time now exceeded
 		ECL_WARN("dead reckon time exceeded");
 	}
 
-	_deadreckon_time_exceeded = deadreckon_time_exceeded;
+	_horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded;
+}
+
+void Ekf::updateVerticalDeadReckoningStatus()
+{
+	if (isVerticalPositionAidingActive()) {
+		_time_last_v_pos_aiding = _time_last_hgt_fuse;
+		_vertical_position_deadreckon_time_exceeded = false;
+
+	} else if ((_time_last_v_pos_aiding == 0) || isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
+		_vertical_position_deadreckon_time_exceeded = true;
+	}
+
+	if (isVerticalVelocityAidingActive()) {
+		_time_last_v_vel_aiding = _time_last_ver_vel_fuse;
+		_vertical_velocity_deadreckon_time_exceeded = false;
+
+	} else if (((_time_last_v_vel_aiding == 0) || isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max))
+		   && _vertical_position_deadreckon_time_exceeded) {
+		_vertical_velocity_deadreckon_time_exceeded = true;
+	}
 }
 
 // calculate the variances for the rotation vector equivalent

src/modules/ekf2/EKF/estimator_interface.h

@@ -349,7 +349,9 @@ protected:
 	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
 	innovation_fault_status_u _innov_check_fail_status{};
 
-	bool _deadreckon_time_exceeded{true};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
+	bool _horizontal_deadreckon_time_exceeded{true};
+	bool _vertical_position_deadreckon_time_exceeded{true};
+	bool _vertical_velocity_deadreckon_time_exceeded{true};
 
 	float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
 	float _gps_vertical_position_drift_rate_m_s{NAN};   // Vertical position drift rate (m/s)

src/modules/ekf2/EKF2.cpp

@@ -1098,9 +1098,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	// TODO: better status reporting
 	lpos.xy_valid = _ekf.local_position_is_valid();
-	lpos.z_valid = !_preflt_checker.hasVertFailed();
+	lpos.z_valid = _ekf.isLocalVerticalPositionValid();
 	lpos.v_xy_valid = _ekf.local_position_is_valid();
-	lpos.v_z_valid = !_preflt_checker.hasVertFailed();
+	lpos.v_z_valid = _ekf.isLocalVerticalVelocityValid();
 
 	// Position of local NED origin in GPS / WGS84 frame
 	if (_ekf.global_origin_valid()) {