Skip to content

AP_NavEKF3: keep baro ground effect out of a height source switch - #34432

Merged
peterbarker merged 2 commits into
ArduPilot:masterfrom
andyp1per:pr-baro-offset-gnd-effect
Sep 23, 2026
Merged

peterbarker merged 2 commits into
ArduPilot:masterfrom
andyp1per:pr-baro-offset-gnd-effect

Conversation

@andyp1per

Copy link
Copy Markdown
Contributor

Summary

With EK3_RNG_USE_HGT set and a baro that sees ground effect, a copter's EKF height reads about 2.5 m high for the whole flight. The baro offset learns the ground effect error while the range finder is the height source during the takeoff, and carries it through the switch back to baro. This stops the offset learning, and skips the height reset to the baro, while takeoff or touchdown is expected.

Classification & Testing (check all that apply and add your own)

  • Checked by a human programmer
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)

New autotest BaroGroundEffectRangefinderSwitch: ALT_HOLD takeoff with SIM_BARO_GEFF_M 3, EK3_RNG_USE_HGT 50 and a range finder that reads on the ground, EKF height checked against the SITL truth up to the switch to LAND.

build worst, arming to 5 m mean above 5 m
master -0.66 m +2.50 m
offset held, reset kept -3.05 m +0.14 m
reset skipped, offset learned -0.01 m +2.47 m
this PR -0.17 m +0.03 m

Also passing: Copter TakeoffGroundEffectAlt, TouchdownGroundEffectAlt, RangeFinder, SurfaceTracking, FlyRangeFinderSITL, RangeFinderPowerDown, EK3_RNG_USE_HGT and Replay; QuadPlane Mission, PrecisionLanding, LoiterAltQLand, MAV_CMD_NAV_TAKEOFF, VTOLLandGoAround and TakeoffCheck.

Description

Copter trusts the range finder for height only while taking off or landing (set_terrain_hgt_stable()). ALT_HOLD's takeoff ends as the vehicle leaves the ground, so the height source goes from range finder back to baro right at liftoff, still in ground effect. Two things then carry the baro error into the flight: calcFiltBaroOffset() has been learning baro minus range height through the spool-up, and the source switch resets the height to that baro. Holding only the offset leaves the reset to drop the height by the whole error at liftoff; skipping only the reset leaves the offset to carry it.

Both now key on takeoff or touchdown expected, the flags the height fusion dead zone already uses. The baro is still fused at its ground effect weighting, and the height converges on it once the flags clear.

Baro drift is not learned while the flags are set. I tried letting the offset rise but not fall, since ground effect only reads low, in two forms. Neither did better in SITL: one lets baro noise ratchet the offset upwards (0.07 m after a 10 s armed wait with SIM_BARO_RND 0.2), and the other needs its floor reset everywhere the offset is.

Fixed wing sets the same flags for a launch rather than for rotor wash, so fly-forward vehicles are left as they were. QuadPlane in VTOL gets the change.

Found while testing #32232, whose on-ground range substitution sends a sensor that reads out of range low on the ground down this path too. On master it takes a range finder that reads on the ground. Only the takeoff is tested; touchdown goes through the same two lines.

With EK3_RNG_USE_HGT, Copter uses a range finder that reads on the ground
for height while taking off, so the baro offset filter runs while the
motors spool up and learns the prop-wash baro error as offset. ALT_HOLD's
takeoff ends as the vehicle leaves the ground, the source goes back to baro
there, and the offset carries the error into the flight: in SITL with
SIM_BARO_GEFF_M 3 the EKF height read 2.5 m high from then until landing.

While takeoff or touchdown is expected, do not update the offset, and do
not reset the height to the baro on switching to it. Either alone is not
enough: with only the offset held, the reset drops the height by the whole
error at liftoff (3.0 m); with only the reset skipped, the offset still
carries it (2.5 m). With both, -0.17 m worst in the takeoff and 0.03 m mean
above 5 m.

Baro drift is not learned while the flags are set, which on Copter
includes the whole time it sits armed on the ground. Learning only the
upward part of the error was tried: in SITL it did no better, and baro
noise ratchets it upwards (0.07 m after a 10 s armed wait with
SIM_BARO_RND 0.2). Fixed wing sets the flags for a launch rather than for
rotor wash, so it is left as it was.
Fails without the fix (EKF height 2.5 m high above 5 m), with only the
baro offset held (3.0 m drop at liftoff) and with only the reset skipped
(2.5 m high). Takeoff only; the touchdown half of the fix is not covered.
@AP-Review

AP-Review commented Sep 19, 2026 •

Copy link
Copy Markdown

Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting.

Full report: https://uav.tridgell.net/DevCallReviews/2026_09_23/devcall_pr_reviews.html#pr34432

Reviewed at head f2be29f74e. COMMENT — the logic is correct for the takeoff case it targets and I found no bug at the default configuration. Two independent passes agreed on the verdict, and CI is 102/102 green.

I reproduced your A/B table in SITL rather than taking it on trust — built at this head, then rebuilt with each guard individually disabled (patching only the !baroInGndEffect terms at :1343/:1447, keeping the variable referenced so it still compiled):

build worst, arming→5 m mean above 5 m your table
master (both guards off) −0.65 m +2.50 m −0.66 / +2.50
offset held, reset kept −2.99 m (test raises here) — −3.05 / +0.14
reset skipped, offset learned −0.01 m +2.47 m −0.01 / +2.47
this PR −0.17 m +0.03 m −0.17 / +0.03

So the test is genuinely RED on master, RED for each half of the fix alone, and GREEN only with both — the "either alone is not enough" claim holds.

1. touchdown_expected is not altitude-bounded, so both suppressions reach an emergency fallback — re-raised

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:1343 and :1447. My 2026-09-19 note already raised this and proposed the same narrowing; it is unanswered, so this is a re-raise rather than a new finding — but both passes this round reached it independently, and the second one showed it is less niche than I said.

AP_GroundEffect::update() drops the near-ground gate irrespective of height in two branches (libraries/AP_GroundEffect/AP_GroundEffect.cpp:149-158): GNDEFF_ALT <= 0 (the documented legacy mode), and drift_ne_m >= AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M (20.0 m, :32) when HAGL is unavailable. HAGL needs gndOffsetValid, which needs a rangefinder or an optflow terrain estimate (AP_NavEKF3_OptFlowFusion.cpp:42) — so every copter without a rangefinder takes that second branch. Combined with slow_horizontal (≤1.25 m/s) and slow_descent, such a vehicle more than 20 m from its takeoff point, descending gently, has touchdown_expected true at any altitude, for as long as the descent lasts.

In that state this PR (a) freezes baroHgtOffset for the whole descent — potentially minutes, where the description's "baro drift is not learned while the flags are set" caveat reads as if the window is short — and (b) suppresses the snap on the emergency fallback_to_baro path (:1319-1333: lost GPS height, lost external nav, lost beacon), where the snap was the mechanism that made baro immediately consistent.

Scope, stated plainly so this is not overblown: with EK3_SRC1_POSZ = BARO (the default) activeHgtSource is already BARO, so neither suppression fires and the vast majority of users are unaffected — both hunks are dead code for them. It is the GPS / EXTNAV / BEACON height-source configurations that see it, and it is the one place the change touches a sensor-loss fallback.

Note prevHgtSource is overwritten at :1445 before the guard at :1447, so the outgoing source has to be captured where it is still valid. Cleanest form:

const bool baroInGndEffect = (dal.get_takeoff_expected() || dal.get_touchdown_expected()) &&
                             !assume_zero_sideslip() &&
                             (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER ||
                              prevHgtSource   == AP_NavEKF_Source::SourceZ::RANGEFINDER);

That keeps the targeted rangefinder→baro case (including the lostRngHgt fallback, i.e. the #32232 path) and restores the old snap for a GPS/extnav/beacon height loss at altitude. The trade-off is real and yours to judge — an indoor extnav→baro switch during a genuine VTOL takeoff would lose the protection, and its baro is equally in ground effect. Is there a reason you preferred the wider form? A sentence either way would close this out.

2. The innovation floor inverts at EK3_GND_EFF_DZ = 0

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:1117. Pre-existing on master, but this PR makes it load-bearing: with the reset skipped, the only thing bringing the held height back onto baro is fusion, and the only thing stopping ground effect dragging it down is this floor.

constrain_ftype(-innovVelPos[5] + gndBaroInnovFloor, 0.0f, gndBaroInnovFloor + gndMaxBaroErr) with gndBaroInnovFloor = -0.5. When EK3_GND_EFF_DZ is 0 — a documented setting ("Set to 0 if no ground effect is present", AP_NavEKF3.cpp:735) — the bounds become (0.0, -0.5), i.e. low > high. constrain_value_line() tests amt < low first and amt > high second (AP_Math.cpp:295-303), so instead of a zero dead zone every negative innovation picks up an extra −0.5 m. Evaluated against those exact semantics:

  • EK3_GND_EFF_DZ = 4.0 (default): raw −1.00 → −0.50, −3.00 → −0.50, −4.00 → −0.50, −6.00 → −2.50, −10.00 → −6.50
  • EK3_GND_EFF_DZ = 0.0: raw −1.00 → −1.50, −3.00 → −3.50, −10.00 → −10.50

Two things fall out. At the default the floor does what the fix needs, but note it attenuates rather than bounds — a −10 m error still fuses as −6.5 m — so it is worth stating in the description what ground-effect magnitude the design is meant to be safe for. And at DZ = 0 the held height is pulled toward the ground-effect baro harder than the raw innovation would, so the fix silently degrades for a user who has declared "no ground effect". Only DZ = 0 is affected; at DZ = 0.5 the bounds are (0.0, 0.0), already a no-op. Either fix the bound — constrain_ftype(..., 0.0f, MAX(gndBaroInnovFloor + gndMaxBaroErr, 0.0f)) — or key baroInGndEffect on is_positive(frontend->_baroGndEffectDeadZone) so the documented opt-out also opts out of the new behaviour.

3. Notes

  • The new autotest can pass vacuously — but not the way I said last time. My earlier draft of this finding claimed a GNDEFF_ALT/GNDEFF_TMO default change would make it pass vacuously; that is wrong, and I am correcting it rather than letting it stand: if the flags are not latched at the switch, ResetPositionD runs, the filter snaps to the ground-effect baro, and the test fails with master's +2.5 m. The real vacuous mode is different — if RNGFND1_TYPE/EK3_RNG_USE_HGT/the switch-height logic ever change such that the source stays BARO throughout, there is no offset learning and no source switch, the existing dead zone absorbs the error, and Tools/autotest/arducopter.py:15363-15423 passes while exercising nothing. Worth asserting from the same log that the scenario had teeth — e.g. that RFND reads valid on the ground, and/or that BARO.Alt under-reads SIM by >1 m while armed below 1 m. Pinning GNDEFF_ALT/GNDEFF_TMO alongside the other parameters is still worth doing to make the window explicit. (context_pop() does restore the parameters, so the teardown is fine.)
  • prevHgtSource is consumed even when the reset is skipped (:1445), so if ground effect clears a cycle later while still on baro there is no second chance and convergence is left entirely to fusion. I think that is the right call — deferring it would move the discontinuity rather than remove it, and your |mean_high| ≤ 0.5 m assertion shows it converges — recording it so the choice is visible.
  • The comment at :1335-1337 reads wider than the code. assume_zero_sideslip() is on fly-forward operation, not on being a fixed wing: Plane::update_fly_forward() also clears fly_forward for in_vtol_mode(), in_assisted_flight(), idle_mode and a LAND stage where landing.is_flying_forward() is false, so a QuadPlane in assisted forward flight gets baroInGndEffect. Arguably what you want (lift motors running), but the comment says "Fixed wing sets those flags for a launch … so it is left as it was", which reads as a vehicle-type exclusion. No practical impact for a fixed-wing launch.

Checked and clean, so these do not get re-asked

The skipped reset cannot cause an innovation rejection or a hgtTimeout reset — computed, not assumed. With Copter defaults EK3_ALT_M_NSE 2.0, gndEffectBaroScaler 4.0 and EK3_HGT_I_GATE 500, R_OBS_DATA_CHECKS[5] = 16.0 and rejection needs |innov| > 20.3–21.2 m for P[9][9] in 0.5–2.0; the badIMUdata detector at :880 needs |hgtErr| > 12 m and a co-signed vertical velocity error. For scale, the SITL model (AP_Baro_SITL.cpp:69-77) with SIM_BARO_GEFF_M 3.0 gives 2.70 m of under-read at 0.2 m AGL.

Also: flag clearing on every exit path (no stuck-flag path found — takeoff_expected is capped at 5 s hard and both flags time out after 1000 ms); every transition into and out of each height source (BARO→RANGEFINDER still resets; prevHgtSource seeded at core.cpp:356-357 so no spurious first-switch reset; the other two ResetPositionD sites at :631/:642 are gated on a non-BARO source and untouched); fusion gating entirely untouched, and a frozen baroHgtOffset can only make a good baro sample look bad, never let a bad one through; ResetPositionD() touches no covariance, so skipping it introduces no asymmetry; replay determinism (both flags come from the logged RFRN record, no logged field changes meaning); two commits, one per subsystem; flash cost is one bool and two branch terms.

Not covered: the long-freeze case in finding 1 was not evaluated against a real baro drift trace, and no hardware was involved.

@peterbarker
peterbarker merged commit 66c8985 into ArduPilot:master Sep 23, 2026
102 checks passed
@andyp1per
andyp1per deleted the pr-baro-offset-gnd-effect branch September 23, 2026 08:47
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants