feat(ekf2): add ranging beacon fusion support

- Add Symforce-derivation
- No altitude correction
- EKF2 replay
- New params

msg/EstimatorAidSource1d.msg

@@ -21,7 +21,7 @@ float32 test_ratio_filtered  # signed filtered 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_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
+# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt estimator_aid_src_ranging_beacon
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw

src/lib/lat_lon_alt/lat_lon_alt.cpp

@@ -111,6 +111,21 @@ void LatLonAlt::computeRadiiOfCurvature(const double latitude, double &meridian_
 	transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp;
 }
 
+matrix::Dcmf LatLonAlt::computeRotEcefToNed() const
+{
+	const double cos_lat = cos(_latitude_rad);
+	const double sin_lat = sin(_latitude_rad);
+	const double cos_lon = cos(_longitude_rad);
+	const double sin_lon = sin(_longitude_rad);
+
+	const float val[] = {(float)(-sin_lat * cos_lon), (float)(-sin_lat * sin_lon), (float)cos_lat,
+			     (float) - sin_lon, (float)cos_lon, 0.f,
+			     (float)(-cos_lat * cos_lon), (float)(-cos_lat * sin_lon), (float) - sin_lat
+			    };
+
+	return matrix::Dcmf(val);
+}
+
 LatLonAlt LatLonAlt::operator+(const matrix::Vector3f &delta_pos) const
 {
 	const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude());

src/lib/lat_lon_alt/lat_lon_alt.hpp

@@ -103,6 +103,9 @@ public:
 	 */
 	matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;
 
+	// Compute the ECEF to NED coordinate transformation matrix at the current position
+	matrix::Dcmf computeRotEcefToNed() const;
+
 	struct Wgs84 {
 		static constexpr double equatorial_radius = 6378137.0;
 		static constexpr double eccentricity = 0.0818191908425;

src/modules/ekf2/CMakeLists.txt

@@ -220,6 +220,13 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_MODULE_PARAMS params_range_finder.yaml)
 endif()
 
+if(CONFIG_EKF2_RANGING_BEACON)
+	list(APPEND EKF_SRCS
+		EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp
+	)
+	list(APPEND EKF_MODULE_PARAMS params_ranging_beacon.yaml)
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp)
 	list(APPEND EKF_MODULE_PARAMS params_sideslip.yaml)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -125,6 +125,12 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	)
 endif()
 
+if(CONFIG_EKF2_RANGING_BEACON)
+	list(APPEND EKF_SRCS
+		aid_sources/ranging_beacon/ranging_beacon_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS aid_sources/sideslip/sideslip_fusion.cpp)
 endif()

src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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 ranging_beacon_control.cpp
+ * Control functions for EKF ranging beacon fusion
+ */
+
+#include "ekf.h"
+#include <ekf_derivation/generated/compute_range_beacon_innov_var_and_h.h>
+#include <mathlib/mathlib.h>
+
+void Ekf::controlRangingBeaconFusion(const imuSample &imu_delayed)
+{
+	if (!_ranging_beacon_buffer || (_params.ekf2_rngbc_ctrl == 0)) {
+		stopRangingBeaconFusion();
+		return;
+	}
+
+	rangingBeaconSample ranging_beacon_sample_delayed{};
+	const bool ranging_beacon_data_ready = _ranging_beacon_buffer->pop_first_older_than(
+			imu_delayed.time_us, &ranging_beacon_sample_delayed);
+
+	if (ranging_beacon_data_ready) {
+
+		const rangingBeaconSample &sample =  ranging_beacon_sample_delayed;
+
+		const bool measurement_valid = PX4_ISFINITE(sample.range_m)
+					       && (sample.range_m > 0.f)
+					       && PX4_ISFINITE(sample.range_var)
+					       && PX4_ISFINITE(sample.beacon_lat)
+					       && PX4_ISFINITE(sample.beacon_lon)
+					       && PX4_ISFINITE(sample.beacon_alt);
+
+		if (measurement_valid && _local_origin_lat_lon.isInitialized() && PX4_ISFINITE(_local_origin_alt)) {
+			fuseRangingBeacon(sample);
+		}
+
+	}
+
+	if (_control_status.flags.rngbcn_fusion
+	    && isTimedOut(_aid_src_ranging_beacon.time_last_fuse, 2 * RNGBC_MAX_INTERVAL)) {
+		stopRangingBeaconFusion();
+	}
+}
+
+void Ekf::fuseRangingBeacon(const rangingBeaconSample &sample)
+{
+	const matrix::Vector3d vehicle_ecef = _gpos.toEcef();
+
+	const LatLonAlt beacon_lla(sample.beacon_lat, sample.beacon_lon, sample.beacon_alt);
+	const matrix::Vector3d beacon_ecef = beacon_lla.toEcef();
+
+	const matrix::Vector3d delta_ecef = beacon_ecef - vehicle_ecef;
+	const double predicted_range = delta_ecef.norm();
+
+	const float innovation = static_cast<float>(predicted_range) - sample.range_m;
+	const float R = fmaxf(sample.range_var, sq(_params.ekf2_rngbc_noise));
+
+	// Compute beacon position for the symforce H matrix.
+	// _state.pos(0:1) is always 0 in the global-position EKF, so N,E are relative.
+	// _state.pos(2) contains altitude, so beacon_pos(2) must be absolute.
+	const matrix::Dcmf R_ecef_to_ned = _gpos.computeRotEcefToNed();
+	const Vector3f delta_ecef_f(static_cast<float>(delta_ecef(0)),
+				    static_cast<float>(delta_ecef(1)),
+				    static_cast<float>(delta_ecef(2)));
+	const Vector3f delta_ned = R_ecef_to_ned * delta_ecef_f;
+	const Vector3f beacon_pos(delta_ned(0), delta_ned(1), _state.pos(2) + delta_ned(2));
+	float innov_var;
+	VectorState H;
+
+	sym::ComputeRangeBeaconInnovVarAndH(_state.vector(), P, beacon_pos, R, FLT_EPSILON, &innov_var, &H);
+
+	updateAidSourceStatus(_aid_src_ranging_beacon,
+			      sample.time_us,
+			      sample.range_m,
+			      R,
+			      innovation,
+			      innov_var,
+			      _params.ekf2_rngbc_gate);
+
+	_aid_src_ranging_beacon.device_id = sample.beacon_id;
+
+	if (_aid_src_ranging_beacon.innovation_rejected) {
+		return;
+	}
+
+	VectorState K = P * H / innov_var;
+	K(State::pos.idx + 2) = 0.f; // altitude is handled by height reference
+	measurementUpdate(K, H, R, innovation);
+
+	_aid_src_ranging_beacon.fused = true;
+	_aid_src_ranging_beacon.time_last_fuse = _time_delayed_us;
+	_time_last_hor_pos_fuse = _time_delayed_us;
+
+	if (!_control_status.flags.rngbcn_fusion) {
+		ECL_INFO("starting ranging beacon fusion");
+		_control_status.flags.rngbcn_fusion = true;
+	}
+
+}
+
+void Ekf::stopRangingBeaconFusion()
+{
+	if (_control_status.flags.rngbcn_fusion) {
+		ECL_INFO("stopping ranging beacon fusion");
+		_control_status.flags.rngbcn_fusion = false;
+	}
+}

src/modules/ekf2/EKF/common.h

@@ -76,6 +76,8 @@ static constexpr uint64_t GNSS_YAW_MAX_INTERVAL =
 	1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL      =
 	500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)
+static constexpr uint64_t RNGBC_MAX_INTERVAL    =
+	5000e3;  ///< Maximum allowable time interval between ranging beacon measurements (uSec)
 
 // bad accelerometer detection and mitigation
 static constexpr uint64_t BADACC_PROBATION =
@@ -262,6 +264,16 @@ struct auxVelSample {
 };
 #endif // CONFIG_EKF2_AUXVEL
 
+struct rangingBeaconSample {
+	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
+	uint8_t     beacon_id{};   ///< beacon identifier
+	float       range_m{};     ///< measured range to beacon (m)
+	float       range_var{};   ///< range measurement variance (m^2)
+	double      beacon_lat{};  ///< beacon latitude (degrees)
+	double      beacon_lon{};  ///< beacon longitude (degrees)
+	float       beacon_alt{};  ///< beacon altitude AMSL (m)
+};
+
 struct systemFlagUpdate {
 	uint64_t time_us{};
 	bool at_rest{false};
@@ -503,6 +515,14 @@ struct parameters {
 	const float auxvel_gate{5.0f};          ///< velocity fusion innovation consistency gate size (STD)
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	// ranging beacon fusion
+	int32_t ekf2_rngbc_ctrl{0};            ///< ranging beacon fusion control (0=disabled, 1=enabled)
+	float ekf2_rngbc_delay{0.f};          ///< ranging beacon measurement delay relative to the IMU (mSec)
+	float ekf2_rngbc_noise{1.f};           ///< ranging beacon measurement noise (m)
+	float ekf2_rngbc_gate{5.f};            ///< ranging beacon fusion innovation consistency gate size (STD)
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 };
 
 union fault_status_u {
@@ -592,6 +612,8 @@ uint64_t gnss_hgt_fault              :
 		1; ///< 47 - true if GNSS measurements (alt) have been declared faulty and are no longer used
 		uint64_t in_transition 	         : 1; ///< 48 - true if the vehicle is in vtol transition
 		uint64_t heading_observable      : 1; ///< 49 - true when heading is observable
+		uint64_t rngbcn_fusion           : 1; ///< 50 - true when ranging beacon position fusion is active
+
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -116,6 +116,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlGpsFusion(imu_delayed);
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	controlRangingBeaconFusion(imu_delayed);
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.update(*this, imu_delayed);
 	_control_status.flags.aux_gpos = _aux_global_position.anySourceFusing();

src/modules/ekf2/EKF/ekf.h

@@ -413,6 +413,10 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	const auto &aid_src_ranging_beacon() const { return _aid_src_ranging_beacon; }
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
@@ -589,6 +593,10 @@ private:
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	estimator_aid_source1d_s _aid_src_ranging_beacon {};
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	estimator_aid_source3d_s _aid_src_gravity {};
 #endif // CONFIG_EKF2_GRAVITY_FUSION
@@ -920,6 +928,12 @@ private:
 
 #endif // CONFIG_EKF2_GNSS
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	void controlRangingBeaconFusion(const imuSample &imu_delayed);
+	void fuseRangingBeacon(const rangingBeaconSample &sample);
+	void stopRangingBeaconFusion();
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion(const imuSample &imu_sample);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -810,7 +810,8 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 	}
 
 	// position aiding active
-	if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
+	if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos
+	     || _control_status.flags.aux_gpos || _control_status.flags.rngbcn_fusion)
 	    && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 	   ) {
 		inertial_dead_reckoning = false;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -73,6 +73,9 @@ EstimatorInterface::~EstimatorInterface()
 #if defined(CONFIG_EKF2_AUXVEL)
 	delete _auxvel_buffer;
 #endif // CONFIG_EKF2_AUXVEL
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	delete _ranging_beacon_buffer;
+#endif // CONFIG_EKF2_RANGING_BEACON
 }
 
 // Accumulate imu data and store to buffer at desired rate
@@ -436,6 +439,46 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+void EstimatorInterface::setRangingBeaconData(const rangingBeaconSample &ranging_beacon_sample)
+{
+
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_ranging_beacon_buffer == nullptr) {
+		_ranging_beacon_buffer = new TimestampedRingBuffer<rangingBeaconSample>(_obs_buffer_length);
+
+		if (_ranging_beacon_buffer == nullptr || !_ranging_beacon_buffer->valid()) {
+			delete _ranging_beacon_buffer;
+			_ranging_beacon_buffer = nullptr;
+			printBufferAllocationFailed("ranging beacon");
+			return;
+		}
+	}
+
+	const int64_t time_us = ranging_beacon_sample.time_us
+				- static_cast<int64_t>(_params.ekf2_rngbc_delay * 1000)
+				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+	// limit data rate to prevent data being lost
+	if (time_us >= static_cast<int64_t>(_ranging_beacon_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		rangingBeaconSample ranging_beacon_sample_new{ranging_beacon_sample};
+		ranging_beacon_sample_new.time_us = time_us;
+
+		_ranging_beacon_buffer->push(ranging_beacon_sample_new);
+		_time_last_ranging_beacon_buffer_push = _time_latest_us;
+
+	} else {
+		ECL_WARN("ranging beacon data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us,
+			 _ranging_beacon_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
 	if (!_initialised) {
@@ -618,7 +661,8 @@ int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const
 {
 	return int(_control_status.flags.gnss_pos)
 	       + int(_control_status.flags.ev_pos)
-	       + int(_control_status.flags.aux_gpos);
+	       + int(_control_status.flags.aux_gpos)
+	       + int(_control_status.flags.rngbcn_fusion);
 }
 
 bool EstimatorInterface::isHorizontalPositionAidingActive() const

src/modules/ekf2/EKF/estimator_interface.h

@@ -146,6 +146,10 @@ public:
 	void setAuxVelData(const auxVelSample &auxvel_sample);
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	void setRangingBeaconData(const rangingBeaconSample &ranging_beacon_sample);
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 	void setSystemFlagData(const systemFlagUpdate &system_flags);
 
 	// return a address to the parameters struct
@@ -454,6 +458,11 @@ protected:
 #endif // CONFIG_EKF2_AUXVEL
 	TimestampedRingBuffer<systemFlagUpdate> *_system_flag_buffer {nullptr};
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	TimestampedRingBuffer<rangingBeaconSample> *_ranging_beacon_buffer {nullptr};
+	uint64_t _time_last_ranging_beacon_buffer_push{0};
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_BAROMETER)
 	TimestampedRingBuffer<baroSample> *_baro_buffer {nullptr};
 	uint64_t _time_last_baro_buffer_push{0};

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,6 +721,23 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def compute_range_beacon_innov_var_and_h(
+        state: VState,
+        P: MTangent,
+        beacon_pos: sf.V3,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VTangent):
+
+    state = vstate_to_state(state)
+    delta = beacon_pos - state["pos"]
+    range_pred = delta.norm(epsilon=epsilon)
+
+    H = jacobian_chain_rule(range_pred, state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var, H.T)
+
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -752,5 +769,6 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
+generate_px4_function(compute_range_beacon_innov_var_and_h, output_names=["innov_var", "H"])
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_range_beacon_innov_var_and_h.h

@@ -0,0 +1,71 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_range_beacon_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix25_1
+ *     P: Matrix24_24
+ *     beacon_pos: Matrix31
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeRangeBeaconInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
+                                    const matrix::Matrix<Scalar, 3, 1>& beacon_pos, const Scalar R,
+                                    const Scalar epsilon, Scalar* const innov_var = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 40
+
+  // Input arrays
+
+  // Intermediate terms (7)
+  const Scalar _tmp0 = beacon_pos(2, 0) - state(9, 0);
+  const Scalar _tmp1 = beacon_pos(1, 0) - state(8, 0);
+  const Scalar _tmp2 = beacon_pos(0, 0) - state(7, 0);
+  const Scalar _tmp3 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
+                                       std::pow(_tmp2, Scalar(2)) + epsilon),
+                                Scalar(Scalar(-1) / Scalar(2)));
+  const Scalar _tmp4 = _tmp0 * _tmp3;
+  const Scalar _tmp5 = _tmp2 * _tmp3;
+  const Scalar _tmp6 = _tmp1 * _tmp3;
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R - _tmp4 * (-P(6, 8) * _tmp5 - P(7, 8) * _tmp6 - P(8, 8) * _tmp4) -
+                 _tmp5 * (-P(6, 6) * _tmp5 - P(7, 6) * _tmp6 - P(8, 6) * _tmp4) -
+                 _tmp6 * (-P(6, 7) * _tmp5 - P(7, 7) * _tmp6 - P(8, 7) * _tmp4);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(6, 0) = -_tmp5;
+    _h(7, 0) = -_tmp6;
+    _h(8, 0) = -_tmp4;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF2.cpp

@@ -164,6 +164,12 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
 	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	_param_ekf2_rngbc_ctrl(_params->ekf2_rngbc_ctrl),
+	_param_ekf2_rngbc_delay(_params->ekf2_rngbc_delay),
+	_param_ekf2_rngbc_noise(_params->ekf2_rngbc_noise),
+	_param_ekf2_rngbc_gate(_params->ekf2_rngbc_gate),
+#endif // CONFIG_EKF2_RANGING_BEACON
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ekf2_ev_delay),
 	_param_ekf2_ev_ctrl(_params->ekf2_ev_ctrl),
@@ -365,6 +371,14 @@ void EKF2::AdvertiseTopics()
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+
+		if (_param_ekf2_rngbc_ctrl.get()) {
+			_estimator_aid_src_ranging_beacon_pub.advertise();
+		}
+
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_SIDESLIP)
 
 		if (_param_ekf2_fuse_beta.get()) {
@@ -790,6 +804,9 @@ void EKF2::Run()
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 		UpdateRangeSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+		UpdateRangingBeaconSample(ekf2_timestamps);
+#endif // CONFIG_EKF2_RANGING_BEACON
 		UpdateSystemFlagsSample(ekf2_timestamps);
 
 		// run the EKF update and output
@@ -992,6 +1009,12 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(timestamp, _ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	// ranging beacon
+	PublishAidSourceStatus(timestamp, _ekf.aid_src_ranging_beacon(), _status_ranging_beacon_pub_last,
+			       _estimator_aid_src_ranging_beacon_pub);
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 	// fake position
 	PublishAidSourceStatus(timestamp, _ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
 	PublishAidSourceStatus(timestamp, _ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub);
@@ -2148,6 +2171,30 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 }
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+void EKF2::UpdateRangingBeaconSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	ranging_beacon_s ranging_beacon;
+
+	if (_ranging_beacon_sub.update(&ranging_beacon)) {
+		const float range_var = PX4_ISFINITE(ranging_beacon.range_accuracy)
+					? sq(ranging_beacon.range_accuracy) : sq(_param_ekf2_rngbc_noise.get());
+
+		rangingBeaconSample sample{
+			.time_us = ranging_beacon.timestamp_sample,
+			.beacon_id = ranging_beacon.beacon_id,
+			.range_m = ranging_beacon.range,
+			.range_var = range_var,
+			.beacon_lat = ranging_beacon.lat,
+			.beacon_lon = ranging_beacon.lon,
+			.beacon_alt = ranging_beacon.alt,
+		};
+
+		_ekf.setRangingBeaconData(sample);
+	}
+}
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_BAROMETER)
 void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 {

src/modules/ekf2/EKF2.hpp

@@ -123,6 +123,10 @@
 # include <uORB/topics/wind.h>
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+# include <uORB/topics/ranging_beacon.h>
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 extern pthread_mutex_t ekf2_module_mutex;
 
 class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
@@ -229,6 +233,9 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	void UpdateRangingBeaconSample(ekf2_timestamps_s &ekf2_timestamps);
+#endif // CONFIG_EKF2_RANGING_BEACON
 
 	void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
 
@@ -339,6 +346,10 @@ private:
 	hrt_abstime _status_aux_vel_pub_last{0};
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	uORB::Subscription _ranging_beacon_sub {ORB_ID(ranging_beacon)};
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
 	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
@@ -407,6 +418,11 @@ private:
 	int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	hrt_abstime _status_ranging_beacon_pub_last {0};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ranging_beacon_pub{ORB_ID(estimator_aid_src_ranging_beacon)};
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 	bool _callback_registered{false};
 
 	hrt_abstime _last_event_flags_publish{0};
@@ -622,6 +638,14 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+		// ranging beacon fusion
+		(ParamExtInt<px4::params::EKF2_RNGBC_CTRL>) _param_ekf2_rngbc_ctrl,
+		(ParamExtFloat<px4::params::EKF2_RNGBC_DELAY>) _param_ekf2_rngbc_delay,
+		(ParamExtFloat<px4::params::EKF2_RNGBC_NOISE>) _param_ekf2_rngbc_noise,
+		(ParamExtFloat<px4::params::EKF2_RNGBC_GATE>) _param_ekf2_rngbc_gate,
+#endif // CONFIG_EKF2_RANGING_BEACON
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		// vision estimate fusion
 		(ParamExtFloat<px4::params::EKF2_EV_DELAY>)

src/modules/ekf2/Kconfig

@@ -120,6 +120,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 range finder fusion support.
 
+menuconfig EKF2_RANGING_BEACON
+depends on MODULES_EKF2
+	bool "ranging beacon fusion support"
+	default y
+	---help---
+		EKF2 ranging beacon fusion support.
+
 menuconfig EKF2_SIDESLIP
 depends on MODULES_EKF2
         bool "sideslip fusion support"

src/modules/ekf2/params_ranging_beacon.yaml

@@ -0,0 +1,42 @@
+module_name: ekf2
+parameters:
+- group: EKF2
+  definitions:
+    EKF2_RNGBC_CTRL:
+      description:
+        short: Ranging beacon fusion control
+        long: Enable/disable ranging beacon fusion.
+      type: enum
+      values:
+        0: Disable ranging beacon fusion
+        1: Enable ranging beacon fusion
+      default: 0
+    EKF2_RNGBC_DELAY:
+      description:
+        short: Ranging beacon measurement delay relative to IMU measurements
+      type: float
+      default: 0
+      min: 0
+      max: 1000
+      unit: ms
+      reboot_required: true
+      decimal: 1
+    EKF2_RNGBC_GATE:
+      description:
+        short: Gate size for ranging beacon fusion
+        long: Sets the number of standard deviations used by the innovation consistency test.
+      type: float
+      default: 5.0
+      min: 1.0
+      unit: SD
+      decimal: 1
+    EKF2_RNGBC_NOISE:
+      description:
+        short: Measurement noise for ranging beacon fusion
+        long: Used to lower bound or replace the uncertainty included in the message
+      type: float
+      default: 30.0
+      min: 0.1
+      max: 500.0
+      unit: m
+      decimal: 1

src/modules/replay/ReplayEkf2.cpp

@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/ranging_beacon.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/aux_global_position.h>
 
@@ -138,6 +139,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 	} else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) {
 		_vehicle_global_position_groundtruth_msg_id = msg_id;
 
+	} else if (sub.orb_meta == ORB_ID(ranging_beacon)) {
+		_ranging_beacon_msg_id = msg_id;
+
 	} else if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
 		_ekf2_timestamps_exists = true;
 	}
@@ -160,6 +164,7 @@ ReplayEkf2::publishEkf2Topics(sensor_combined_s &sensor_combined, std::ifstream
 	findTimestampAndPublish(sensor_combined.timestamp, _vehicle_magnetometer_msg_id, replay_file);
 	findTimestampAndPublish(sensor_combined.timestamp, _vehicle_visual_odometry_msg_id, replay_file);
 	findTimestampAndPublish(sensor_combined.timestamp, _aux_global_position_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp, _ranging_beacon_msg_id, replay_file);
 
 	// sensor_combined: publish last because ekf2 is polling on this
 	if (_last_sensor_combined_timestamp > 0) {
@@ -195,6 +200,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
 	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);
+	handle_sensor_publication(0, _ranging_beacon_msg_id);
 	handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id);
 	handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id);
 	handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id);
@@ -291,6 +297,7 @@ ReplayEkf2::onExitMainLoop()
 	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");
+	print_sensor_statistics(_ranging_beacon_msg_id, "ranging_beacon");
 }
 
 } // namespace px4

src/modules/replay/ReplayEkf2.hpp

@@ -95,6 +95,7 @@ private:
 	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;
+	uint16_t _ranging_beacon_msg_id = msg_id_invalid;
 	uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid;
 	uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid;
 	uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid;