fusion-altitude

A no_std-friendly altitude and vertical-velocity estimator that fuses barometric pressure with gravity-compensated vertical acceleration from an AHRS via a 3rd-order complementary observer, yielding drift-corrected altitude and online accel-bias estimation.

Companion crate to fusion-ahrs.

Pre-1.0 — API may change before 1.0.

Why

fusion-ahrs (and the upstream xioTechnologies C library) is scoped to orientation — gyro + accel + mag. It does not estimate altitude. Barometers drift slowly and are noisy on short timescales; integrated vertical acceleration is accurate short-term but drifts without bound. Fusing the two gives a stable altitude signal suitable for drones, balloons, and other airborne platforms.

Algorithm

3rd-order complementary observer with three states — altitude h, vertical velocity v, and additive Z-axis accel bias b. Semi-implicit Euler discretisation of the continuous-time form v̇ = (a - b) + K_v·(z - h), ḣ = v + K_h·(z - h), ḃ = -K_b·(z - h):

residual        = baro_altitude - h
corrected_accel = a - b
v += (corrected_accel + velocity_gain * residual) * dt
h += (v + position_gain * residual) * dt    # uses the just-updated v
b -= bias_gain * residual * dt

• Accel is integrated twice (a → v → h); the baro residual feeds back into all three states, so velocity is drift-corrected and a constant accel bias produces no steady-state altitude error.
• Three independent gains tune the position, velocity, and bias time constants. Larger gains trust the baro more (faster correction, more noise/lag in the output); smaller gains trust the inertial integration more (smoother, slower to recover from drift).
• The bias loop is intentionally slow (~10 s default settling) — fast enough to converge within a typical flight, slow enough to avoid chasing transients or short-term baro noise.

The vertical acceleration input is expected to be gravity-compensated, Earth frame, in m/s², positive = up. When paired with fusion-ahrs, earth_acceleration() returns units of g — multiply by fusion_altitude::GRAVITY (and negate for NED).

Usage

use fusion_ahrs::Ahrs;
use fusion_altitude::AltitudeEstimator;

let mut ahrs = Ahrs::new();
let mut altitude = AltitudeEstimator::new();

// Optionally: altitude.with_settings(custom_settings)
// Optionally: altitude.reset(initial_baro_altitude);

loop {
    let dt = 0.01; // 100 Hz

    ahrs.update(gyro, accel, mag, dt);
    // earth_acceleration() returns g; convert to m/s². Negate for NED.
    let vertical_accel = ahrs.earth_acceleration().z * fusion_altitude::GRAVITY;
    let baro_altitude = read_barometer(); // m

    altitude.update(vertical_accel, baro_altitude, dt);

    println!(
        "alt = {:.2} m, vz = {:.2} m/s",
        altitude.altitude(),
        altitude.vertical_velocity(),
    );
}

AltitudeEstimator::new() constructs with defaults; AltitudeEstimator::with_settings(s) overrides. reset(baro_altitude) zeroes the reference explicitly; otherwise the first update() call auto-zeroes against the first baro sample.

Diagnostics

baro_residual() returns the baro innovation baro_altitude - altitude() from the most recent update, sign matching the residual that drove the correction. Useful at the outer loop / autopilot for fault detection (sustained large residual ⇒ baro stuck, prop-wash, or filter divergence), confidence weighting, or innovation-gated handoff to a nav EKF. Zero before the first update and immediately after reset.

Settings

The three gains define the observer's characteristic polynomial s³ + position_gain·s² + velocity_gain·s + bias_gain. Stability (Routh-Hurwitz) requires position_gain · velocity_gain > bias_gain. The defaults target a VTOL multirotor with ~1 s position settling and ~10 s bias settling. See the next section to retune.

Tuning Guide

Don't tune the gains directly — they carry physical units (1/s, 1/s², 1/s³) and are coupled. Pick a position-loop bandwidth ω, a damping ratio ζ, and a separate (slower) bias-loop bandwidth ω_b, then compute the gains:

position_gain = 2·ζ·ω + ω_b           (1/s)
velocity_gain = ω² + 2·ζ·ω·ω_b        (1/s²)
bias_gain     = ω² · ω_b              (1/s³)

Bandwidth by platform

Values below use ζ = 0.7 and ω_b = ω / 5.

Position τ is the error-envelope decay constant; 2% settling ≈ 4τ. Bias 1/ω_b is the bias-loop time constant; bias estimate is converged within ~3/ω_b seconds.

Practical bounds

• Lower bound on ω — ω ≳ 0.1 rad/s. Below that, accel noise and baro drift dominate the bias-loop input.
• Discretisation bound — keep ω · dt < 0.1 for the semi-implicit Euler step to faithfully track the continuous-time response. At 100 Hz that means ω < 10 rad/s. Beyond, the discrete dynamics deviate.
• Loop separation — keep ω_b ≲ ω / 5 so the bias loop doesn't fight the position/velocity transient.
• Noise bound — the baro path is low-pass with corner ≈ ω. Higher ω admits more baro noise into altitude. Pick the lowest ω that meets your controller's bandwidth requirement.

Bias estimation: tradeoffs

Estimating the bias as a state drives the steady-state altitude error from a constant accel bias to zero. Without it (bias_gain = 0.0), the filter parks at +a_bias / velocity_gain — about +5 cm for a 12 mg Z accel bias with the default velocity_gain.

The cost is a slower initial transient: the bias loop takes ~3/ω_b seconds to converge. With defaults that's ~10 s. Motion does not slow convergence — the error dynamics are autonomous (the true trajectory cancels between truth and filter), so bias converges normally during any flight regime, hover or otherwise. The "transient" is the cold-filter startup, not a stationary-only requirement. If you cannot tolerate a startup transient at all, set bias_gain = 0.0 to disable bias estimation. A warm-start API for accel_bias from a stationary pre-flight calibration is on the roadmap; not currently exposed.

Caveat: AHRS attitude error during sustained pitch/roll couples into the bias estimate (rotation-induced error in earth_acceleration().z is indistinguishable from real accel bias to this filter). On a level platform this is zero; on aggressive maneuvers the estimate absorbs a few mg of effective bias. Usually a feature — you want the lumped DC offset gone — but worth knowing.

Quick recipe

1. Estimate the bandwidth your controller needs (rule of thumb: 3–5× your altitude-hold loop bandwidth) — that's ω.
2. Pick ζ — 0.7 if you can tolerate ~5% overshoot, 1.0 for no overshoot.
3. Pick ω_b ≈ ω / 5 (or slower if you want bias to be conservative).
4. Compute position_gain, velocity_gain, bias_gain from the formulas above.
5. Verify ω · dt < 0.1 at your sample rate, and position_gain · velocity_gain > bias_gain (Routh-Hurwitz).
6. Run the realistic_noise example with your gains plugged in and confirm the steady-state bias / RMS error match what you observe.

Installation

cargo add fusion-altitude

Examples

cargo run --release --example with_fusion_ahrs    # clean signal, ~3 cm typical / ~10 cm peak steady-state error
cargo run --release --example realistic_noise     # MEMS-class noise + prop wash, 3.6 cm RMS, 0.9 cm mean

with_fusion_ahrs shows the full fusion-ahrs → fusion-altitude pipeline on a synthetic ±5 m, 0.1 Hz vertical oscillation. realistic_noise adds representative gyro/accel biases, Gaussian sensor noise, and a periodic prop-wash component on the barometer — see the file header for the sensor budget and the observed error.

Development

cargo test                         # all tests
cargo build --no-default-features  # verify no_std build
cargo bench                        # criterion benches
cargo fmt
cargo clippy

License

MIT — see LICENSE.