Crate fusion_altitude

Expand description

§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

Setting	Type	Units	Default (VTOL)	Effect
`position_gain`	`f32`	`1/s`	`2.40`	Feedback gain from baro residual into the altitude state. Larger → tighter tracking of baro, more baro noise visible in altitude.
`velocity_gain`	`f32`	`1/s²`	`2.88`	Feedback gain from baro residual into the velocity state. Damps integrated-acceleration drift in `v`.
`bias_gain`	`f32`	`1/s³`	`0.675`	Feedback gain from baro residual into the accel-bias state. Sets the bias-loop bandwidth (`ω_b ≈ bias_gain / velocity_gain`). Set to `0.0` to disable bias estimation.

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³)

Symbol	Meaning	Typical
`ω`	Position-loop bandwidth (rad/s). Sets how fast `h, v` respond to true motion.	platform-dependent (table below)
`ζ`	Damping ratio of the position/velocity loop.	`0.7` (mild overshoot) to `1.0` (no overshoot)
`ω_b`	Bias-loop bandwidth (rad/s). Sets how fast `b` converges. Must be ≪ `ω` so the loops are decoupled.	`ω / 5` (10× slower)

§Bandwidth by platform

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

Platform	`ω` (rad/s)	`ω_b` (rad/s)	`position_gain`	`velocity_gain`	`bias_gain`	Position `τ = 1/(ζω)`	Bias `1/ω_b`
Balloon / very slow	`0.2`	`0.04`	`0.32`	`0.06`	`0.008`	~7 s	25 s
Fixed-wing	`0.5`	`0.10`	`0.80`	`0.35`	`0.025`	~3 s	10 s
VTOL multirotor (default)	`1.5`	`0.30`	`2.40`	`2.88`	`0.675`	~1 s	~3 s
Aggressive racing	`5.0`	`1.00`	`8.00`	`32.00`	`25.00`	~0.3 s	1 s

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

Estimate the bandwidth your controller needs (rule of thumb: 3–5× your altitude-hold loop bandwidth) — that’s ω.
Pick ζ — 0.7 if you can tolerate ~5% overshoot, 1.0 for no overshoot.
Pick ω_b ≈ ω / 5 (or slower if you want bias to be conservative).
Compute position_gain, velocity_gain, bias_gain from the formulas above.
Verify ω · dt < 0.1 at your sample rate, and position_gain · velocity_gain > bias_gain (Routh-Hurwitz).
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.

Structs§

AltitudeEstimator: 3rd-order complementary observer fusing vertical acceleration with barometric altitude to produce drift-corrected altitude, vertical velocity, and a running estimate of the Z-axis accelerometer bias. All three states are corrected by the baro residual through independent gains; the estimated bias is subtracted from the measured acceleration before integration, so a constant accel bias produces no steady-state altitude error.
AltitudeSettings: Tuning parameters for crate::AltitudeEstimator.

Constants§

GRAVITY: Standard gravity in m/s². Convenience constant for converting fusion-ahrs-style accelerations (units of g) to the m/s² expected by AltitudeEstimator::update.