[RFC] EKF2 altitude semantics for Flow+Range and Multi-EKF

Describe problem solved by the proposed feature

This feature request proposes a set of EKF2 improvements for indoor/no-GPS configurations using Optical Flow + Distance Sensor, and for multi-EKF configurations where the selected estimator instance can change during flight.

The main problems are:

• vehicle_local_position.z can be hard to interpret because its meaning depends on the active EKF height reference.
• It is not always clear whether range is the active height reference or only an aiding/terrain source.
• EKF2 can fall back from the configured height reference to another source without a clear user-facing notification.
• Sudden rangefinder steps over discontinuous surfaces can affect the controller-facing local Z output.
• Switching the primary EKF instance can cause a vertical output discontinuity if different EKF instances have different local Z origins.

A key ambiguity is the difference between:

Range is fused

and:

Range is the active height reference

These two cases can produce different altitude behavior, but they may look similar to users in logs or QGroundControl.

Related issues:

• https://github.com/PX4/PX4-Autopilot/issues/24653
• https://github.com/PX4/PX4-Autopilot/issues/26226
• https://github.com/PX4/PX4-Autopilot/issues/27110
• https://github.com/3drobotics/PX4Firmware/issues/3 as historical/background context

Prototype branch for early review:

https://github.com/vogiahuy257/PX4-Autopilot/tree/dev/ekf2-altitude-rfc

This branch is intended for discussion and early feedback. It is not intended as a final merge-ready PR yet.I would appreciate feedback on the proposed separation before I split this into smaller PRs.

Describe your preferred solution

I would like to propose separating the solution into four parts:

1. Expose EKF2 height reference semantics.
2. Add user-facing events when EKF2 changes or falls back between height sources.
3. Add optional stable-floor handling for sudden range steps.
4. Keep the controller-facing vertical output continuous during multi-EKF instance handover.

1. Expose EKF2 height semantics

Reason

At the moment, it is not always clear from logs whether range is only being fused as an aiding source or whether range is actually the active height reference.

This distinction is important because:

• vehicle_local_position.z may not have the same meaning in every EKF height configuration.
• Range may be used for aiding or terrain/optical-flow support without being the main height reference.
• A fallback from the configured height reference to another source can change altitude behavior without being obvious to the user.

For example, a user may assume:

Distance sensor is available and fused

therefore:

vehicle_local_position.z is range-referenced

However, this is not always true. Range can be used as an aiding or terrain source while another source, such as barometer, GNSS, or external vision, remains the active height reference.

Proposed change

Add an estimator_height_status uORB topic to make the active height reference easier to debug from logs.

Example fields:

configured_ref
active_ref
fuse_mask
range_role
semantics
ref_fallback
range_aid_only

Possible range_role values:

OFF
AIDING_ONLY
HEIGHT_REFERENCE
TERRAIN_ONLY

The goal is to distinguish:

Range is fused

from:

Range is the active height reference

This would make vehicle_local_position.z semantics clearer when using:

EKF2_HGT_REF
EKF2_RNG_CTRL
range aid
terrain estimation
height reference fallback

2. Add user-facing height reference transition events

Reason

The status topic above is useful for logs and debugging, but it is still not enough for normal users.

A user may not continuously inspect estimator_height_status. If EKF2 loses data from the configured height sensor and silently falls back to another height source, the user may only see the result as unexpected altitude behavior.

This can cause confusion such as:

I configured range as height reference, but altitude behavior looks baro-like.

or:

The distance sensor is connected, but the vehicle seems to use another height source.

For this reason, I propose adding user-facing events using events::send() when EKF2 changes the active height reference or falls back to another source.

Proposed change

Use events::send() to notify the user when the height reference state changes.

Example events:

EKF2 height reference changed: BARO -> RANGE
EKF2 height reference fallback: configured RANGE, using BARO
EKF2 height reference recovered: active RANGE
Range is fused as aiding only, not active height reference

The intended separation is:

estimator_height_status  -> detailed log/debug state
events::send()           -> user-facing notification when the state changes

Example logic:

if (active_ref != _last_active_ref) {
    events::send(events::ID("ekf2_height_ref_changed"),
                 events::Log::Info,
                 "EKF2 height reference changed");
}

if (configured_ref != active_ref) {
    events::send(events::ID("ekf2_height_ref_fallback"),
                 events::Log::Warning,
                 "EKF2 height reference fallback");
}

if ((configured_ref == active_ref) && _was_in_fallback) {
    events::send(events::ID("ekf2_height_ref_recovered"),
                 events::Log::Info,
                 "EKF2 height reference recovered");
}

The exact message text can be adjusted to follow PX4 event style, but the key point is to make sensor loss and fallback behavior visible to the user.

Expected benefit:

• Users can see when EKF2 is no longer using the configured height source.
• Logs become easier to interpret.
• QGroundControl can show meaningful warnings instead of users only noticing unexplained altitude behavior.
• Developers can correlate altitude jumps or changes with height reference transitions.

3. Optional stable-floor / range step handling

Reason

For indoor Flow + Distance Sensor use cases, sudden rangefinder changes can happen when flying over:

• furniture,
• floor edges,
• stairs,
• uneven surfaces,
• objects below the vehicle.

In these cases, the rangefinder measurement may change suddenly even though the vehicle itself has not moved vertically by the same amount.

If this range step is treated as a true vehicle altitude change, the controller-facing local Z output may jump. This is especially problematic for indoor position hold or terrain/range-based altitude behavior.

Proposed change

Add an optional stable-floor mode for range height handling.

Example mode:

EKF2_RNG_HGT_MD = 1  # Stable floor

Proposed parameters:

The stable-floor logic detects whether a range change looks like a surface change rather than true vehicle vertical motion.

Stable-floor decision diagram

flowchart TD
    A["Range height fusion update"] --> B{"EKF2_RNG_HGT_MD == Stable floor?"}

    B -- "No" --> C["Legacy range height fusion"]
    C --> C1["update_height = true<br/>update_terrain = true"]

    B -- "Yes" --> D["Read inputs<br/>range residual r<br/>vertical velocity v_z"]
    D --> E["Read parameters<br/>g = EKF2_RNG_SF_GATE<br/>v_d = EKF2_RNG_SF_D_VZ<br/>v_c = EKF2_RNG_SF_C_VZ<br/>T_hold = EKF2_RNG_SF_HOLD"]

    E --> F{"Raised surface detected?"}

    F -- "Yes" --> G["Raised surface or obstacle below"]
    G --> G1["Block height rebasing<br/>update_height = false<br/>update_terrain = true<br/>hold for T_hold"]

    F -- "No" --> H{"Lower surface detected?"}

    H -- "Yes" --> I["Lower surface or floor drop"]
    I --> I1["Block height rebasing<br/>update_height = false<br/>update_terrain = true<br/>hold for T_hold"]

    H -- "No" --> J["Normal floor"]
    J --> J1["update_height = true<br/>update_terrain = true"]

    G1 --> K{"Hold expired or residual back inside gate?"}
    I1 --> K

    K -- "No" --> L["Keep stable-floor protection active"]
    L --> G1

    K -- "Yes" --> M["Recover to normal floor logic"]
    M --> J1

Suggested decision labels for the implementation/design document:

raised_surface = (r > g) and (v_z < v_d)

lower_surface = (r < -g) and (v_z > -v_c)

Expected behavior for raised/lower surface discontinuity:

update_height = false
update_terrain = true

Reason:

• Do not immediately rebase local Z to a sudden surface change.
• Still keep terrain/range information available where needed, especially for optical-flow scale.

If no surface discontinuity is detected:

update_height = true
update_terrain = true

4. EKF2Selector vertical output continuity

Reason

In multi-EKF configurations, different EKF instances may have different local Z origins. When EKF2Selector switches the primary instance, the raw candidate Z can be different from the previously published output.

This can cause jumps in controller-facing outputs:

vehicle_local_position.z
vehicle_odometry.position[2]

Proposed change

Add a vertical handover offset in EKF2Selector.

The selector still chooses the candidate using the existing PX4 estimator selection logic. The vertical handover layer does not replace the estimator selection logic.

It only decides whether to:

• switch directly, or
• switch while applying a vertical output offset.

EKF2Selector vertical handover diagram

flowchart TD
    A["Candidate selected by EKF2Selector"] --> B{"EKF2_SEL_Z_MODE enabled?"}

    B -- "No" --> C["Legacy switch<br/>offset_z = 0"]
    C --> Z["Publish selector output"]

    B -- "Yes" --> D["Read current output<br/>z_out_minus"]
    D --> E["Read candidate raw output<br/>z_candidate"]
    E --> F{"Both values valid?"}

    F -- "No" --> G["Fallback legacy switch<br/>offset_z = 0"]
    G --> Z

    F -- "Yes" --> H["Compute vertical difference<br/>Delta_z = z_candidate - z_out_minus"]
    H --> I{"abs(Delta_z) <= EKF2_SEL_Z_DGATE?"}

    I -- "Yes" --> J["Direct switch<br/>offset_z = 0"]
    J --> Z

    I -- "No" --> K["Offset switch"]
    K --> L["offset_z = z_out_minus - z_candidate"]
    L --> M["z_out_plus = z_candidate + offset_z"]
    M --> N["z_out_plus = z_out_minus"]
    N --> Z

    Z --> O["vehicle_local_position.z continuous"]
    Z --> P["vehicle_odometry.position_z continuous"]

    Q["Raw EKF instance outputs stay unchanged<br/>estimator_local_position_i.z"] -. "debug only" .-> Z

For testing, I keep this behind:

EKF2_SEL_Z_MODE
EKF2_SEL_Z_DGATE

EKF2_SEL_Z_MODE = 0 keeps legacy behavior.

EKF2_SEL_Z_MODE = 1 enables vertical output continuity during EKF2Selector handover.

EKF2_SEL_Z_DGATE defines the small threshold where direct switching is allowed without offset.

Important design choice:

Z difference is not used to reject the candidate.

The candidate is still selected by the existing EKF2Selector logic. The vertical handover layer only decides whether to switch directly or apply an output offset.

Expected behavior:

If abs(Delta_z) <= EKF2_SEL_Z_DGATE:
    offset_z = 0
    switch directly

If abs(Delta_z) > EKF2_SEL_Z_DGATE:
    offset_z = z_out_minus - z_candidate
    switch with vertical output offset

This keeps the controller-facing outputs continuous:

vehicle_local_position.z
vehicle_odometry.position[2]

The raw EKF instance outputs remain unchanged, for example:

estimator_local_position[0].z
estimator_local_position[1].z

Describe possible alternatives

Possible alternatives:

1. Keep the current behavior and rely on users to interpret vehicle_local_position.z from existing EKF and sensor logs. However, this can be difficult because the active height reference and range role are not always explicit.

2. Only document the difference between range aiding, range height reference, and terrain/range usage. This would help users understand the behavior, but it would not notify users when EKF2 loses one height source and falls back to another.

3. Add only estimator_height_status without events::send(). This would help developers during log analysis, but it would not provide a clear user-facing notification in QGroundControl.

4. Reject EKF instance switches when the candidate Z differs too much from the current output. I considered this, but I think it is not ideal because the candidate has already been selected by EKF2Selector based on estimator health and innovation checks. Therefore, the Z difference should only be used to decide direct switch vs offset switch, not to reject the candidate.

5. Apply the vertical offset inside each EKF instance. I think this is less clean because it modifies estimator-internal behavior. Applying the offset at the selector output keeps raw EKF outputs unchanged and only protects controller-facing outputs.

6. Fully trust every rangefinder step as a real altitude change. This is simple, but it can produce unwanted altitude jumps when the vehicle flies over indoor objects or discontinuous surfaces.

Additional context

Proposed PR split:

1. feat(ekf2): publish estimator height semantics status
2. feat(ekf2): add user-facing height reference transition events
3. feat(ekf2): add vertical output continuity for EKF2 selector handover
4. feat(ekf2): add optional stable-floor handling for range height fusion
5. docs(ekf2): document height reference, range aid, stable-floor, and selector handover behavior

Prototype branch for early review:

https://github.com/vogiahuy257/PX4-Autopilot/tree/dev/ekf2-altitude-rfc

This branch is for discussion and feedback only.

Test plan: height semantics and events

For height semantics/status, I plan to log:

estimator_height_status.configured_ref
estimator_height_status.active_ref
estimator_height_status.range_role
estimator_height_status.semantics
estimator_height_status.ref_fallback

Expected result:

• Logs should clearly show whether range is the active height reference or only an aiding/terrain source.
• Logs should show fallback behavior when the configured height reference is not usable.
• events::send() should notify the user when the active height reference changes, falls back, or recovers.

Test plan: stable-floor range handling

Relevant logged signals:

distance_sensor.current_distance
vehicle_local_position.z
vehicle_local_position.dist_bottom
vehicle_local_position.vz
estimator_height_status.range_role
estimator_height_status.active_ref

Expected result:

• Sudden range steps that are inconsistent with vehicle vertical motion should not immediately cause a large jump in controller-facing local Z.
• Terrain/range information should still be available for optical-flow scale where appropriate.

Test plan: selector handover

For the selector handover change, I plan to compare:

EKF2_SEL_Z_MODE = 0

against:

EKF2_SEL_Z_MODE = 1

Relevant logged signals:

estimator_selector_status.primary_instance
estimator_local_position[0].z
estimator_local_position[1].z
vehicle_local_position.z
vehicle_odometry.position[2]

Expected result:

• Raw estimator_local_position[i].z may differ between EKF instances.
• When primary_instance changes, vehicle_local_position.z should remain continuous with the proposed handover logic enabled.
• vehicle_odometry.position[2] should also remain continuous.
• With legacy behavior, a vertical discontinuity may still be visible if the selected candidate has a different local Z origin.

Feedback requested

I would appreciate feedback on:

1. Does it make sense to separate the problem into height semantics visibility, user-facing events, stable-floor range handling, and selector vertical handover continuity?
2. Is estimator_height_status an acceptable way to expose height reference semantics for logging and debugging?
3. Is events::send() appropriate for notifying users about height reference changes, fallback, and recovery?
4. Does the stable-floor idea make sense for raised/lower surface discontinuities?
5. Does the vertical handover offset belong in EKF2Selector, or should it be implemented elsewhere?
6. Is it acceptable that Z difference is only used to choose between direct switch and offset switch, but not used to reject the candidate?
7. Which part would be the most useful as a first small PR?