From f4c820c7e128e48e7e8e631c14df1b831f3dc295 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 31 Mar 2026 20:43:15 +0200 Subject: [PATCH] feat(ekf2): add ranging beacon fusion support - Add Symforce-derivation - No altitude correction - EKF2 replay - New params --- msg/EstimatorAidSource1d.msg | 2 +- src/lib/lat_lon_alt/lat_lon_alt.cpp | 15 ++ src/lib/lat_lon_alt/lat_lon_alt.hpp | 3 + src/modules/ekf2/CMakeLists.txt | 7 + src/modules/ekf2/EKF/CMakeLists.txt | 6 + .../ranging_beacon/ranging_beacon_control.cpp | 139 ++++++++++++++++++ src/modules/ekf2/EKF/common.h | 22 +++ src/modules/ekf2/EKF/control.cpp | 4 + src/modules/ekf2/EKF/ekf.h | 14 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 3 +- src/modules/ekf2/EKF/estimator_interface.cpp | 46 +++++- src/modules/ekf2/EKF/estimator_interface.h | 9 ++ .../EKF/python/ekf_derivation/derivation.py | 18 +++ .../compute_range_beacon_innov_var_and_h.h | 71 +++++++++ src/modules/ekf2/EKF2.cpp | 47 ++++++ src/modules/ekf2/EKF2.hpp | 24 +++ src/modules/ekf2/Kconfig | 7 + src/modules/ekf2/params_ranging_beacon.yaml | 42 ++++++ src/modules/replay/ReplayEkf2.cpp | 7 + src/modules/replay/ReplayEkf2.hpp | 1 + 20 files changed, 484 insertions(+), 3 deletions(-) create mode 100644 src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_range_beacon_innov_var_and_h.h create mode 100644 src/modules/ekf2/params_ranging_beacon.yaml diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 7bd8ea7658..df428000a6 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/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 diff --git a/src/lib/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp index 40b86a635a..1732939fe4 100644 --- a/src/lib/lat_lon_alt/lat_lon_alt.cpp +++ b/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()); diff --git a/src/lib/lat_lon_alt/lat_lon_alt.hpp b/src/lib/lat_lon_alt/lat_lon_alt.hpp index e72fc5fa79..448212eaa8 100644 --- a/src/lib/lat_lon_alt/lat_lon_alt.hpp +++ b/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; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 16661da5b4..1f05add4fc 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/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) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index dd31388e57..38dc2718ea 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/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() diff --git a/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp b/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp new file mode 100644 index 0000000000..ffb1c003be --- /dev/null +++ b/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 +#include + +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(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(delta_ecef(0)), + static_cast(delta_ecef(1)), + static_cast(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; + } +} diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 972da2eaf8..0c29ecf6bd 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0624cba6d9..574e7f70c2 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/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(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 70e117bd1a..d1c0dfcf20 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e497ad2a52..c89d72ba11 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index c468697b8a..a6d6ca8038 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/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(_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(_params.ekf2_rngbc_delay * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + + // limit data rate to prevent data being lost + if (time_us >= static_cast(_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 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index c95b5c56e6..f00c295465 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/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 *_system_flag_buffer {nullptr}; +#if defined(CONFIG_EKF2_RANGING_BEACON) + TimestampedRingBuffer *_ranging_beacon_buffer {nullptr}; + uint64_t _time_last_ranging_beacon_buffer_push{0}; +#endif // CONFIG_EKF2_RANGING_BEACON + #if defined(CONFIG_EKF2_BAROMETER) TimestampedRingBuffer *_baro_buffer {nullptr}; uint64_t _time_last_baro_buffer_push{0}; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 91c3fd734e..11f500a6a2 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/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) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_range_beacon_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_range_beacon_innov_var_and_h.h new file mode 100644 index 0000000000..f2dd55e297 --- /dev/null +++ b/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 + +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 +void ComputeRangeBeaconInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& beacon_pos, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* 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& _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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 84227fab6a..b6301898db 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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 ×tamp) 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) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 36d7763995..56b3c56746 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -123,6 +123,10 @@ # include #endif // CONFIG_EKF2_WIND +#if defined(CONFIG_EKF2_RANGING_BEACON) +# include +#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 _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_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) _param_ekf2_rng_pos_z, #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_RANGING_BEACON) + // ranging beacon fusion + (ParamExtInt) _param_ekf2_rngbc_ctrl, + (ParamExtFloat) _param_ekf2_rngbc_delay, + (ParamExtFloat) _param_ekf2_rngbc_noise, + (ParamExtFloat) _param_ekf2_rngbc_gate, +#endif // CONFIG_EKF2_RANGING_BEACON + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision estimate fusion (ParamExtFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 04bcfc104c..721f5ae265 100644 --- a/src/modules/ekf2/Kconfig +++ b/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" diff --git a/src/modules/ekf2/params_ranging_beacon.yaml b/src/modules/ekf2/params_ranging_beacon.yaml new file mode 100644 index 0000000000..ab5360428a --- /dev/null +++ b/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 diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index fd334c24e8..0e009aee04 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -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 diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index bbf478c73e..77ba3d8e8c 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/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;