ekf2: auxiliary position fusion

Co-authored-by: Daniel Agar <daniel@agar.ca>

boards/px4/sitl/default.px4board

@@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

msg/EstimatorAidSource2d.msg

@@ -17,6 +17,6 @@ float32[2] test_ratio
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
-# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
+# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
 # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
 # TOPICS estimator_aid_src_drag

msg/EstimatorStatusFlags.msg

@@ -42,6 +42,7 @@ bool cs_gravity_vector          # 34 - true when gravity vector measurements are
 bool cs_mag                     # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 bool cs_ev_yaw_fault            # 36 - true when the EV heading has been declared faulty and is no longer being used
 bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag data is declared consistent with the filter
+bool cs_aux_gpos                # 38 - true if auxiliary global position measurement fusion is intended
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/VehicleGlobalPosition.msg

@@ -27,3 +27,4 @@ bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
 # TOPICS estimator_global_position
+# TOPICS aux_global_position

src/modules/ekf2/CMakeLists.txt

@@ -139,6 +139,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
+	list(APPEND EKF_SRCS EKF/aux_global_position.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -56,6 +56,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
+	list(APPEND EKF_SRCS aux_global_position.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/aux_global_position.cpp

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "ekf.h"
+
+#include "aux_global_position.hpp"
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+
+void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
+{
+
+#if defined(MODULE_NAME)
+
+	if (_aux_global_position_sub.updated()) {
+
+		vehicle_global_position_s aux_global_position{};
+		_aux_global_position_sub.copy(&aux_global_position);
+
+		const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
+
+		AuxGlobalPositionSample sample{};
+		sample.time_us = time_us;
+		sample.latitude = aux_global_position.lat;
+		sample.longitude = aux_global_position.lon;
+		sample.altitude_amsl = aux_global_position.alt;
+		sample.eph = aux_global_position.eph;
+		sample.epv = aux_global_position.epv;
+
+		_aux_global_position_buffer.push(sample);
+
+		_time_last_buffer_push = imu_delayed.time_us;
+	}
+
+#endif // MODULE_NAME
+
+	AuxGlobalPositionSample sample;
+
+	if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
+
+		if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::HPOS))) {
+			return;
+		}
+
+		estimator_aid_source2d_s aid_src{};
+		Vector2f position;
+
+		if (ekf.global_origin_valid()) {
+			position = ekf.global_origin().project(sample.latitude, sample.longitude);
+			//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
+			// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
+			float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get());
+			const float pos_var = sq(pos_noise);
+			const Vector2f pos_obs_var(pos_var, pos_var);
+			ekf.updateHorizontalPositionAidSrcStatus(sample.time_us,
+					position,                                   // observation
+					pos_obs_var,                                // observation variance
+					math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate
+					aid_src);
+		}
+
+		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
+						   && ekf.control_status_flags().yaw_align;
+		const bool continuing_conditions = starting_conditions
+						   && ekf.global_origin_valid();
+
+		switch (_state) {
+		case State::stopped:
+		/* FALLTHROUGH */
+		case State::starting:
+			if (starting_conditions) {
+				_state = State::starting;
+
+				if (ekf.global_origin_valid()) {
+					ekf.enableControlStatusAuxGpos();
+					_state = State::active;
+
+				} else {
+					// Try to initialize using measurement
+					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
+						ekf.enableControlStatusAuxGpos();
+						_state = State::active;
+					}
+				}
+			}
+			break;
+
+		case State::active:
+			if (continuing_conditions) {
+				ekf.fuseHorizontalPosition(aid_src);
+
+			} else {
+				ekf.disableControlStatusAuxGpos();
+				_state = State::stopped;
+			}
+			break;
+
+		default:
+			break;
+		}
+
+#if defined(MODULE_NAME)
+		aid_src.timestamp = hrt_absolute_time();
+		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
+#endif // MODULE_NAME
+	} else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
+		ekf.disableControlStatusAuxGpos();
+		_state = State::stopped;
+		ECL_WARN("Aux global position data stopped");
+	}
+}
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

src/modules/ekf2/EKF/aux_global_position.hpp

@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_AUX_GLOBAL_POSITION_HPP
+#define EKF_AUX_GLOBAL_POSITION_HPP
+
+// interface?
+//  - ModuleParams
+//  - Base class EKF
+//  -  bool update(imu)
+//   how to get delay?
+//  WelfordMean for init?
+//  WelfordMean for rate
+
+#include "common.h"
+#include "RingBuffer.h"
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+
+#if defined(MODULE_NAME)
+# include <px4_platform_common/module_params.h>
+# include <uORB/PublicationMulti.hpp>
+# include <uORB/Subscription.hpp>
+# include <uORB/topics/estimator_aid_source2d.h>
+# include <uORB/topics/vehicle_global_position.h>
+#endif // MODULE_NAME
+
+class Ekf;
+
+class AuxGlobalPosition : public ModuleParams
+{
+public:
+	AuxGlobalPosition() : ModuleParams(nullptr) {}
+	~AuxGlobalPosition() = default;
+
+	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
+
+	void updateParameters()
+	{
+		updateParams();
+	}
+
+private:
+	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
+	{
+		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
+	}
+
+	struct AuxGlobalPositionSample {
+		uint64_t time_us{};     ///< timestamp of the measurement (uSec)
+		double latitude{};
+		double longitude{};
+		float altitude_amsl{};
+		float eph{};
+		float epv{};
+	};
+
+	RingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
+	uint64_t _time_last_buffer_push{0};
+
+	enum Ctrl : uint8_t {
+		HPOS  = (1<<0),
+		VPOS  = (1<<1)
+	};
+
+	enum class State {
+		stopped,
+		starting,
+		active,
+	};
+
+	State _state{State::stopped};
+
+#if defined(MODULE_NAME)
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)};
+	uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::EKF2_AGP_CTRL>) _param_ekf2_agp_ctrl,
+		(ParamFloat<px4::params::EKF2_AGP_DELAY>) _param_ekf2_agp_delay,
+		(ParamFloat<px4::params::EKF2_AGP_NOISE>) _param_ekf2_agp_noise,
+		(ParamFloat<px4::params::EKF2_AGP_GATE>) _param_ekf2_agp_gate
+	)
+
+#endif // MODULE_NAME
+};
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
+#endif // !EKF_AUX_GLOBAL_POSITION_HPP

src/modules/ekf2/EKF/common.h

@@ -614,6 +614,7 @@ union filter_control_status_u {
 		uint64_t mag                     : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 		uint64_t ev_yaw_fault            : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
 		uint64_t mag_heading_consistent  : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
+		uint64_t aux_gpos                : 1;
 
 	} flags;
 	uint64_t value;

src/modules/ekf2/EKF/control.cpp

@@ -115,6 +115,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlGpsFusion(imu_delayed);
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	_aux_global_position.update(*this, imu_delayed);
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
 #if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);
 #endif // CONFIG_EKF2_AIRSPEED

src/modules/ekf2/EKF/ekf.cpp

@@ -340,3 +340,10 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
+
+void Ekf::updateParameters()
+{
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	_aux_global_position.updateParameters();
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+}

src/modules/ekf2/EKF/ekf.h

@@ -62,6 +62,10 @@
 #include "aid_sources/ZeroGyroUpdate.hpp"
 #include "aid_sources/ZeroVelocityUpdate.hpp"
 
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
+# include "aux_global_position.hpp"
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
 enum class Likelihood { LOW, MEDIUM, HIGH };
 
 class Ekf final : public EstimatorInterface
@@ -255,7 +259,7 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -467,7 +471,6 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-
 	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
 	{
 		clearInhibitedStateKalmanGains(K);
@@ -498,6 +501,10 @@ public:
 		return is_healthy;
 	}
 
+	void updateParameters();
+
+	friend class AuxGlobalPosition;
+
 private:
 
 	// set the internal states and status to their default value
@@ -1228,6 +1235,10 @@ private:
 
 	ZeroGyroUpdate _zero_gyro_update{};
 	ZeroVelocityUpdate _zero_velocity_update{};
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	AuxGlobalPosition _aux_global_position{};
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
 };
 
 #endif // !EKF_EKF_H

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -302,7 +302,7 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }
 
-bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
+bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, const float epv)
 {
 	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
 	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
@@ -339,9 +339,8 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-		// We don't know the uncertainty of the origin
-		_gpos_origin_eph = 0.f;
-		_gpos_origin_epv = 0.f;
+		_gpos_origin_eph = eph;
+		_gpos_origin_epv = epv;
 
 		_NED_origin_initialised = true;
 
@@ -789,7 +788,7 @@ void Ekf::updateDeadReckoningStatus()
 
 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
+	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -656,6 +656,7 @@ int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
 	       + int(_control_status.flags.opt_flow)
 	       + int(_control_status.flags.ev_pos)
 	       + int(_control_status.flags.ev_vel)
+	       + int(_control_status.flags.aux_gpos)
 	       // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning
 	       // and so is treated as a single aiding source.
 	       + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);

src/modules/ekf2/EKF/estimator_interface.h

@@ -284,6 +284,9 @@ public:
 	const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
 	const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
 
+	void enableControlStatusAuxGpos() { _control_status.flags.aux_gpos = true; }
+	void disableControlStatusAuxGpos() { _control_status.flags.aux_gpos = false; }
+
 	// get EKF internal fault status
 	const fault_status_u &fault_status() const { return _fault_status; }
 	const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }

src/modules/ekf2/EKF2.cpp

@@ -511,6 +511,8 @@ void EKF2::Run()
 		}
 
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+		_ekf.updateParameters();
 	}
 
 	if (!_callback_registered) {
@@ -1896,6 +1898,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag                     = _ekf.control_status_flags().mag;
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
+		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
 
 		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/Kconfig

@@ -22,6 +22,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 airspeed fusion support.
 
+menuconfig EKF2_AUX_GLOBAL_POSITION
+depends on MODULES_EKF2
+	bool "aux global position fusion support"
+	default n
+	---help---
+		EKF2 auxiliary global position fusion support.
+
 menuconfig EKF2_AUXVEL
 depends on MODULES_EKF2
         bool "aux velocity fusion support"

src/modules/ekf2/ekf2_params.c

@@ -1537,3 +1537,51 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
  * @decimal 1
  */
 PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
+
+/**
+ * Aux global position (AGP) sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Horizontal position fusion
+ * 1 : Vertical position fusion
+ *
+ * @group EKF2
+ * @min 0
+ * @max 3
+ * @bit 0 Horizontal position
+ * @bit 1 Vertical position
+ */
+PARAM_DEFINE_INT32(EKF2_AGP_CTRL, 1);
+
+/**
+ * Aux global position estimator delay relative to IMU measurements
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ * @reboot_required true
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_AGP_DELAY, 0);
+
+/**
+ * Measurement noise for aux global position observations used to lower bound or replace the uncertainty included in the message
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_AGP_NOISE, 0.9f);
+
+/**
+ * Gate size for aux global position fusion
+ *
+ * Sets the number of standard deviations used by the innovation consistency test.
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+*/
+PARAM_DEFINE_FLOAT(EKF2_AGP_GATE, 3.0f);

src/modules/logger/logged_topics.cpp

@@ -203,6 +203,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+	//add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES);
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
 	add_topic_multi("battery_status", 200, 2);
@@ -220,6 +221,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
 	add_topic("vehicle_optical_flow", 500);
+	add_topic("aux_global_position", 500);
 	//add_optional_topic("vehicle_optical_flow_vel", 100);
 	add_optional_topic("pps_capture");
 
@@ -342,6 +344,7 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("vehicle_magnetometer");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
+	add_topic("aux_global_position");
 	add_topic_multi("distance_sensor");
 }
 

src/modules/replay/ReplayEkf2.cpp

@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
@@ -104,6 +105,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 
 	} else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) {
 		_vehicle_visual_odometry_msg_id = msg_id;
+
+	} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
+		_aux_global_position_msg_id = msg_id;
 	}
 
 	// the main loop should only handle publication of the following topics, the sensor topics are
@@ -130,6 +134,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
 	handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
 	handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
 	handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id);
+	handle_sensor_publication(0, _aux_global_position_msg_id);
 
 	// sensor_combined: publish last because ekf2 is polling on this
 	if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
@@ -214,6 +219,7 @@ ReplayEkf2::onExitMainLoop()
 	print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
 	print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
 	print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
+	print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position");
 }
 
 } // namespace px4

src/modules/replay/ReplayEkf2.hpp

@@ -88,6 +88,7 @@ private:
 	uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
 	uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
 	uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid;
+	uint16_t _aux_global_position_msg_id = msg_id_invalid;
 };
 
 } //namespace px4

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -151,5 +151,8 @@ subscriptions:
   - topic: /fmu/in/actuator_servos
     type: px4_msgs::msg::ActuatorServos
 
+  - topic: /fmu/in/aux_global_position
+    type: px4_msgs::msg::VehicleGlobalPosition
+
 # Create uORB::PublicationMulti
 subscriptions_multi: