fix(ekf2): avoid failover to an instance with a failing test ratio [1.18] - #28533
Merged
Merged
Conversation
…X4#28418) * fix(ekf2): keep evaluating instance selection while the primary is unhealthy The selection block only ran when UpdateErrorScores() reported a change: a new instance appearing or a health transition. A primary that stops publishing produces exactly one such transition, on the cycle its timeout is detected. If no switch happens on that cycle, nothing sets updated again: the stable alternatives do not count as primary updates and the timed out instance is skipped thereafter, so the fallback logic is never evaluated again even though the module keeps being scheduled. Today that single evaluation always resolves the situation, because the fallback switches unconditionally to the best healthy candidate on that same cycle. But any selection policy that can decline to switch on the transition cycle, for example one that waits out a transient fault, needs the decision re-evaluated while the primary remains unhealthy. Re-enter the selection block whenever the selected instance is unhealthy. The decisions inside are unchanged and switching is idempotent, so behaviour today is identical. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * fix(ekf2): do not fail over to an instance with a sustained test ratio failure When the primary EKF goes unhealthy the selector falls back to the best instance that is healthy, and healthy only requires zero filter fault flags and a positive combined test ratio. An instance whose test ratio has been failing for a long time therefore remains a first class switch target even though its state can be far from the truth. b7efd4f introduced this on purpose for the switch-away direction: a test ratio at or above one became a warning rather than ill health, so a transient ratio spike cannot hard fail an instance, and a warned primary is left through the lower relative error path once the warning has been sustained for one second. What that commit did not do is apply the same reasoning to the switch-to direction. The candidate loop only filters on healthy, so a brief hard fault on the primary, for example transient accelerometer clipping, sends the selector straight to a diverged instance. That is the mechanism behind the repeated altitude jumps in issue 27013: one instance had stopped fusing baro, its vertical state up to 155.6 m from the other instance while its combined test ratio sat pegged at 2, and each of the seven short clipping faults on the good instance bounced the selector back to it (15 instance switches in total counting the returns), the worst switch stepping the published altitude by 128.5 m and provoking a hard TECS reaction. Classify fallback candidates with the same sustained warning test the switch-away trigger already uses. When the primary goes unhealthy, fail over immediately to the best candidate without a sustained warning; the different IMU preference is kept within each tier, and a candidate without a sustained warning is preferred even over a warned candidate on a different IMU, since a warned instance is the one known to be diverging. A sustained warned candidate is accepted in two cases only: the primary has timed out entirely, where frozen attitude and position outputs are worse than any live alternative, or the primary has been continuously unhealthy for kWarnedFallbackDelay (five seconds), so a brief fault rides out on the current state while a persistently faulted primary still gets the least bad alternative rather than none. The re-evaluation of this decision while the primary stays unhealthy is provided by the previous commit. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> * test(ekf2): add a functional test for the instance selector The selector had no test at any level. This drives EKF2Selector through published multi instance estimator_status messages on the real work queue and observes estimator_selector_status, so the selection policy is exercised without a simulator. The scenarios encode the failure pattern from issue 27013 and the no-whipsaw property discussed there: a clean fallback on a hard primary fault stays immediate, the switch away from a degraded primary through the sustained warning path still works, three separate brief hard faults on the primary no longer bounce the selector to an instance whose test ratio has been failing for seconds, a primary that stops publishing falls back to the degraded instance without delay, and a primary that stays hard faulted for longer than the ride-out window still falls back rather than being kept forever. The scenario setup helpers run until the selector reaches the intended starting state rather than assuming fixed timings, since the health hysteresis and warning windows run on wall clock time. The fault-clear windows exceed the selector's one second healthy hysteresis so the faults are genuinely separate and the unhealthy-since tracking restarts between them. The harness waits until the work queue manager actually serves queues before constructing the selector: a fixed delay races the manager startup on a loaded runner. to run: make tests TESTFILTER=EKF2Selector Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> --------- Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu> (cherry picked from commit 436cc71)
github-actions
Bot
added
kind:bug
dakejahl
approved these changes
Sep 4, 2026
Contributor
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 344 byte (0.02 %)]px4_fmu-v6x [Total VM Diff: 336 byte (0.02 %)]Updated: 2026-09-04T01:35:45 |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Backport of #28418 to
release/1.18, the same commit as on main.The selector could fall back to an instance whose vertical state was already failing, which is what produced the altitude spikes in #27013.