ekf2: auxiliary position fusion

Co-authored-by: Daniel Agar <[email protected]>

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 @@ class Ekf final : public EstimatorInterface
 	// 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 @@ class Ekf final : public EstimatorInterface
 	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 @@ class Ekf final : public EstimatorInterface
 		return is_healthy;
 	}
 
+	void updateParameters();
+
+	friend class AuxGlobalPosition;
+
 private:
 
 	// set the internal states and status to their default value
@@ -1228,6 +1235,10 @@ class Ekf final : public EstimatorInterface
 
 	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