EKF2: Aux Globlal Pos (AGP) reset mode #25223

bresch · 2025-07-15T11:55:10Z

Solved Problem

The EKF performs statistical consistency checks using normalized innovation tests. However, when the AGP measurement starts to fail a test, we directly reset to the measurement after a timeout period.

Solution

Add a "reset mode" parameter that tells the EKF whether it should always try to reset to the AGP position measurement when the statistical check fails or if the EKF is allowed to continue using solely velocity measurements.

New parameter: EKF2_AGP_MODE

Automatic: reset on fusion timeout if no other source of position is available
Dead-reckoning: reset on fusion timeout if no source of velocity is available

Additionally, the VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE is no longer filtered by the EKF2 front end. The command is sent and executed by the EKF as any other measurement. This allows the user to override the current position estimate.

Changelog Entry

For release notes:

Feature/Bugfix XYZ
New parameter: EKF2_AGP_MODE
Documentation: -

Test coverage

tested in SIH with simulated AGP failures

This allows the pilot to override position esitmates manually

github-actions · 2025-07-15T12:02:31Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 256 byte (0.01 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +256  +0.0%    +256    .text
  [NEW]    +184  [NEW]    +184    AuxGlobalPosition::isResetAllowed()
  +0.0%     +72  +0.0%     +72    g_cromfs_image
  +7.2%     +24  +7.2%     +24    AuxGlobalPosition::AuxGlobalPosition()
  +3.0%     +24  +3.0%     +24    Ekf::resetGlobalPosToExternalObservation()
  +0.0%     +24  +0.0%     +24    [section .text]
  +4.2%     +16  +4.2%     +16    Ekf::updateParameters()
   +21%     +12   +21%     +12    AuxGlobalPosition::updateParamsImpl()
  +0.1%      +8  +0.1%      +8    px4::parameters
  -0.8%      -4  -0.8%      -4    EstimatorInterface::setBaroData()
  -1.2%     -40  -1.2%     -40    EKF2::Run()
  -4.3%     -64  -4.3%     -64    AuxGlobalPosition::update()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%     +16  [ = ]       0    .debug_aranges
+0.0%     +52  [ = ]       0    .debug_frame
+0.0% +2.93Ki  [ = ]       0    .debug_info
+0.0%    +526  [ = ]       0    .debug_line
   +20%      +1  [ = ]       0    [Unmapped]
  +0.0%    +525  [ = ]       0    [section .debug_line]
+0.0%    +676  [ = ]       0    .debug_loclists
+0.0%     +65  [ = ]       0    .debug_rnglists
 -66.7%      -2  [ = ]       0    [Unmapped]
  +0.0%     +67  [ = ]       0    [section .debug_rnglists]
+0.0% +1.11Ki  [ = ]       0    .debug_str
+0.4%      +1  [ = ]       0    .shstrtab
+0.0%     +47  [ = ]       0    .strtab
  [NEW]     +47  [ = ]       0    AuxGlobalPosition::isResetAllowed()
   +36%     +16  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
 -41.0%     -16  [ = ]       0    __up_block_task_veneer
+0.0%     +16  [ = ]       0    .symtab
  [NEW]     +16  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  -9.1%     -16  [ = ]       0    EKF2::Run()
  +100%     +16  [ = ]       0    EKF2::updateParamsImpl()
   +33%     +16  [ = ]       0    Ekf::resetVelUsingAirspeed()
 -25.0%     -16  [ = ]       0    EstimatorInterface::setSystemFlagData()
   +13%     +32  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
   +50%     +16  [ = ]       0    __clock_abstime2ticks_veneer
 -33.3%     -16  [ = ]       0    __stm32_configxfrints_veneer
   +33%     +16  [ = ]       0    __stm32_dmastop_veneer
 -25.0%     -16  [ = ]       0    __stm32_i2c_sendstart_veneer
 -40.0%     -32  [ = ]       0    __up_block_task_veneer
-2.6%    -256  [ = ]       0    [Unmapped]
+0.0% +5.46Ki  +0.0%    +256    TOTAL

px4_fmu-v6x [Total VM Diff: 256 byte (0.01 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +256  +0.0%    +256    .text
  [NEW]    +184  [NEW]    +184    AuxGlobalPosition::isResetAllowed()
  +0.0%     +72  +0.0%     +72    g_cromfs_image
  +0.0%     +28  +0.0%     +28    [section .text]
  +7.2%     +24  +7.2%     +24    AuxGlobalPosition::AuxGlobalPosition()
  +3.0%     +24  +3.0%     +24    Ekf::resetGlobalPosToExternalObservation()
  +4.2%     +16  +4.2%     +16    Ekf::updateParameters()
   +21%     +12   +21%     +12    AuxGlobalPosition::updateParamsImpl()
  +0.1%      +8  +0.1%      +8    px4::parameters
  -0.8%      -4  -0.8%      -4    EstimatorInterface::setBaroData()
 -30.8%      -4 -30.8%      -4    g_nullstring
  -1.2%     -40  -1.2%     -40    EKF2::Run()
  -4.3%     -64  -4.3%     -64    AuxGlobalPosition::update()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%     +16  [ = ]       0    .debug_aranges
+0.0%     +52  [ = ]       0    .debug_frame
+0.0% +2.85Ki  [ = ]       0    .debug_info
+0.0%    +526  [ = ]       0    .debug_line
   +25%      +1  [ = ]       0    [Unmapped]
  +0.0%    +525  [ = ]       0    [section .debug_line]
+0.0%    +667  [ = ]       0    .debug_loclists
+0.0%     +70  [ = ]       0    .debug_rnglists
  [NEW]      +3  [ = ]       0    [Unmapped]
  +0.0%     +67  [ = ]       0    [section .debug_rnglists]
+0.0% +1.11Ki  [ = ]       0    .debug_str
+0.4%      +1  [ = ]       0    .shstrtab
+0.0%     +47  [ = ]       0    .strtab
  [NEW]     +47  [ = ]       0    AuxGlobalPosition::isResetAllowed()
+0.0%     +16  [ = ]       0    .symtab
  [NEW]     +16  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  -9.1%     -16  [ = ]       0    EKF2::Run()
  +100%     +16  [ = ]       0    EKF2::updateParamsImpl()
   +33%     +16  [ = ]       0    Ekf::resetVelUsingAirspeed()
 -25.0%     -16  [ = ]       0    EstimatorInterface::setSystemFlagData()
-4.7%    -256  [ = ]       0    [Unmapped]
+0.0% +5.37Ki  +0.0%    +256    TOTAL

Updated: 2025-07-15T12:02:30

haumarco · 2025-07-15T15:31:06Z

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

+	enum class Mode : uint8_t {
+		AUTO           = 0,   	///< Reset on fusion timeout if no other source of position is available
+		DEAD_RECKONING = 1   	///< Reset on fusion timeout if no source of velocity is availabl
+	};


https://google.github.io/styleguide/cppguide.html#Enumerator_Names

haumarco · 2025-07-15T15:37:33Z

src/modules/ekf2/EKF/ekf.cpp

@@ -335,9 +335,11 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl

 		const bool innov_rejected = (test_ratio > 1.f);

-		if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) {
-			// when on ground or accuracy chosen to be very low, we hard reset position
+		if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected || isHorizontalPositionAidingActive()) {


Why do we want to trigger a hard reset when we have horizontal position aiding?
I see that we already had _!ekf.control_status_flags().gnss_pos as a condition and I understand the intent. One wants to quickly reset to the correct position so that AGP/EV don’t remain stuck, but wouldn’t it still be preferable to perform a fusion-based reset when the variance / test-ratio allows it?

bresch added 2 commits July 15, 2025 09:00

ekf-agp: add reset mode behavior

1a03def

ekf2: allow manual position reset when horizontal aiding is active

0beb23c

This allows the pilot to override position esitmates manually

bresch requested a review from haumarco July 15, 2025 11:55

bresch self-assigned this Jul 15, 2025

bresch added this to PX4 EKF Jul 15, 2025

bresch added the EKF2 label Jul 15, 2025

haumarco reviewed Jul 15, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

EKF2: Aux Globlal Pos (AGP) reset mode #25223

EKF2: Aux Globlal Pos (AGP) reset mode #25223

bresch commented Jul 15, 2025

Uh oh!

github-actions bot commented Jul 15, 2025

Uh oh!

haumarco Jul 15, 2025

Uh oh!

haumarco Jul 15, 2025

Uh oh!

Uh oh!

EKF2: Aux Globlal Pos (AGP) reset mode #25223

Are you sure you want to change the base?

EKF2: Aux Globlal Pos (AGP) reset mode #25223

Conversation

bresch commented Jul 15, 2025

Solved Problem

Solution

Changelog Entry

Test coverage

Uh oh!

github-actions bot commented Jul 15, 2025

🔎 FLASH Analysis

Uh oh!

haumarco Jul 15, 2025

Choose a reason for hiding this comment

Uh oh!

haumarco Jul 15, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!