Skip to content

EKF2: Multi-instance AGP fusion #26151

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
haumarco wants to merge 4 commits into from
Closed

EKF2: Multi-instance AGP fusion #26151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fee7da6
uxrce_dds: support multi-instance uORB topics
haumarco Dec 18, 2025
ec7973e
uorb: add AuxGlobalPosition message
haumarco Dec 18, 2025
b9af959
ekf2: add multi-instance AGP parameters
haumarco Dec 18, 2025
8542261
ekf2: implement multi-instance AGP fusion
haumarco Dec 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 22 additions & 0 deletions msg/AuxGlobalPosition.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
# Auxiliary global position

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

@beniaminopozzan beniaminopozzan Dec 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@haumarco haumarco Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thx


uint32 MESSAGE_VERSION = 0
Copy link
Member

@bkueng bkueng Jan 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


uint64 timestamp # [us] Time since system start
uint64 timestamp_sample # [us] Timestamp of the raw data

uint8 id # Unique identifier for the AGP sourcxe, 1X for Visual Navigation, 2X for Radio Triangulation

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

@beniaminopozzan beniaminopozzan Dec 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@haumarco haumarco Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

@beniaminopozzan beniaminopozzan Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


float32 eph # [m] Standard deviation of horizontal position error
Copy link
Member

@beniaminopozzan beniaminopozzan Dec 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@haumarco haumarco Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes. that's something on our list.

float32 epv # [m] Standard deviation of vertical position error

uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates

# TOPICS aux_global_position
1 change: 1 addition & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ set(msg_files
Airspeed.msg
AirspeedWind.msg
AutotuneAttitudeControlStatus.msg
AuxGlobalPosition.msg
BatteryInfo.msg
ButtonEvent.msg
CameraCapture.msg
Expand Down
1 change: 0 additions & 1 deletion msg/versioned/VehicleGlobalPosition.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,4 +34,3 @@ 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
222 changes: 117 additions & 105 deletions src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,147 +42,159 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed

#if defined(MODULE_NAME)

if (_aux_global_position_sub.updated()) {
for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
if (_aux_global_position_subs[instance].updated()) {

vehicle_global_position_s aux_global_position{};
_aux_global_position_sub.copy(&aux_global_position);
aux_global_position_s aux_global_position{};
_aux_global_position_subs[instance].copy(&aux_global_position);

const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
const uint8_t sensor_id = aux_global_position.id;
const int slot = mapSensorIdToSlot(sensor_id);

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;
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
if (slot >= MAX_AGP_IDS) {
// All parameter slots are full, cannot handle this sensor. Set ID of unused slot to 0
continue;
}

const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(getDelayParam(slot) * 1000);

_aux_global_position_buffer.push(sample);
AuxGlobalPositionSample sample{};
sample.time_us = time_us;
sample.id = sensor_id;
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;
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;

_time_last_buffer_push = imu_delayed.time_us;
_sources[slot].buffer.push(sample);
_sources[slot].time_last_buffer_push = imu_delayed.time_us;
}
}

#endif // MODULE_NAME

AuxGlobalPositionSample sample;
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
SourceData &source = _sources[slot];
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::kHPos))) {
return;
}
if (source.buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {

estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used

// 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(), 0.01f);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);

ekf.updateAidSourceStatus(aid_src,
sample.time_us, // sample timestamp
matrix::Vector2d(sample.latitude, sample.longitude), // observation
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate

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::kStopped:

/* FALLTHROUGH */
case State::kStarting:
if (starting_conditions) {
_state = State::kStarting;

if (ekf.global_origin_valid()) {
const bool fused = ekf.fuseHorizontalPosition(aid_src);
bool reset = false;

if (!fused && isResetAllowed(ekf)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(aid_src);
reset = true;
}
if (!(getCtrlParam(slot) & static_cast<int32_t>(Ctrl::kHPos))) {
Copy link
Member

@beniaminopozzan beniaminopozzan Dec 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@haumarco haumarco Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

continue;
}

if (fused || reset) {
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
}
estimator_aid_source2d_s &aid_src = source.aid_src;
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used

float pos_noise = math::max(sample.eph, getNoiseParam(slot), 0.01f);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);

ekf.updateAidSourceStatus(aid_src,
sample.time_us, // sample timestamp
matrix::Vector2d(sample.latitude, sample.longitude), // observation
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(getGateParam(slot), 1.f)); // innovation gate

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 (source.state) {
case State::kStopped:

/* FALLTHROUGH */
case State::kStarting:
if (starting_conditions) {
source.state = State::kStarting;

if (ekf.global_origin_valid()) {
const bool fused = ekf.fuseHorizontalPosition(aid_src);
bool reset = false;

if (!fused && isResetAllowed(ekf, slot)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(aid_src);
reset = true;
}

if (fused || reset) {
ekf.enableControlStatusAuxGpos();
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
source.state = State::kActive;
}

} else {
// Try to initialize using measurement
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
sq(sample.epv))) {
ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
} else {
// Try to initialize using measurement
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
sq(sample.epv))) {
ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.enableControlStatusAuxGpos();
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
source.state = State::kActive;
}
}
}
}

break;
break;

case State::kActive:
if (continuing_conditions) {
ekf.fuseHorizontalPosition(aid_src);
case State::kActive:
if (continuing_conditions) {
ekf.fuseHorizontalPosition(aid_src);

if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
if (isResetAllowed(ekf)) {
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|| (source.reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
if (isResetAllowed(ekf, slot)) {

ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));

ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.resetAidSourceStatusZeroInnovation(aid_src);

_reset_counters.lat_lon = sample.lat_lon_reset_counter;
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;

} else {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
} else {
ekf.disableControlStatusAuxGpos();
source.state = State::kStopped;
}
}
}

} else {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
}
} else {
ekf.disableControlStatusAuxGpos();
Copy link
Member

@beniaminopozzan beniaminopozzan Dec 23, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@haumarco haumarco Jan 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

source.state = State::kStopped;
}

break;
break;

default:
break;
}
default:
break;
}

#if defined(MODULE_NAME)
aid_src.timestamp = hrt_absolute_time();
_estimator_aid_src_aux_global_position_pub.publish(aid_src);
aid_src.timestamp = hrt_absolute_time();
source.aid_src_pub.publish(aid_src);

_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
source.test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
#endif // MODULE_NAME

} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
ECL_WARN("Aux global position data stopped");
} else if ((source.state != State::kStopped) && isTimedOut(source.time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
ekf.disableControlStatusAuxGpos();
source.state = State::kStopped;
ECL_WARN("Aux global position data stopped for slot %d (sensor ID %d)", slot, getIdParam(slot));
}
}
}

bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf, int slot) const
{
return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
return ((static_cast<Mode>(getModeParam(slot)) == Mode::kAuto)
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
|| ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
|| ((static_cast<Mode>(getModeParam(slot)) == Mode::kDeadReckoning)
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
}

Expand Down
Loading
Loading