EKF2: Multi-instance AGP fusion #26151

haumarco · 2025-12-19T08:28:15Z

Solved Problem

EKF2 could only fuse a single Auxiliary Global Position (AGP) source, limiting the ability to use multiple external positioning systems simultaneously, e.g. Visual-Map-Matching-Navigation, Radio-Triangulaiton.

Solution

This PR enables EKF2 to fuse up to 4 independent AGP sources simultaneously by implementing multi-instance support throughout the entire stack:

New uORB message type (AuxGlobalPosition)
- Separates AGP from VehicleGlobalPosition for cleaner architecture
- Includes id field to identify the source
- Enables multi-instance publishing via uORB
Multi-instance DDS bridge support
- Maps 4 separate ROS2/DDS topics to a single uORB topic
- Topics: DDS /fmu/in/aux_global_position_A/B/C/D to aux_global_position uORB topic
- Each external system publishes to its own DDS topic with a unique message ID
Per-instance EKF2 parameters (EKF2_AGP0-3_*)
- new: EKF2_AGP{N}_ID: Sensor ID mapping (parameter slot → message ID).
- existing: EKF2_AGP{N}_CTRL etc...
Independent fusion state machines
- Each AGP source has its own state tracking, buffers, and fusion logic
- Dynamic parameter lookup without boilerplate switch statements
- Automatic sensor-to-slot mapping: message id field routes to parameter slot
- Per-source innovation monitoring and fault detection

Changelog Entry

For release notes:

Feature: Multi-instance Auxiliary Global Position fusion in EKF2
New parameters: EKF2_AGP{0-3}_{ID,CTRL,MODE,DELAY,NOISE,GATE} (24 parameters total)
New uORB message: AuxGlobalPosition (replaces aux_global_position topic usage of VehicleGlobalPosition)
DDS bridge: Multi-instance topic support via /fmu/in/aux_global_position_{A,B,C,D}

Migration notes:
- Old parameter EKF2_AGP_* renamed to EKF2_AGP{N}_*
- DDS topic /fmu/in/aux_global_position still works
- Use new multi-instance topics /fmu/in/aux_global_position_{A,B,C,D} for multiple sources

Test coverage

Tested both with ROS topics: /fmu/in/aux_global_position_A, /fmu/in/aux_global_position_B
and also as used with one source: /fmu/in/aux_global_position

Additional Notes

Extension to of multi-sensor capability Optical Flow or also GNSS is planned. I think I can generalize quite some things to avoid having duplicate code.

Enable DDS bridge to handle multi-instance uORB topics by mapping multiple topic instances to a single DDS topic with instance field.

github-actions · 2025-12-19T08:36:53Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 2616 byte (0.13 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +2.57Ki  +0.1% +2.57Ki    .text
   +56%    +788   +56%    +788    AuxGlobalPosition::update()
  +0.3%    +560  +0.3%    +560    [section .text]
  +125%    +444  +125%    +444    AuxGlobalPosition::AuxGlobalPosition()
  [NEW]    +296  [NEW]    +296    ucdr_deserialize_aux_global_position()
  +8.5%    +172  +8.5%    +172    RcvTopicsPubs::init()
  +0.1%    +156  +0.1%    +156    g_cromfs_image
  +1.0%    +152  +1.0%    +152    px4::parameters
   +74%    +136   +74%    +136    AuxGlobalPosition::isResetAllowed()
   +16%     +56   +16%     +56    Ekf::~Ekf()
   +22%     +48   +22%     +48    AuxGlobalPosition::~AuxGlobalPosition()
  +4.7%     +44  +4.7%     +44    RcvTopicsPubs::~RcvTopicsPubs()
  +2.6%     +40  +2.6%     +40    EKF2::~EKF2()
  +0.2%     +40  +0.2%     +40    uORB::compressed_fields
  +0.9%     +32  +0.9%     +32    UxrceddsClient::run()
  +0.8%     +16  +0.8%     +16    px4::parameters_type
 -11.1%     -16 -11.1%     -16    px4::wq_configurations::lp_default
  -4.5%     -20  -4.5%     -20    param_reset_specific
 -100.5%     -44 -100.5%     -44    [26 Others]
  [DEL]     -68  [DEL]     -68    AuxGlobalPosition::updateParamsImpl()
 -19.4%     -76 -19.4%     -76    Ekf::updateParameters()
  -1.7%    -124  -1.7%    -124    on_topic_update()
+0.0%    +913  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%     +48  [ = ]       0    .debug_frame
+0.2% +43.3Ki  [ = ]       0    .debug_info
+0.1% +4.07Ki  [ = ]       0    .debug_line
   +20%      +1  [ = ]       0    [Unmapped]
  +0.1% +4.07Ki  [ = ]       0    [section .debug_line]
+0.0% +1.18Ki  [ = ]       0    .debug_loclists
+0.2% +1.30Ki  [ = ]       0    .debug_rnglists
 -33.3%      -1  [ = ]       0    [Unmapped]
  +0.2% +1.31Ki  [ = ]       0    [section .debug_rnglists]
-0.1% -2.13Ki  [ = ]       0    .debug_str
+0.8%      +2  [ = ]       0    .shstrtab
+0.0%     +62  [ = ]       0    .strtab
  +2.1%      +1  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [DEL]     -43  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
  +0.1%     +24  [ = ]       0    [section .strtab]
 -37.2%     -16  [ = ]       0    __arm_switchcontext_veneer
   +64%     +16  [ = ]       0    __nxsched_get_tcb_veneer
  [NEW]     +80  [ = ]       0    ucdr_deserialize_aux_global_position()
+0.0%    +112  [ = ]       0    .symtab
  +200%     +32  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  +100%     +64  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -32  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
  +100%     +16  [ = ]       0    ConstLayer::containedAsBitset()
   +50%     +16  [ = ]       0    ConstLayer::contains()
 -33.3%     -16  [ = ]       0    ConstLayer::store()
 -25.0%     -16  [ = ]       0    Ekf::resetVelUsingAirspeed()
 -20.0%     -16  [ = ]       0    OutputPredictor::OutputPredictor()
   +33%     +32  [ = ]       0    RcvTopicsPubs::init()
 -33.3%     -16  [ = ]       0    RcvTopicsPubs::~RcvTopicsPubs()
   +50%     +16  [ = ]       0    UxrceddsClient::deinit()
  +100%     +32  [ = ]       0    UxrceddsClient::init()
 -90.9%     +32  [ = ]       0    [6 Others]
  -0.4%     -48  [ = ]       0    [section .symtab]
   +33%     +16  [ = ]       0    ____aeabi_ul2f_veneer
 -40.0%     -32  [ = ]       0    __arm_switchcontext_veneer
   +67%     +32  [ = ]       0    __nxsched_get_tcb_veneer
 -25.0%     -16  [ = ]       0    __nxsem_wait_irq_veneer
   +50%     +16  [ = ]       0    __orb_aux_global_position
   +50%     +16  [ = ]       0    __stm32_i2c_setclock_veneer
 -33.3%     -16  [ = ]       0    __up_udelay_veneer
 +15% +1.45Ki  [ = ]       0    [Unmapped]
-0.1%     -16  -0.1%     -16    .ramfunc
   +14%      +1   +14%      +1    __stm32_i2c_setclock_veneer
 -12.5%      -1 -12.5%      -1    __up_udelay_veneer
  -1.7%      -4  -1.7%      -4    param_get
  -1.4%      -4  -1.4%      -4    param_get_default_value
 -25.0%      -4 -25.0%      -4    param_get_index
  -8.3%      -4  -8.3%      -4    param_get_system_default_value
+0.1% +52.9Ki  +0.1% +2.55Ki    TOTAL

px4_fmu-v6x [Total VM Diff: 2680 byte (0.14 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +2.62Ki  +0.1% +2.62Ki    .text
   +56%    +788   +56%    +788    AuxGlobalPosition::update()
  +0.3%    +524  +0.3%    +524    [section .text]
  +125%    +444  +125%    +444    AuxGlobalPosition::AuxGlobalPosition()
  [NEW]    +296  [NEW]    +296    ucdr_deserialize_aux_global_position()
  +8.5%    +172  +8.5%    +172    RcvTopicsPubs::init()
  +1.0%    +152  +1.0%    +152    px4::parameters
   +74%    +136   +74%    +136    AuxGlobalPosition::isResetAllowed()
  +0.1%    +132  +0.1%    +132    g_cromfs_image
   +16%     +56   +16%     +56    Ekf::~Ekf()
   +22%     +48   +22%     +48    AuxGlobalPosition::~AuxGlobalPosition()
  +4.7%     +44  +4.7%     +44    RcvTopicsPubs::~RcvTopicsPubs()
  +2.6%     +40  +2.6%     +40    EKF2::~EKF2()
  +0.2%     +40  +0.2%     +40    uORB::compressed_fields
  +0.9%     +32  +0.9%     +32    UxrceddsClient::run()
  +1.0%     +20  +1.0%     +20    px4::parameters_type
  +4.4%     +12  +4.4%     +12    px4::logger::LoggedTopics::add_estimator_replay_topics()
  +0.2%      +8  +0.2%      +8    px4::logger::LoggedTopics::add_default_topics()
 -99.8%      +4 -99.8%      +4    [5 Others]
  [DEL]     -68  [DEL]     -68    AuxGlobalPosition::updateParamsImpl()
 -19.4%     -76 -19.4%     -76    Ekf::updateParameters()
  -1.7%    -124  -1.7%    -124    on_topic_update()
+0.0%    +913  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%     +68  [ = ]       0    .debug_frame
+0.2% +42.1Ki  [ = ]       0    .debug_info
+0.1% +4.16Ki  [ = ]       0    .debug_line
+0.0% +1.29Ki  [ = ]       0    .debug_loclists
+0.2% +1.32Ki  [ = ]       0    .debug_rnglists
-0.1% -2.12Ki  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%     +62  [ = ]       0    .strtab
  +2.1%      +1  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [DEL]     -43  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
  +0.1%     +24  [ = ]       0    [section .strtab]
  [NEW]     +80  [ = ]       0    ucdr_deserialize_aux_global_position()
+0.0%    +112  [ = ]       0    .symtab
  +200%     +32  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  +100%     +64  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -32  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
 -25.0%     -16  [ = ]       0    Ekf::resetVelUsingAirspeed()
 -20.0%     -16  [ = ]       0    OutputPredictor::OutputPredictor()
   +33%     +32  [ = ]       0    RcvTopicsPubs::init()
 -33.3%     -16  [ = ]       0    RcvTopicsPubs::~RcvTopicsPubs()
   +50%     +16  [ = ]       0    UxrceddsClient::deinit()
  +100%     +32  [ = ]       0    UxrceddsClient::init()
  -0.1%     -16  [ = ]       0    [section .symtab]
   +50%     +16  [ = ]       0    __orb_aux_global_position
  [NEW]     +16  [ = ]       0    ucdr_deserialize_aux_global_position()
 +36% +1.38Ki  [ = ]       0    [Unmapped]
+0.1% +51.9Ki  +0.1% +2.62Ki    TOTAL

Updated: 2025-12-19T14:14:27

Add dedicated message type for auxiliary global position sources. Separates aux_global_position from VehicleGlobalPosition topic.

Add per-instance parameters (ID, CTRL, MODE, DELAY, NOISE, GATE) for up to 4 auxiliary global position sources.

- Support up to 4 independent AGP sources simultaneously - Dynamic parameter lookup without boilerplate switch statements - Per-source state tracking and fusion logic - Update simulator to use new AuxGlobalPosition message

beniaminopozzan · 2025-12-23T13:43:06Z

PX4-Autopilot/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

Line 115 in 8542261

_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);

AGP altitude is expected in AMSL but sensor_agp_sim fills it with above ellipsoid data

beniaminopozzan

Thanks @haumarco !
the uxrce_dds_client change alone is very much welcome (it could even be a standalone PR)!

Could you also update the documentation please? 🙏🏻
Thanks!

beniaminopozzan · 2025-12-23T13:25:43Z

msg/AuxGlobalPosition.msg

+# Auxiliary global position
+
+# This message provides global position data from an external source such as
+# radio-triangulation, viusal navigation, or other positioning system.


Suggested change

# radio-triangulation, viusal navigation, or other positioning system.

# radio-triangulation, visual navigation, or other positioning systems.

beniaminopozzan · 2025-12-23T13:35:03Z

msg/AuxGlobalPosition.msg

+
+float64 lat                     # [deg] Latitude in WGS84
+float64 lon                     # [deg] Longitude in WGS84
+float32 alt                     # [m] Altitude above mean sea level (AMSL)


Is there a specific reason for using AMSL instead of ellipsoid altitude?

From the ROS 2 PoV, the NavSatFix message type would convert to AuxGlobalPosition in an easier way if the AGP altitude were w.r.t. WGS84

in EKF2 we use AMSL, so to keep consistency. we can discuss this, but it's not in the scope of this PR.

I though WGS84 -> AMSL conversion would have been trivial inside the EKF, but actually

PX4-Autopilot/src/modules/ekf2/EKF2.hpp

Line 214 in 4757158

float altEllipsoidToAmsl(float ellipsoid_alt) const;

only works if there is valid a valid gps source.

beniaminopozzan · 2025-12-23T13:36:35Z

msg/AuxGlobalPosition.msg

+float64 lon                     # [deg] Longitude in WGS84
+float32 alt                     # [m] Altitude above mean sea level (AMSL)
+
+float32 eph                     # [m] Standard deviation of horizontal position error


Not striclty related to this PR, but wouldn't splitting eph into North and East be more useful?

yes. that's something on our list.

beniaminopozzan · 2025-12-23T14:07:44Z

src/modules/uxrce_dds_client/dds_topics.h.em

+@[        for idx, sub in enumerate(subs)]@
+		{ORB_ID(@(topic_simple))}@('' if idx == len(subs)-1 else ',')
+
+@[        end for]@


Suggested change

@[ for idx, sub in enumerate(subs)]@

{ORB_ID(@(topic_simple))}@('' if idx == len(subs)-1 else ',')

@[ end for]@

@[ for idx, sub in enumerate(subs)]@

{ORB_ID(@(topic_simple))}@('' if idx == len(subs)-1 else ',')

@[ end for]@

beniaminopozzan · 2025-12-23T17:44:00Z

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

-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-						reset = true;
-					}
+			if (!(getCtrlParam(slot) & static_cast<int32_t>(Ctrl::kHPos))) {


with all get***Param functions calling snprintf at some point, will this add not-negligible delays?

i dont think using snprintf adds relevant delay. i now also improved the handling of the parameter caching without overloading _params...

beniaminopozzan · 2025-12-23T17:55:34Z

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

-				_state = State::kStopped;
-			}
+				} else {
+					ekf.disableControlStatusAuxGpos();


shouldn't this (_control_status.flags.aux_gpos) be slot-specific too?
one slot could fail and stop aiding but the others could still be healthy.

yes. I'll disable the cs-flag as soon as all sources are stopped. thanks

beniaminopozzan · 2025-12-23T18:00:51Z

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

 	}

-	float test_ratio_filtered() const { return _test_ratio_filtered; }
+	float test_ratio_filtered(uint8_t id = 0) const


the rest of the EKF2 module only ever uses id=0 (like in

PX4-Autopilot/src/modules/ekf2/EKF/ekf_helper.cpp

Lines 544 to 545 in 8542261

test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));

}

) so the other instances are not accounted

you are right!

bkueng

I just went through the DDS part and it looks good, nice work.

bkueng · 2026-01-07T09:22:06Z

msg/AuxGlobalPosition.msg

+# This message provides global position data from an external source such as
+# radio-triangulation, viusal navigation, or other positioning system.
+
+uint32 MESSAGE_VERSION = 0


If you want to version it, it should be under msg/versioned/

haumarco · 2026-01-20T16:45:10Z

I am closing this PR because it's covering too much at once.
New PRs will have to be merged in order because of dependencies:
DDS: #26305 (nothing changed, except cleaning empty whitespace)
EKF2: #26306
Mavlink: #26307

uxrce_dds: support multi-instance uORB topics

fee7da6

Enable DDS bridge to handle multi-instance uORB topics by mapping multiple topic instances to a single DDS topic with instance field.

haumarco requested review from bkueng and bresch December 19, 2025 09:09

haumarco force-pushed the ekf2_multi_agp branch from f939832 to c42be08 Compare December 19, 2025 09:20

beniaminopozzan self-requested a review December 19, 2025 09:51

haumarco added 3 commits December 19, 2025 14:42

uorb: add AuxGlobalPosition message

ec7973e

Add dedicated message type for auxiliary global position sources. Separates aux_global_position from VehicleGlobalPosition topic.

ekf2: add multi-instance AGP parameters

b9af959

Add per-instance parameters (ID, CTRL, MODE, DELAY, NOISE, GATE) for up to 4 auxiliary global position sources.

ekf2: implement multi-instance AGP fusion

8542261

- Support up to 4 independent AGP sources simultaneously - Dynamic parameter lookup without boilerplate switch statements - Per-source state tracking and fusion logic - Update simulator to use new AuxGlobalPosition message

haumarco force-pushed the ekf2_multi_agp branch from c42be08 to 8542261 Compare December 19, 2025 14:06

beniaminopozzan requested changes Dec 23, 2025

View reviewed changes

bkueng reviewed Jan 7, 2026

View reviewed changes

This was referenced Jan 20, 2026

uxrce_dds: support multi-instance uORB topics #26305

Open

EKF2: support multiple AGP sources #26306

Open

Mavlink: GLOBAL_POSITION for aux positioning #26307

Open

haumarco closed this Jan 20, 2026

	# radio-triangulation, viusal navigation, or other positioning system.
	# radio-triangulation, visual navigation, or other positioning systems.

	test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
	}

EKF2: Multi-instance AGP fusion #26151

EKF2: Multi-instance AGP fusion #26151

Uh oh!

Conversation

haumarco commented Dec 19, 2025

Solved Problem

Solution

Changelog Entry

Test coverage

Additional Notes

Uh oh!

github-actions bot commented Dec 19, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

beniaminopozzan commented Dec 23, 2025

Uh oh!

beniaminopozzan left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bkueng left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

haumarco commented Jan 20, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

github-actions bot commented Dec 19, 2025 •

edited

Loading