ekf2: add fake height fusion logic

When there is no vertical aiding, fake height is started to constrain
the vertical channel of the EKF

msg/estimator_aid_source_1d.msg

@@ -20,4 +20,5 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_1d
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed
+# TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw

msg/estimator_status_flags.msg

@@ -37,6 +37,7 @@ bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measureme
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 bool cs_fake_pos                # 32 - true when fake position measurements are being fused
+bool cs_fake_hgt                # 33 - true when fake height measurements are being fused
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/CMakeLists.txt

@@ -59,6 +59,7 @@ px4_add_module(
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
 		EKF/ev_height_control.cpp
+		EKF/fake_height_control.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gnss_height_control.cpp
 		EKF/gps_checks.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -43,6 +43,7 @@ add_library(ecl_EKF
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
 	ev_height_control.cpp
+	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp
 	gps_checks.cpp

src/modules/ekf2/EKF/common.h

@@ -533,6 +533,7 @@ union filter_control_status_u {
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
 		uint64_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
 		uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
+		uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -190,6 +190,7 @@ void Ekf::controlFusionModes()
 
 	// Fake position measurement for constraining drift when no other velocity or position measurements
 	controlFakePosFusion();
+	controlFakeHgtFusion();
 
 	// check if we are no longer fusing measurements that directly constrain velocity drift
 	update_deadreckoning_status();
@@ -521,7 +522,7 @@ void Ekf::controlOpticalFlowFusion()
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
 				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
 					fuseOptFlow();
-					_last_known_posNE = _state.pos.xy();
+					_last_known_pos.xy() = _state.pos.xy();
 				}
 
 				_flow_data_ready = false;

src/modules/ekf2/EKF/ekf.h

@@ -402,6 +402,7 @@ public:
 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
+	const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
 	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
@@ -479,7 +480,7 @@ private:
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
-	Vector2f _last_known_posNE{};		///< last known local NE position vector (m)
+	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
@@ -549,6 +550,7 @@ private:
 	estimator_aid_source_1d_s _aid_src_airspeed{};
 
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
+	estimator_aid_source_1d_s _aid_src_fake_hgt{};
 
 	estimator_aid_source_1d_s _aid_src_ev_yaw{};
 
@@ -932,6 +934,13 @@ private:
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
+	void controlFakeHgtFusion();
+	void startFakeHgtFusion();
+	void resetFakeHgtFusion();
+	void resetHeightToLastKnown();
+	void stopFakeHgtFusion();
+	void fuseFakeHgt();
+
 	void controlZeroVelocityUpdate();
 
 	void controlZeroInnovationHeadingUpdate();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -179,7 +179,7 @@ void Ekf::resetHorizontalPositionToOpticalFlow()
 		resetHorizontalPositionTo(Vector2f(0.f, 0.f));
 
 	} else {
-		resetHorizontalPositionTo(_last_known_posNE);
+		resetHorizontalPositionTo(_last_known_pos.xy());
 	}
 
 	// estimate is relative to initial position in this mode, so we start with zero error.
@@ -191,7 +191,7 @@ void Ekf::resetHorizontalPositionToLastKnown()
 	_information_events.flags.reset_pos_to_last_known = true;
 	ECL_INFO("reset position to last known position");
 	// Used when falling back to non-aiding mode of operation
-	resetHorizontalPositionTo(_last_known_posNE);
+	resetHorizontalPositionTo(_last_known_pos.xy());
 	P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
 }
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -584,6 +584,11 @@ int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
 	       + int(_control_status.flags.ev_hgt);
 }
 
+bool EstimatorInterface::isVerticalAidingActive() const
+{
+	return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive();
+}
+
 bool EstimatorInterface::isVerticalVelocityAidingActive() const
 {
 	return getNumberOfActiveVerticalVelocityAidingSources() > 0;

src/modules/ekf2/EKF/estimator_interface.h

@@ -178,6 +178,7 @@ public:
 	// Return true if at least one source of horizontal aiding is active
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isHorizontalAidingActive() const;
+	bool isVerticalAidingActive() const;
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 

src/modules/ekf2/EKF/fake_height_control.cpp

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 fake_height_control.cpp
+ * Control functions for ekf fake height fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlFakeHgtFusion()
+{
+	auto &fake_hgt = _aid_src_fake_hgt;
+
+	// clear
+	resetEstimatorAidStatusFlags(fake_hgt);
+
+	// If we aren't doing any aiding, fake position measurements at the last known vertical position to constrain drift
+	const bool fake_hgt_data_ready = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)2e5); // Fuse fake height at a limited rate
+
+	if (fake_hgt_data_ready) {
+		const bool continuing_conditions_passing = !isVerticalAidingActive();
+		const bool starting_conditions_passing = continuing_conditions_passing;
+
+		if (_control_status.flags.fake_hgt) {
+			if (continuing_conditions_passing) {
+				fuseFakeHgt();
+
+				const bool is_fusion_failing = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)4e5);
+
+				if (is_fusion_failing) {
+					resetFakeHgtFusion();
+				}
+
+			} else {
+				stopFakeHgtFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				startFakeHgtFusion();
+			}
+		}
+	}
+}
+
+void Ekf::startFakeHgtFusion()
+{
+	if (!_control_status.flags.fake_hgt) {
+		ECL_INFO("start fake height fusion");
+		_control_status.flags.fake_hgt = true;
+		resetFakeHgtFusion();
+	}
+}
+
+void Ekf::resetFakeHgtFusion()
+{
+	ECL_INFO("reset fake height fusion");
+	_last_known_pos(2) = _state.pos(2);
+
+	resetVerticalVelocityToZero();
+	resetHeightToLastKnown();
+
+	_aid_src_fake_hgt.time_last_fuse = _imu_sample_delayed.time_us;
+}
+
+void Ekf::resetHeightToLastKnown()
+{
+	_information_events.flags.reset_pos_to_last_known = true;
+	ECL_INFO("reset height to last known");
+	resetVerticalPositionTo(_last_known_pos(2));
+	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.pos_noaid_noise));
+}
+
+void Ekf::stopFakeHgtFusion()
+{
+	if (_control_status.flags.fake_hgt) {
+		ECL_INFO("stop fake height fusion");
+		_control_status.flags.fake_hgt = false;
+
+		resetEstimatorAidStatus(_aid_src_fake_hgt);
+	}
+}
+
+void Ekf::fuseFakeHgt()
+{
+	const float obs_var = sq(_params.pos_noaid_noise);
+
+	const float innov_gate = 3.f;
+
+	auto &fake_hgt = _aid_src_fake_hgt;
+
+	fake_hgt.observation = _last_known_pos(2);
+	fake_hgt.observation_variance = obs_var;
+
+	fake_hgt.innovation = _state.pos(2) - _last_known_pos(2);
+	fake_hgt.innovation_variance = P(9, 9) + obs_var;
+
+	setEstimatorAidStatusTestRatio(fake_hgt, innov_gate);
+
+	// always protect against extreme values that could result in a NaN
+	fake_hgt.fusion_enabled = fake_hgt.test_ratio < sq(100.0f / innov_gate);
+
+	// fuse
+	if (fake_hgt.fusion_enabled && !fake_hgt.innovation_rejected) {
+		if (fuseVelPosHeight(fake_hgt.innovation, fake_hgt.innovation_variance, 5)) {
+			fake_hgt.fused = true;
+			fake_hgt.time_last_fuse = _imu_sample_delayed.time_us;
+		}
+	}
+
+	fake_hgt.timestamp_sample = _imu_sample_delayed.time_us;
+}

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -94,7 +94,7 @@ void Ekf::startFakePosFusion()
 void Ekf::resetFakePosFusion()
 {
 	ECL_INFO("reset fake position fusion");
-	_last_known_posNE = _state.pos.xy();
+	_last_known_pos.xy() = _state.pos.xy();
 
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();
@@ -134,10 +134,10 @@ void Ekf::fuseFakePosition()
 	auto &fake_pos = _aid_src_fake_pos;
 
 	for (int i = 0; i < 2; i++) {
-		fake_pos.observation[i] = _last_known_posNE(i);
+		fake_pos.observation[i] = _last_known_pos(i);
 		fake_pos.observation_variance[i] = obs_var(i);
 
-		fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i);
+		fake_pos.innovation[i] = _state.pos(i) - _last_known_pos(i);
 		fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
 	}
 

src/modules/ekf2/EKF2.cpp

@@ -722,6 +722,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 
 	// fake position
 	PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
+	PublishAidSourceStatus(_ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub);
 
 	// EV yaw
 	PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
@@ -1429,6 +1430,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
 		status_flags.cs_fake_pos                = _ekf.control_status_flags().fake_pos;
+		status_flags.cs_fake_hgt                = _ekf.control_status_flags().fake_hgt;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/EKF2.hpp

@@ -263,6 +263,7 @@ private:
 	hrt_abstime _status_baro_hgt_pub_last{0};
 	hrt_abstime _status_rng_hgt_pub_last{0};
 
+	hrt_abstime _status_fake_hgt_pub_last{0};
 	hrt_abstime _status_fake_pos_pub_last{0};
 
 	hrt_abstime _status_ev_yaw_pub_last{0};
@@ -346,6 +347,7 @@ private:
 	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
 	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
 
+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
 	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
 
 	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};

src/modules/logger/logged_topics.cpp

@@ -181,6 +181,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES);
@@ -264,6 +265,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);