Skip to content

EKF2: gnss reset improvements #25297

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

Jump to bottom
Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

EKF2: gnss reset improvements #25297

wants to merge 7 commits into from
+770 −636

Conversation

haumarco
Copy link
Contributor

Solved Problem

As already implemented by @bresch in the AGP, he also added the same logic for GNSS.
This allows us to avoid position resets during crude GNSS spoofing.

See description:
#25223

Test coverage

tested in simulation with GNSS jumps

bresch and others added 6 commits July 22, 2025 13:40
@bresch @haumarco
ekf2: allow manual position reset when horizontal aiding is active
5900d82 
This allows the pilot to override position esitmates manually
@bresch @haumarco
mavlink sim: add support of failure gps struck
837140c
@bresch @haumarco
mavlink sim: add GNSS failure "wrong" type
e6d8bd3
@bresch @haumarco
ekf2-gnss: add reset mode
6e45d93 
This allows the user to choose whether the position should immediately
be reset to GNSS on fusion timeout or if the EKF can continue with
velocity dead-reckoning.
@bresch @haumarco
ekf2: fix unit test changes due to GNSS start logic
b2728c4 
Especially because the EKF doesn't need to reset the states if the test
ratio is already passing
@haumarco
rename mode enum
034d285
@haumarco haumarco requested a review from bresch July 25, 2025 07:19
@haumarco haumarco added the EKF2 label Jul 25, 2025
@github-actions GitHub Actions
Copy link

github-actions bot commented Jul 25, 2025 •
edited
Loading

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 464 byte (0.02 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +464  +0.0%    +464    .text
  [NEW]    +196  [NEW]    +196    Ekf::isGnssPosResetAllowed()
  [NEW]    +196  [NEW]    +196    Ekf::isGnssVelResetAllowed()
  [NEW]    +112  [NEW]    +112    Ekf::stopGnssVelFusion()
  [NEW]    +108  [NEW]    +108    Ekf::stopGnssPosFusion()
   +53%    +100   +53%    +100    Ekf::controlGnssPosFusion()
  +2.0%     +68  +2.0%     +68    Ekf::Ekf()
  +1.5%     +40  +1.5%     +40    EKF2::PublishAidSourceStatus()
 -99.9%     +40 -99.9%     +40    [61 Others]
  +0.0%     +36  +0.0%     +36    g_cromfs_image
  +0.6%     +32  +0.6%     +32    EKF2::EKF2()
  +4.0%     +32  +4.0%     +32    Ekf::resetGlobalPosToExternalObservation()
  +9.9%     +28  +9.9%     +28    Ekf::stopGnssFusion()
  +4.7%     +20  +4.7%     +20    param_reset_specific
  +0.0%     +16  +0.0%     +16    [section .text]
  +0.8%     +12  +0.8%     +12    EKF2::updateParamsImpl()
  -8.7%     -16  -8.7%     -16    EKF2::PublishBaroBias()
  -2.0%     -20  -2.0%     -20    Ekf::controlGpsFusion()
  -1.4%     -28  -1.4%     -28    Ekf::controlOpticalFlowFusion()
  -1.2%     -40  -1.2%     -40    EKF2::Run()
 -46.4%    -180 -46.4%    -180    Ekf::controlGnssVelFusion()
  [DEL]    -288  [DEL]    -288    Ekf::shouldResetGpsFusion()
+0.0%     +54  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%    +128  [ = ]       0    .debug_frame
+0.0% +3.12Ki  [ = ]       0    .debug_info
+0.0%    +683  [ = ]       0    .debug_line
 -75.0%      -3  [ = ]       0    [Unmapped]
  +0.0%    +686  [ = ]       0    [section .debug_line]
+0.0% +1.44Ki  [ = ]       0    .debug_loclists
+0.0%    +258  [ = ]       0    .debug_rnglists
  [NEW]      +2  [ = ]       0    [Unmapped]
  +0.0%    +256  [ = ]       0    [section .debug_rnglists]
+0.0% +1.30Ki  [ = ]       0    .debug_str
+1.3%      +3  [ = ]       0    .shstrtab
+0.0%     +93  [ = ]       0    .strtab
  [NEW]     +34  [ = ]       0    Ekf::isGnssPosResetAllowed()
  [NEW]     +34  [ = ]       0    Ekf::isGnssVelResetAllowed()
  [DEL]     -33  [ = ]       0    Ekf::shouldResetGpsFusion()
  [NEW]     +29  [ = ]       0    Ekf::stopGnssPosFusion()
  [NEW]     +29  [ = ]       0    Ekf::stopGnssVelFusion()
   +50%     +16  [ = ]       0    __nxsched_remove_blocked_veneer
 -40.0%     -16  [ = ]       0    __nxsem_tickwait_veneer
+0.0%     +96  [ = ]       0    .symtab
 -50.0%     -16  [ = ]       0    ConstLayer::containedAsBitset()
  +100%     +16  [ = ]       0    EKF2::PublishAidSourceStatus()
  +100%     +16  [ = ]       0    EKF2::PublishGpsStatus()
 -50.0%     -16  [ = ]       0    EKF2::PublishWindEstimate()
   +50%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
  -9.1%     -16  [ = ]       0    EKF2::Run()
 -25.0%     -16  [ = ]       0    EKF2::VerifyParams()
 -33.3%     -16  [ = ]       0    EKF2::print_usage()
  +100%     +16  [ = ]       0    EKF2::updateParamsImpl()
   +33%     +16  [ = ]       0    Ekf::controlEvHeightFusion()
   +50%     +32  [ = ]       0    Ekf::controlEvPosFusion()
 -66.7%     -32  [ = ]       0    Ekf::controlGnssPosFusion()
 -66.7%     -32  [ = ]       0    Ekf::controlGnssVelFusion()
 -50.0%     -16  [ = ]       0    Ekf::get_ekf_lpos_accuracy()
  [NEW]     +16  [ = ]       0    Ekf::isGnssPosResetAllowed()
  [NEW]     +16  [ = ]       0    Ekf::isGnssVelResetAllowed()
   +33%     +16  [ = ]       0    Ekf::reset()
   +50%     +16  [ = ]       0    Ekf::resetVelocityToGnss()
  +100%     +16  [ = ]       0    Ekf::resetWindCov()
 -25.0%     -16  [ = ]       0    Ekf::resetWindToExternalObservation()
 -99.4%     +80  [ = ]       0    [17 Others]
-4.8%    -464  [ = ]       0    [Unmapped]
+0.0% +7.19Ki  +0.0%    +464    TOTAL
px4_fmu-v6x [Total VM Diff: 440 byte (0.02 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +440  +0.0%    +440    .text
  [NEW]    +196  [NEW]    +196    Ekf::isGnssPosResetAllowed()
  [NEW]    +196  [NEW]    +196    Ekf::isGnssVelResetAllowed()
  [NEW]    +112  [NEW]    +112    Ekf::stopGnssVelFusion()
  [NEW]    +108  [NEW]    +108    Ekf::stopGnssPosFusion()
   +53%    +100   +53%    +100    Ekf::controlGnssPosFusion()
  +2.0%     +68  +2.0%     +68    Ekf::Ekf()
  +0.0%     +56  +0.0%     +56    g_cromfs_image
  +1.5%     +40  +1.5%     +40    EKF2::PublishAidSourceStatus()
  +0.6%     +32  +0.6%     +32    EKF2::EKF2()
  +4.0%     +32  +4.0%     +32    Ekf::resetGlobalPosToExternalObservation()
  +9.9%     +28  +9.9%     +28    Ekf::stopGnssFusion()
 -100.0%     +28 -100.0%     +28    [52 Others]
  +0.0%     +16  +0.0%     +16    [section .text]
  +0.8%     +12  +0.8%     +12    EKF2::updateParamsImpl()
  -0.8%     -12  -0.8%     -12    Ekf::controlBaroHeightFusion()
  -8.7%     -16  -8.7%     -16    EKF2::PublishBaroBias()
  -2.0%     -20  -2.0%     -20    Ekf::controlGpsFusion()
  -1.4%     -28  -1.4%     -28    Ekf::controlOpticalFlowFusion()
  -1.2%     -40  -1.2%     -40    EKF2::Run()
 -46.4%    -180 -46.4%    -180    Ekf::controlGnssVelFusion()
  [DEL]    -288  [DEL]    -288    Ekf::shouldResetGpsFusion()
+0.0%     +54  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%    +108  [ = ]       0    .debug_frame
+0.0% +3.04Ki  [ = ]       0    .debug_info
+0.0%    +627  [ = ]       0    .debug_line
   +75%      +3  [ = ]       0    [Unmapped]
  +0.0%    +624  [ = ]       0    [section .debug_line]
+0.0% +1.38Ki  [ = ]       0    .debug_loclists
+0.0%    +249  [ = ]       0    .debug_rnglists
  +200%      +2  [ = ]       0    [Unmapped]
  +0.0%    +247  [ = ]       0    [section .debug_rnglists]
+0.0% +1.30Ki  [ = ]       0    .debug_str
+1.3%      +3  [ = ]       0    .shstrtab
+0.0%     +93  [ = ]       0    .strtab
  [NEW]     +34  [ = ]       0    Ekf::isGnssPosResetAllowed()
  [NEW]     +34  [ = ]       0    Ekf::isGnssVelResetAllowed()
  [DEL]     -33  [ = ]       0    Ekf::shouldResetGpsFusion()
  [NEW]     +29  [ = ]       0    Ekf::stopGnssPosFusion()
  [NEW]     +29  [ = ]       0    Ekf::stopGnssVelFusion()
+0.0%     +96  [ = ]       0    .symtab
  +100%     +16  [ = ]       0    EKF2::PublishAidSourceStatus()
  +100%     +16  [ = ]       0    EKF2::PublishGpsStatus()
 -50.0%     -16  [ = ]       0    EKF2::PublishWindEstimate()
   +50%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
  -9.1%     -16  [ = ]       0    EKF2::Run()
 -25.0%     -16  [ = ]       0    EKF2::VerifyParams()
 -33.3%     -16  [ = ]       0    EKF2::print_usage()
  +100%     +16  [ = ]       0    EKF2::updateParamsImpl()
   +67%     +32  [ = ]       0    Ekf::controlEvHeightFusion()
   +50%     +32  [ = ]       0    Ekf::controlEvPosFusion()
 -66.7%     -32  [ = ]       0    Ekf::controlGnssPosFusion()
 -66.7%     -32  [ = ]       0    Ekf::controlGnssVelFusion()
 -50.0%     -16  [ = ]       0    Ekf::get_ekf_lpos_accuracy()
  [NEW]     +16  [ = ]       0    Ekf::isGnssPosResetAllowed()
  [NEW]     +16  [ = ]       0    Ekf::isGnssVelResetAllowed()
   +33%     +16  [ = ]       0    Ekf::reset()
   +50%     +16  [ = ]       0    Ekf::resetVelocityToGnss()
  +100%     +16  [ = ]       0    Ekf::resetWindCov()
 -25.0%     -16  [ = ]       0    Ekf::resetWindToExternalObservation()
  [DEL]     -16  [ = ]       0    Ekf::shouldResetGpsFusion()
 -99.5%     +64  [ = ]       0    [8 Others]
-8.7%    -440  [ = ]       0    [Unmapped]
+0.0% +6.96Ki  +0.0%    +440    TOTAL

Updated: 2025-07-25T14:35:38

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@bresch bresch Awaiting requested review from bresch

At least 1 approving review is required to merge this pull request.

Assignees
No one assigned
Labels
EKF2
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@haumarco @bresch