ekf2-fake pos: add valid fake position fusion

This is similar to fake pos but is only used when the ekf has an
external information telling it that the vehicle is not changing
position. This information can then be used to keep a valid local
position even when the vehicle isn't exactly at rest.

msg/EstimatorStatusFlags.msg

@@ -45,6 +45,8 @@ bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag d
 bool cs_aux_gpos                # 38 - true if auxiliary global position measurement fusion is intended
 bool cs_rng_terrain             # 39 - true if we are fusing range finder data for terrain
 bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain
+bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
+bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -75,55 +75,20 @@ void Ekf::controlFakePosFusion()
 				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 				      innov_gate);                                        // innovation gate
 
-		const bool enable_conditions_passing = !isHorizontalAidingActive()
-						       && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-						       && (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
-						       && _horizontal_deadreckon_time_exceeded;
+		const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
+		const bool enable_fake_pos = !enable_valid_fake_pos
+					     && (getTiltVariance() > sq(math::radians(3.f)))
+					     && !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
+					     && _horizontal_deadreckon_time_exceeded;
 
-		if (_control_status.flags.fake_pos) {
-			if (enable_conditions_passing) {
+		_control_status.flags.fake_pos = runFakePosStateMachine(enable_fake_pos, _control_status.flags.fake_pos, aid_src);
+		_control_status.flags.valid_fake_pos = runFakePosStateMachine(enable_valid_fake_pos,
+						       _control_status.flags.valid_fake_pos, aid_src);
 
-				// always protect against extreme values that could result in a NaN
-				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
-				    && (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
-				   ) {
-					if (!aid_src.innovation_rejected
-					    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
-									  State::pos.idx + 0)
-					    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
-									  State::pos.idx + 1)
-					   ) {
-						aid_src.fused = true;
-						aid_src.time_last_fuse = _time_delayed_us;
-					}
-				}
-
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
-
-				if (is_fusion_failing) {
-					ECL_WARN("fake position fusion failing, resetting");
-					resetFakePosFusion();
-				}
-
-			} else {
-				stopFakePosFusion();
-			}
-
-		} else {
-			if (enable_conditions_passing) {
-				ECL_INFO("start fake position fusion");
-				_control_status.flags.fake_pos = true;
-				resetFakePosFusion();
-
-				if (_control_status.flags.tilt_align) {
-					// The fake position fusion is not started for initial alignement
-					ECL_WARN("stopping navigation");
-				}
-			}
-		}
-
-	} else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) {
-		stopFakePosFusion();
+	} else if ((_control_status.flags.fake_pos || _control_status.flags.valid_fake_pos) && isHorizontalAidingActive()) {
+		ECL_INFO("stop fake position fusion");
+		_control_status.flags.fake_pos = false;
+		_control_status.flags.valid_fake_pos = false;
 	}
 }
 
@@ -138,10 +103,41 @@ void Ekf::resetFakePosFusion()
 	_aid_src_fake_pos.time_last_fuse = _time_delayed_us;
 }
 
-void Ekf::stopFakePosFusion()
+bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool status_flag,
+				 estimator_aid_source2d_s &aid_src)
 {
-	if (_control_status.flags.fake_pos) {
-		ECL_INFO("stop fake position fusion");
-		_control_status.flags.fake_pos = false;
+	if (status_flag) {
+		if (enable_conditions_passing) {
+			if (!aid_src.innovation_rejected
+			    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+							  State::pos.idx + 0)
+			    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+							  State::pos.idx + 1)
+			   ) {
+				aid_src.fused = true;
+				aid_src.time_last_fuse = _time_delayed_us;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
+
+			if (is_fusion_failing) {
+				ECL_WARN("fake position fusion failing, resetting");
+				resetFakePosFusion();
+			}
+
+		} else {
+			ECL_INFO("stop fake position fusion");
+			status_flag = false;
+		}
+
+	} else {
+		if (enable_conditions_passing) {
+			ECL_INFO("start fake position fusion");
+			status_flag = true;
+
+			resetFakePosFusion();
+		}
 	}
+
+	return status_flag;
 }

src/modules/ekf2/EKF/common.h

@@ -261,6 +261,7 @@ struct systemFlagUpdate {
 	bool in_air{true};
 	bool is_fixed_wing{false};
 	bool gnd_effect{false};
+	bool constant_pos{false};
 };
 
 struct parameters {
@@ -614,6 +615,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t aux_gpos                : 1; ///< 38 - true if auxiliary global position measurement fusion is intended
 		uint64_t rng_terrain             : 1; ///< 39 - true if we are fusing range finder data for terrain
 		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
+		uint64_t valid_fake_pos          : 1; ///< 41 - true if a valid constant position is being fused
+		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 
 	} flags;
 	uint64_t value;

src/modules/ekf2/EKF/control.cpp

@@ -62,6 +62,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 			if (system_flags_delayed.gnd_effect) {
 				set_gnd_effect();
 			}
+
+			set_constant_pos(system_flags_delayed.constant_pos);
 		}
 	}
 

src/modules/ekf2/EKF/ekf.h

@@ -1132,7 +1132,7 @@ private:
 	}
 
 	void resetFakePosFusion();
-	void stopFakePosFusion();
+	bool runFakePosStateMachine(bool enable_condition_passing, bool status_flag, estimator_aid_source2d_s &aid_src);
 
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -642,7 +642,8 @@ uint16_t Ekf::get_ekf_soln_status() const
 #endif // CONFIG_EKF2_TERRAIN
 
 	// 128	ESTIMATOR_CONST_POS_MODE	True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
-	soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest;
+	soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos
+					   || _control_status.flags.vehicle_at_rest;
 
 	// 256	ESTIMATOR_PRED_POS_HORIZ_REL	True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
 	soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
@@ -793,6 +794,13 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 		}
 	}
 
+	if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) {
+		// only respect as a valid aiding source now if we expect to have another valid source once in air
+		if (aiding_expected_in_air) {
+			inertial_dead_reckoning = false;
+		}
+	}
+
 	if (inertial_dead_reckoning) {
 		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
 			// deadreckon time exceeded

src/modules/ekf2/EKF/estimator_interface.h

@@ -179,6 +179,8 @@ public:
 		_control_status.flags.vehicle_at_rest = at_rest;
 	}
 
+	void set_constant_pos(bool constant_pos) { _control_status.flags.constant_pos = constant_pos; }
+
 	// return true if the attitude is usable
 	bool attitude_valid() const { return _control_status.flags.tilt_align; }
 

src/modules/ekf2/EKF2.cpp

@@ -1925,6 +1925,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
 		status_flags.cs_rng_terrain    = _ekf.control_status_flags().rng_terrain;
 		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
+		status_flags.cs_valid_fake_pos      = _ekf.control_status_flags().valid_fake_pos;
+		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -2592,6 +2594,15 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 			flags.gnd_effect = vehicle_land_detected.in_ground_effect;
 		}
 
+		launch_detection_status_s launch_detection_status;
+
+		if (_launch_detection_status_sub.copy(&launch_detection_status)
+		    && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) {
+
+			flags.constant_pos = (launch_detection_status.launch_detection_state ==
+					      launch_detection_status_s::STATE_WAITING_FOR_LAUNCH);
+		}
+
 		_ekf.setSystemFlagData(flags);
 	}
 }

src/modules/ekf2/EKF2.hpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_status_flags.h>
+#include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -388,6 +389,7 @@ private:
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)};
 
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};

src/modules/ekf2/test/CMakeLists.txt

@@ -40,6 +40,7 @@ px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_si
 px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_fake_pos.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)

src/modules/ekf2/test/sensor_simulator/airspeed.cpp

@@ -15,13 +15,18 @@ Airspeed::~Airspeed()
 
 void Airspeed::send(uint64_t time)
 {
-	if (_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) {
-		airspeedSample airspeed_sample;
-		airspeed_sample.time_us = time;
-		airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data;
-		airspeed_sample.true_airspeed = _true_airspeed_data;
-		_ekf->setAirspeedData(airspeed_sample);
+	float ias2tas = 1.f;
+
+	if (PX4_ISFINITE(_indicated_airspeed_data)
+	    && (_indicated_airspeed_data > FLT_EPSILON)) {
+		ias2tas = _true_airspeed_data / _indicated_airspeed_data;
 	}
+
+	airspeedSample airspeed_sample;
+	airspeed_sample.time_us = time;
+	airspeed_sample.eas2tas = ias2tas;
+	airspeed_sample.true_airspeed = _true_airspeed_data;
+	_ekf->setAirspeedData(airspeed_sample);
 }
 
 void Airspeed::setData(float true_airspeed, float indicated_airspeed)

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -78,7 +78,6 @@ public:
 
 TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
 {
-
 	const Vector3f simulated_velocity_earth(0.0f, 1.5f, 0.0f);
 	const Vector2f airspeed_body(2.4f, 0.0f);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
@@ -86,6 +85,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
 	_sensor_simulator._vio.setVelocityFrameToLocalNED();
 	_sensor_simulator.startExternalVision();
 
+	// Let the EV fusion start first to reset the velocity estimate
+	_sensor_simulator.runSeconds(0.5);
+
 	_ekf->set_in_air_status(true);
 	_ekf->set_vehicle_at_rest(false);
 	_ekf->set_is_fixed_wing(true);

src/modules/ekf2/test/test_EKF_fake_pos.cpp

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "sensor_simulator/sensor_simulator.h"
+#include "sensor_simulator/ekf_wrapper.h"
+
+
+class EkfFakePosTest : public ::testing::Test
+{
+public:
+
+	EkfFakePosTest(): ::testing::Test(),
+		_ekf{std::make_shared<Ekf>()},
+		_sensor_simulator(_ekf),
+		_ekf_wrapper(_ekf),
+		_quat_sim(Eulerf(0.0f, 0.0f, math::radians(45.0f))) {};
+
+	std::shared_ptr<Ekf> _ekf;
+	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+	const Quatf _quat_sim;
+
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
+		_ekf->init(0);
+		_sensor_simulator.runSeconds(0.1);
+		_ekf->set_in_air_status(false);
+		_ekf->set_vehicle_at_rest(true);
+
+		_sensor_simulator.simulateOrientation(_quat_sim);
+		_sensor_simulator.runSeconds(7);
+	}
+
+	// Use this method to clean up any memory, network etc. after each test
+	void TearDown() override
+	{
+	}
+};
+
+TEST_F(EkfFakePosTest, testValidFakePos)
+{
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_constant_pos(true);
+	_sensor_simulator.runSeconds(1);
+
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos);
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos);
+}
+
+TEST_F(EkfFakePosTest, testFakePosStopGnss)
+{
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_constant_pos(true);
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(12);
+
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().valid_fake_pos);
+}
+
+TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos)
+{
+	_ekf->set_is_fixed_wing(true);
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(-0.01f, 0.f); // airspeed close to 0
+	_ekf_wrapper.enableBetaFusion();
+
+	// WHEN: the vehicle is not as rest but is known to be at a constant position
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_constant_pos(true);
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the valid fake position is fused
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos);
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos);
+
+	// AND: since airspeed is expected to provide wind-relative dead-reckoning after takeoff
+	// the local position is considered valid
+	_sensor_simulator.runSeconds(60);
+
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos);
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+}