Struct Eskf 

pub struct Eskf { /* private fields */ }

A 15-state error-state Kalman filter.

Every equation the filter needs is implemented: initialization, (5)–(8), propagation, (9)–(22), and the update of (23)–(41) for every source. The state dead reckons from where initialize put it, the covariance grows around it, and fuse_gnss_position, fuse_gnss_geodetic, fuse_gnss_velocity, fuse_baro_altitude, fuse_mag_heading, fuse_gnss_heading and fuse_course each correct both, or are turned down by the gate of (37)–(38).

Example

A whole flight’s worth of calls, in the order they happen, with what each one is allowed to answer. The README’s quick start is the same loop without the assertions.

use fusion_nav::prelude::*;

let mut filter = Eskf::default();

// A vehicle sitting still: no rotation, gravity the only specific force, read by a
// 400 Hz rate IMU whose clock counts microseconds.
let still = |sample: u64| {
    ImuSample::from_rates(
        Timestamp::from_micros(2_500 * sample),
        AngularRate::body(0.0, 0.0, 0.0),
        Acceleration::body(0.0, 0.0, -GRAVITY),
        Seconds::from_secs(0.0025),
    )
};

// Initialization reports what it achieved rather than refusing what it dislikes. A
// window that is short or moving gives `Alignment::Coarse`, and the filter runs and
// says `Status::Aligning` until attitude converges. 800 samples at 400 Hz is the 2 s
// `Initialization::min_duration` wants, folded in one at a time rather than buffered.
let mut window = StaticWindow::new();
for sample in 1..=800 {
    window.push(StaticSample {
        imu: still(sample),
        ..StaticSample::default()
    })?;
}
assert_eq!(filter.initialize(&window)?, Alignment::Static);

// Nothing in that window carried a barometer, so it fixes no reference altitude, and
// the first `fuse_baro_altitude` reads one from the estimate. See `StaticSample::baro`.

assert!(filter.predict(still(801)).is_propagated());

// Every measurement carries the time it was taken, on the IMU's clock: a receiver's fix
// describes where the vehicle was when it was computed, not when it arrived. These were
// all taken at the last sample.
let now = Timestamp::from_micros(2_500 * 801);

// GNSS in latitude and longitude. The filter holds the navigation origin: the first
// fix places it, under the estimate, so every later fix converts about the same point.
// `clamped` bounds the receiver's own `eph` and `epv` the way both autopilots do.
let outcome = filter.fuse_gnss_geodetic(
    now,
    Geodetic::from_degrees(47.397_742, 8.545_594, 488.0),
    PositionNoise::clamped(
        1.5,
        3.0,
        SigmaBounds::new(0.5, 100.0),
        SigmaBounds::new(0.75, 100.0),
    ), Position::zero(),
);
assert!(outcome.is_accepted());
assert!(filter.origin().is_some());

// A solution whose vertical velocity the receiver did not measure: the down axis
// carries a σ large enough that its gain is negligible, rather than a claim.
let outcome = filter.fuse_gnss_velocity(
    now,
    Velocity::ned(0.0, 0.0, 0.0),
    VelocityNoise::horizontal_vertical(0.3, 1000.0), Position::zero(),
);
assert!(outcome.is_accepted());

// Nothing in that window carried a magnetometer either, so nothing observed the
// rotation about gravity: heading is not valid, and `Aligning` says the attitude has
// not converged rather than the aiding having failed.
let state = filter.state();
assert_eq!(state.status, Status::Aligning);
assert!(state.validity.tilt && !state.validity.heading);

// The first accepted magnetic heading is what establishes yaw.
let outcome = filter.fuse_mag_heading(
    now,
    MagField::body(0.22, 0.0, 0.44),
    HeadingNoise::from_sigma(0.1),
);
assert!(outcome.is_accepted());
assert_eq!(filter.state().status, Status::Healthy);

// A vehicle without a magnetometer could have taken it from its course instead, once
// moving: one sitting still goes nowhere, and the direction of no velocity is refused.
let outcome = filter.fuse_course(now, HeadingNoise::from_sigma(0.05));
assert_eq!(outcome, Fusion::Unobservable);

// Sitting still is a claim the application can make, and it holds the tilt.
let outcome = filter.fuse_stationary(now, VelocityNoise::from_speed_accuracy(0.1));
assert!(outcome.is_accepted());

Implementations

impl Eskf

pub fn reset_position_to( &mut self, position: Position<Ned>, noise: PositionNoise<Ned>, ) -> bool

Force position to an external fix and reset its covariance block.

The position becomes the fix, its variances become the fix’s noise, and its correlations with the rest of the state are dropped — the new error came from the measurement and has nothing to do with the errors that preceded it.

For an application that owns recovery: the filter does the same on its own, per source, unless Config::recovery turns it off; see Recovery.

Returns false, changing nothing, for a fix or a noise that is not a number, or a variance that is not positive — the bar every fuse_* applies, and it matters more here: this writes noise straight onto the covariance diagonal, with no gate and no innovation to dilute it. See Fusion::NotFinite and Fusion::InvalidNoise.

A reset is not aiding. While the position hold of Config::hold is engaged it fuses again within 0.2 s, about the position set here, and Validity reads the quantity invalid again: a position that is to stay valid without a sensor wants the hold off.

pub fn reset_velocity_to( &mut self, velocity: Velocity<Ned>, noise: VelocityNoise<Ned>, ) -> bool

Force velocity to an external solution and reset its covariance block.

See reset_position_to, including what is refused.

impl Eskf

pub fn fuse_gnss_position( &mut self, time: Timestamp, position: Position<Ned>, noise: PositionNoise<Ned>, antenna: Position<Body>, ) -> GnssFusion

Fuse a position fix already expressed in NED meters about the filter’s origin. Equation (28).

For a receiver reporting latitude and longitude, use fuse_gnss_geodetic, which converts about the origin the filter holds. This one is for a caller that owns the conversion, or whose positions were never geodetic — a local RTK base, motion capture. Mixing the two is only right if the caller’s origin is origin.

After a coarse start the first fix is adopted rather than fused; see Fusion::Reset.

noise is the receiver’s own accuracy where it reports one, bounded before it arrives: PositionNoise::clamped takes eph and epv and holds each within its own floor and cap, for the reasons recorded there. A two-dimensional fix instead goes through PositionNoise::horizontal_vertical, which leaves the vertical σ where the caller put it: clamped caps both axes, so it would turn a declined height back into a measurement.

The filter applies no bound of its own, because R describes the measurement and belongs with it rather than in Config. A caller handing over a raw eph is therefore trusting the receiver further than either production autopilot does. Nor does it smooth one: ArduPilot runs each accuracy through a decaying envelope with a 5 s time constant before bounding it (AP_NavEKF3_Measurements.cpp:609-633 at 368dc0c4), so a spike in eph deweights the fixes after it for seconds there and only its own fix here. The replay harness takes the same stance, for the reasons data/README.md gives under r_policy=. What the filter does add is the receiver’s rather than the fix’s: a fix’s error persists into the next one, and the update is computed at the variance that leaves, equation (24′), with the gate still reading noise itself. See Config::correlation.

The interval (24′) reads is the time since the previous fix fused through this method, so it assumes one receiver. Fixes from a second one, or from motion capture, interleaved with the first read as the same error arriving sooner and are deweighted though their errors are independent.

The fix is two measurements, gated and reported apart: north and east at Gates::gnss_position, then down at Gates::gnss_height, against the state the first left. A half inconsistent with the estimate is Fusion::Rejected and changes nothing but its own health, so a height the estimate disagrees with costs no horizontal aiding; see GnssFusion for the measurement behind that. A half whose numbers are unusable is refused alone for the same reason — a 2D fix reporting epv = 0 still fuses its horizontal position.

The adoption after a coarse start is the exception, and takes the fix whole: it writes all three axes onto the covariance, so any unusable number refuses both halves.

antenna is where the antenna sits relative to the IMU in body axes, forward, right, down (Position::body), and Position::zero for one on top of it. The fix is the antenna’s, p + R r of (28′), and the estimate stays the IMU’s. Unlike PX4, which subtracts R̂ r from the fix and keeps H as it was (EKF/aid_sources/gnss/gps_control.cpp:351-354 at c4e4ef98), the update carries the arm’s dependence on attitude, −R̂[r]×, so a fix observes heading through a long mast; see observation/gnss.rs for what that was measured against. An argument, as noise is and as PX4 carries it on each GNSS message (antenna_offset_x/y/z), because a second receiver has its own; from PX4’s parameters it is SENS_GPS0_OFF* less EKF2_IMU_POS*.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

pub fn fuse_gnss_geodetic( &mut self, time: Timestamp, fix: Geodetic, noise: PositionNoise<Ned>, antenna: Position<Body>, ) -> GnssFusion

Fuse a GNSS fix given as latitude, longitude and height. Equations (43), (44), then (28).

The filter holds the navigation origin and converts about it, so the fix and the estimate are relative to the same point by construction. The first fix places the origin, unless set_origin already has:

• With a position estimate — a static start, a seed — the origin goes where that estimate says the vehicle started, so the fix lands on the estimate and nothing steps. Equation (44). The fix is not fused: it was spent placing the origin, and fusing it as well would count it twice. What it does settle is the position uncertainty. About the new origin the position error is the fix’s error, so the position covariance block becomes noise and its correlations are dropped, as reset_position_to does, with the value unchanged. Reported as accepted with a zero test ratio: nothing was inconsistent.
• Without one — after a coarse start — the origin goes at the fix, and the fix is adopted as position zero; see Fusion::Reset.

Check the receiver’s fix type before calling. Many report latitude and longitude zero until they have a fix, and a finite zero is a usable origin: the first one would put the navigation frame in the Gulf of Guinea for the rest of the flight.

noise and antenna are as for fuse_gnss_position, floor included, and so is the split into two gated halves. The origin (44) places goes under the estimate of the antenna, since the fix is the antenna’s.

A fix that is not a number is refused with Fusion::NotFinite, both halves, since no conversion survives one; so is a noise that is not, where the fix would place the origin, which writes all three axes. A fix with a latitude beyond ±90° cannot place an origin, nor can one near a pole that no origin puts at the estimate (see LocalOrigin::placing): with none held it is refused with Fusion::NoReference, and the next usable fix places it instead.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

pub fn fuse_gnss_velocity( &mut self, time: Timestamp, velocity: Velocity<Ned>, noise: VelocityNoise<Ned>, antenna: Position<Body>, ) -> Fusion

Fuse a GNSS velocity solution. Equation (29).

After a coarse start the first solution is adopted rather than fused; see Fusion::Reset.

noise is the receiver’s speed accuracy, sacc, bounded the way fuse_gnss_position’s is: VelocityNoise::clamped. Where the solution carries no usable vertical velocity, or the receiver reports the axes separately, VelocityNoise::horizontal_vertical is the constructor to reach for: all three axes are fused or none, and a per-axis σ is what says which of them the receiver actually measured.

A solution inconsistent with the estimate at Gates::gnss_velocity is Fusion::Rejected and changes nothing but the source’s health. One joint test over all three axes, as (28)’s is.

Fused at the variance (24′) leaves for a solution whose error persists from the last, Config::correlation’s gnss_velocity; the gate reads noise itself.

antenna is as for fuse_gnss_position: the solution is the antenna’s velocity, v + R(ω × r) of (29′), with ω the rate the vehicle turned at over the solution’s age, read off the state’s own history rather than the last sample.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

pub fn fuse_baro_altitude( &mut self, time: Timestamp, altitude: Altitude, noise: AltitudeNoise, ) -> Fusion

Fuse a barometric altitude. Equation (30).

The measurement is z = -(α - α₀), so it needs a reference: one initialize derived from a window taken at rest, one set_baro_reference named, or — failing both — one this call establishes from the estimate, below.

noise being per call is what lets it carry a condition neither platform’s parameter can: ArduPilot multiplies the barometer variance by 4 in ground effect (gndEffectBaroScaler, AP_NavEKF3.h:511, applied at AP_NavEKF3_PosVelFusion.cpp:1419), and PX4 runs a deadzone around the same condition. An application here inflates σ on the calls it applies to, and says in its own code when that is.

An altitude inconsistent with the estimate at Gates::baro_altitude is Fusion::Rejected and changes nothing but the source’s health. One degree of freedom, so the threshold is a Gate<1> and a Gate<3> in that field does not compile.

With no reference held, the first altitude offered once position is established sets one, α̂₀ = α + p̂_D, so that it lands on the estimate — PX4’s baro_height_control.cpp:79 at c4e4ef98. That covers every start that leaves no reference: one in motion after its first GNSS fix, a window with no barometer in it, and a seed. Leaving it to the caller costs cd7e0001, a coarse start, all 3530 of its altitudes, with nothing but Fusion::NoReference on a source nobody reads to say so. Config::baro_reference_from_estimate turns it off for a caller that names its own.

The reference inherits the height error of the estimate it was read against, so it is seeded correlated with it, P_bb = P_DD + R_m and P_xb = −P[:, D] of (30′), rather than as an independent σ. That is what keeps the error the first fix left in α̂₀ from reading as a hundred barometer readings’ worth of agreement: held constant and uncorrelated, it took moving_start’s nees_pos to 112.59.

The altitude is spent placing the reference rather than fused, as a first fix is spent placing the origin in fuse_gnss_geodetic, and is reported the same way: accepted with a zero test ratio, since nothing was inconsistent. Fusing it as well would count its noise twice, once in P_bb.

Until position is established there is no estimate to read the reference against, and the altitude is refused with Fusion::NoReference rather than adopted: a height is not a quantity this source can establish, because α₀ is what relates it to the origin.

Fused at the variance (24′) leaves for an altitude whose error persists from the last, Config::correlation’s baro_altitude; the gate reads noise itself.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

impl Eskf

pub fn fuse_mag_heading( &mut self, time: Timestamp, field: MagField<Body>, noise: HeadingNoise, ) -> Fusion

Fuse magnetic heading from a calibrated three-axis magnetometer. Equations (34)–(36).

Heading only: the field is reduced to one scalar, so a magnetic disturbance can corrupt yaw but cannot reach roll or pitch. noise is on the resulting heading, not on the field components.

This is also where heading becomes an estimate. A static window with no magnetometer in it, and any coarse start, leave yaw unobserved — see initialize — and validity reports heading false until the first heading is accepted, here or from fuse_gnss_heading or fuse_course, however tight sigma_yaw was.

That first heading is the point GOALS.md names for a yaw reset rather than an ordinary update: the error-state attitude of equation (2) is a small-angle quantity, so a yaw error of a radian is wrong in a way no variance expresses, and widening the prior does not fix it. It is adopted rather than gated, which is Fusion::Reset’s documented shape — a quantity never established, with no estimate to step away from. Gating it instead would judge the measurement against a prior nothing ever measured: a static window with no magnetometer leaves sigma_yaw, 0.35 rad, on a yaw that is a guess, and a correct heading more than about 64° from that guess would be rejected — locking the filter out of what may be the one source that can ever establish yaw.

Ordinary updates thereafter, gated at Gates::mag_heading with one degree of freedom, until a run of rejections outlasts Recovery::mag_heading while no GNSS position, velocity or heading is arriving, when the next heading is adopted the same way. The field is reduced to a scalar heading before the gate sees it, so a disturbance is tested as the yaw error it is.

The field must be calibrated: this crate corrects no hard- or soft-iron error and carries no magnetic-field states to absorb one, so a bias in field is a bias in heading. A field of exactly zero is finite, and reports the vehicle as D_m off rather than producing NaN — gateable once yaw is established, and adopted where it is not, which is the strongest reason to check a magnetometer before the first call rather than after it.

Fused at the variance (24′) leaves for a heading whose error persists from the last, Config::correlation’s mag_heading; the gate reads noise itself.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

pub fn fuse_gnss_heading( &mut self, time: Timestamp, heading: Radians, noise: HeadingNoise, ) -> Fusion

Fuse a true heading from a dual-antenna (moving-baseline) GNSS receiver. Equations (35′) and (36).

The heading of the line from the primary antenna to the secondary, which is taken to lie along body x: a receiver whose antennas are mounted otherwise has the mounting angle subtracted by the caller first, as PX4 and ArduPilot each do from a parameter. True rather than magnetic, so no declination applies, and nothing is leveled with the estimated attitude, so the (36′) a magnetic heading carries has no counterpart; see observation/heading.rs for the model and why its Jacobian is (36) rather than the exact one PX4 differentiates.

noise is the receiver’s heading_accuracy, bounded before it arrives: HeadingNoise::clamped holds PX4’s and ArduPilot’s floors, and both production estimators apply one because a moving-baseline solution is as optimistic about itself as a position fix is.

The first heading after any start that observed no yaw is adopted rather than gated, as a magnetic one is — Fusion::Reset, with this noise as the adopted variance — and both production estimators align on the first sample the same way (PX4 aid_sources/gnss/gnss_yaw_control.cpp:100-111 at c4e4ef98, ArduPilot AP_NavEKF3_MagFusion.cpp:404-410 at 368dc0c4). Ordinary updates thereafter, gated at Gates::gnss_heading, and recovered after Recovery::gnss_heading.

Fused beside a magnetometer rather than instead of one, where PX4 stops fusing the magnetometer while GNSS yaw is active (aid_sources/magnetometer/mag_control.cpp:189): each is gated against an estimate the other has corrected, and a magnetometer that disagrees is not recovered while this is accepted. A body x within 30° of vertical has no heading to compare, and is refused as Fusion::Unobservable.

time is when the measurement was taken, on the clock the IMU’s samples are timed on; Fusion::OutOfHorizon says which times cannot be placed.

pub fn fuse_course(&mut self, time: Timestamp, sideslip: HeadingNoise) -> Fusion

Constrain heading to the direction of travel: the vehicle points along its estimated velocity, to within sideslip. Equations (35″) and (36).

Yaw from course over ground, for a vehicle with no magnetometer or one it cannot trust. It is a constraint rather than a measurement: nothing new is read, and what the call states is that the nose and the track agree, so it is only as true as the vehicle makes it. A fixed-wing in coordinated flight and a ground vehicle on its wheels hold it to a few degrees; a crosswind adds its crab angle to sideslip. A multirotor holds no such relation — it hovers, and flies in any direction facing any other — and must not call this.

The velocity is the estimate’s, not a GNSS velocity handed in: that velocity has already been fused through fuse_gnss_velocity, and taking a course from it again would count its cross-track error twice. Read off the state, the velocity’s uncertainty reaches the update through P, and the update corrects velocity and heading together. So time is when the constraint is claimed to hold, and a caller fusing one per GNSS velocity passes that fix’s time.

Refused as Fusion::NoReference where no fresh GNSS velocity holds the estimated one (SourceHealth::is_fresh), which also covers one never established, and as Fusion::Unobservable where it names no direction: too slow against its own uncertainty, which is ArduPilot’s 15° bar on the course read from P — see observation/heading.rs — and so a speed threshold the velocity’s accuracy sets rather than a parameter. Refused the same way with body x within 30° of vertical. ArduPilot’s plane realigns yaw from course above 5 m/s (realignYawGPS, AP_NavEKF3_MagFusion.cpp:145-218 at 368dc0c4); PX4 has no course source and reaches the same vehicle through its GSF yaw estimator.

The first course after a start that observed no yaw is adopted, as a first heading is, at sideslip’s variance plus the course’s own; ordinary updates thereafter, continuously rather than ArduPilot’s once, gated at Gates::course and recovered after Recovery::course while no other heading is accepted.

The adoption keeps none of the heading’s correlation with velocity, though the adopted heading error is the course error, ∇χᵀδv, plus the sideslip. Dropping it overstates the next update’s S by about 2σ_χ², which is conservative; what it costs is that a velocity fix cannot correct the part of the heading error the velocity lent it until propagation correlates the two again.

Not a source Status counts: it reads the filter’s own velocity, so it aids nothing the GNSS velocity it needs does not, and a vehicle slowing to a stop, where every course is refused, is not degraded for it.

impl Eskf

pub fn fuse_stationary( &mut self, time: Timestamp, noise: VelocityNoise<Ned>, ) -> Fusion

Fuse the caller’s claim that the vehicle is still: zero velocity on all three axes. Equation (29″).

Without horizontal aiding nothing observes tilt, so a vehicle on the bench with no GNSS loses its tilt on the schedule ImuNoise sets: with the position hold off, Validity::tilt goes false 4.84 to 10.3 s into an unaided start, and the hold only slows that. A zero velocity is what holds it: tilt error times gravity lands in velocity at once, and the update takes it back out. PX4 and ArduPilot hold the attitude on the ground the same way.

Zero velocity rather than PX4’s constant position (EKF2_POS_LOCK): what the caller knows is that the vehicle is not moving, which needs no anchor and observes tilt one integration nearer than a position does. ArduPilot fuses zero velocity for the same case.

The filter cannot know the vehicle is still; the caller asserts it, as it asserts the static window’s stillness, from a landed detector, a disarmed state or its own test. A claim made while the vehicle moves reads its acceleration as tilt, and the gate is what stands between that and the estimate.

noise is how still: the velocity the vibration and the claim’s own error leave. Upstream chooses far apart, ArduPilot 1 m/s for its synthetic zero velocity (AP_NavEKF3_PosVelFusion.cpp:769-775 at 368dc0c4), PX4 0.01 m on the position it holds at rest (fake_pos_control.cpp:57-60 at c4e4ef98). A tighter σ levels faster and lets less vibration through as tilt.

A claim inconsistent with the estimate at Gates::stationary is Fusion::Rejected and changes nothing. It is never adopted, whatever Config::recovery says: a stationary claim the gate keeps turning down is a caller wrong about the vehicle, and adopting it would write that into the state. Nor does it establish velocity after a coarse start, which only a sensor does; it does end the position hold’s claim on velocity, whose covariance the stationary claim then bounds, and never its claim on position. No Config::correlation either: its error is vibration, not a receiver’s error persisting from one solution to the next.

Not a source Status counts: it says where the vehicle is not going, not where it is, so a bench with no GNSS stays DeadReckoning while its tilt holds.

impl Eskf

pub fn predict(&mut self, imu: ImuSample) -> Propagation

Propagate the nominal state and covariance across one IMU sample. Equations (9)–(22).

The hot path, called at IMU rate. The step is Δt, the time from the filter’s time to the sample’s; the filter never reads a clock. The state integrates the sample’s increments over their own intervals, and Δt is what everything that is about time passing reads: the health timers, and the test for a gap.

A Δt longer than Config::max_predict_dt is not integrated: one IMU sample cannot describe a long interval, and propagating it anyway would put a number in the state that looks like an estimate and is not. The filter coasts across it instead, equation (22′), as Propagation::Coasted: position moves on the estimated velocity and the covariance grows by what Config::coast allows, so the first fix after the gap is judged against an uncertainty that grew with it. With coasting off the step is refused, as Propagation::StepTooLong. Either way the timers advance, so Status degrades on schedule. It is Δt that is tested rather than an interval, because a gap is time no sample describes: a driver that integrated across a stall hands over an increment that does describe it, and a logger that dropped samples hands over one that does not.

A sample no later than the filter’s time is refused before the timers move at all, and leaves the clock where it was.

A sample carrying a NaN or an infinity is refused too, as Propagation::NotFinite: propagating it would put the NaN in the quaternion and then in the covariance, where nothing reports it and it never leaves. So is one whose integration interval is under a microsecond or longer than max_predict_dt, as Propagation::InvalidInterval. The timers advance in both cases, since Δt was fine and only the sample was not.

A step that comes out of (11)–(14) or (22) non-finite is discarded rather than stored, as Propagation::StateNotFinite, and the state and the covariance are discarded together: a finite sample is not enough to guarantee a finite result, since f32 has a finite range and both a_n Δt and F P Fᵀ can leave it. Committing one half would leave the filter reporting an estimate whose uncertainty describes a different step.

The covariance only grows here. (22) adds Q and (20) spreads what is already there; nothing in propagation takes uncertainty back out, which is the measurement update’s job. So an unaided filter’s Validity flags go false in the order their variances cross Config::accuracy, and Status does not follow: it reads the aiding timers and an alignment that has already latched. The one exception is the position hold of Config::hold, which a committed step runs while nothing aids the filter.

impl Eskf

pub fn set_origin(&mut self, origin: Geodetic) -> bool

Put the navigation origin at a known point, such as a surveyed home or a landing pad. Equation (43).

Without this, the first fuse_gnss_geodetic places the origin, which is right for most flights. Setting it is for an application whose positions mean something relative to a fixed point.

The vehicle does not move. With an origin already held, the position estimate is re-expressed about the new one, so the geodetic position is unchanged and only the numbers describing it are; its covariance is untouched, because a change of origin adds no uncertainty. With none held, the current position is taken to be relative to this one, which is the caller’s claim to make — true after a static start on the point being named, false anywhere else.

Call it after initializing: a static start clears the origin, since it declares position zero to be wherever the vehicle is. Called before, it still names the site, which is where the magnetic model is read (see set_magnetic_declination): the window then levels its heading at the site’s declination, and a heading only the magnetometer set is turned whenever a later origin changes it.

Returns false, changing nothing, for an origin with a coordinate that is not a number or a latitude beyond ±90°, or one so far from the held position that it cannot be written in f32 meters from the new origin.

pub const fn origin(&self) -> Option<LocalOrigin>

The navigation origin: the point State::position is relative to, and the tangent plane the filter converts geodetic fixes in. None until the first fuse_gnss_geodetic or set_origin.

Also the conversion an application wants for anything else it holds in latitude and longitude — a waypoint, a geofence — so that it lands in the same frame as the estimate.

pub fn geodetic_position(&self) -> Option<Geodetic>

The position estimate as latitude, longitude and height, once the filter is initialized and has an origin to place it with.

pub fn set_magnetic_declination(&mut self, declination: Radians) -> bool

Set the magnetic declination at the operating site: D_m of equations (6) and (35), east-positive, the angle that turns a magnetic heading into a true one.

Optional where the magnetic-model feature is on, as it is by default: the filter reads the declination from a World Magnetic Model table wherever it places its origin, and turns a heading only the magnetometer has referred to north along with it (see Geodetic::magnetic_declination). A value set here is the caller’s and the model never overrides it; that is the call for a site the table describes badly or a date far from its epoch. Zero until one or the other.

Held by the filter rather than Config because it is a property of where the vehicle is, like the origin and α₀, and a vehicle that powers on before a GNSS fix learns its site only once a fix arrives. PX4 tracks it at runtime from the last valid GNSS position (EKF/estimator_interface.h:485 at c4e4ef98).

Read wherever a magnetic heading becomes a true one: the heading a static window commits by (6), every fuse_mag_heading by (35), and the heading adoption and recovery. So set it before initialize when the site is known. It moves no state. A change once heading is established arrives as an innovation of exactly the change on the next heading; a large one against the heading’s σ is Fusion::Rejected until Config::recovery adopts a heading at the new value.

Returns false, changing nothing, for a value that is not a finite number: it enters every heading innovation for the rest of the flight, so a NaN here ends magnetic aiding rather than spoiling one update.

pub const fn magnetic_declination(&self) -> Radians

The magnetic declination in use; see set_magnetic_declination.

pub fn set_baro_reference( &mut self, reference: Altitude, noise: AltitudeNoise, ) -> bool

Set the barometric reference α₀ directly, with the σ it is known to. Equations (30) and (30′).

For a reference known better than the estimate: a surveyed pad, or the ground re-established before takeoff. A filter with no reference needs none of this, since fuse_baro_altitude takes one from the estimate unless Config::baro_reference_from_estimate says otherwise. The filter estimates α₀ from then on, starting from this σ and uncorrelated with the state, so noise is the claim that decides how far the first disagreement with GNSS height moves it.

Returns false, changing nothing, for a reference that is not a number or a σ that is not positive: α₀ appears in every barometric measurement for the rest of the flight, so a NaN here is not one bad update but the end of barometric aiding, and a σ of zero is a reference no disagreement could ever move.

pub const fn baro_reference(&self) -> Option<Altitude>

The barometric reference α₀ as currently estimated, equations (30) and (30′): measured by a window taken at rest, named by set_baro_reference, or read against the estimate by the first altitude once position is established. None until one of those has happened — a start in motion keeps whatever reference the flight already had rather than calling its own altitude the ground.

Exposed because it is what the filter’s zero altitude means: the barometer reading the filter would call the origin’s height. It moves as barometer and GNSS height disagree, at the rate Config::baro_offset_walk allows.

impl Eskf

pub fn initialize( &mut self, window: &StaticWindow, ) -> Result<Alignment, InitError>

Align from a window of samples taken while the vehicle was, ideally, still. Equations (5)–(8), and α₀ of (30).

The window does not have to be a genuine static interval. If it is — long enough and within the tolerances Initialization sets — the result is Alignment::Static and the filter starts with the covariance that configuration describes. If it is not, the result is Alignment::Coarse, carrying what was measured: the filter still runs, with attitude uncertainty inflated to match, and reports Status::Aligning until that uncertainty comes down. Refusing instead would be a launch restriction, and a filter that will not start is worth less than one that starts and says how much to trust it.

Where no window exists at all, use initialize_coarse or initialize_from.

α₀, the barometric reference of equation (30), is taken from the window whenever the window was taken at rest, which is not the same test as the alignment: a window too short to align an attitude from is still a window of a vehicle sitting on the ground, and the altitude it read is what zero will mean. A window that was moving keeps the reference the flight already has, as initialize_coarse does — a restart at 100 m must not call its own altitude the ground.

Position and velocity start at the configured priors either way, which assume the origin is here and the vehicle is at rest. A launch that knows better — off a moving deck, say — should say so through initialize_from rather than let the first GNSS fix arrive as a large innovation.

Whether those priors count as established is the window’s answer, not the alignment’s: a vehicle that held still through it was where the origin says and was not moving, whether or not the window was long enough to align an attitude from. A window taken in motion establishes neither, and the first fix is adopted rather than fused — see Fusion::Reset.

Heading is the one thing stillness cannot supply. Gravity pins roll and pitch; nothing pins the rotation about it, so a window carrying no magnetometer leaves yaw unobserved however long and however still it was, and validity withholds heading until the first heading from fuse_mag_heading, fuse_gnss_heading or fuse_course is accepted. The covariance alone cannot say that: sigma_yaw is a prior on a number nobody measured.

The window’s span, which min_duration is checked against, is the time its samples integrated; the filter’s clock starts at the last sample’s time. As everywhere else, the filter never reads a clock.

Errors

InitError::NoSamples for an empty window. A sample nothing can be made of is refused as it is pushed, so a window holds none.

pub fn alignment_of( &self, window: &StaticWindow, ) -> Result<Alignment, InitError>

What initialize would make of this window, without touching the filter.

For the application that would rather wait for stillness than start coarsely: restart the window, or slide a buffered one forward, until this reports Alignment::Static, then commit; StaticWindow says which suits which. The filter cannot do that waiting itself — it does not know whether the vehicle is about to launch or has been sitting on the bench for an hour.

Errors

As initialize, whose work it does up to the commit.

pub fn initialize_coarse( &mut self, imu: ImuSample, ) -> Result<Alignment, InitError>

Start from a single IMU sample, with no window at all.

For the launch that never offers one: a hand launch, a deck that is always moving, a restart at altitude. Tilt comes from one accelerometer reading, which carries the sensor’s full noise and whatever the vehicle’s own acceleration was at that instant, so the covariance is inflated accordingly and the filter reports Status::Aligning.

Prefer initialize_from where the application has an attitude from somewhere — a companion AHRS, the last flight — since a real estimate beats one sample of gravity.

The sample carries no magnetometer, so heading starts at zero and stays unestablished whatever the vehicle was doing: this entry point levels, and nothing more.

Errors

InitError::NotFinite if the sample carries a value that is not a number, and InitError::InvalidInterval for an integration interval under a microsecond.

pub fn initialize_from( &mut self, state: State, covariance: Covariance, time: Timestamp, ) -> Result<Alignment, InitError>

Initialize from an estimate the application already holds, rather than from a static window.

initialize is the better path wherever stillness is available: it averages sensor noise away, and gyroscope bias is observable at rest. This one exists for the launches that never offer stillness — a moving deck, a hand launch, a restart at altitude — and for a warm start from the last flight, where a saved gyroscope bias is worth far more to a moving vehicle than zero is.

The caller owns the seed’s quality and covariance is how it says so: an attitude from a companion AHRS deserves the uncertainty that AHRS reports, not the static-window figures in Initialization. Seeding a coarse attitude with a confident covariance is the one way to misuse this, and it produces a filter that gates out the measurements that would have corrected it.

state.attitude names its own convention, through whichever Attitude constructor built it — PX4 and ArduPilot quaternions through from_body_to_ned, a ROS one through from_flu_to_enu, which is where the conventions and what a wrong one costs are written down. It is the one error on this path no check downstream can reach.

state.status is ignored: status is derived from aiding, never asserted. baro_reference is left alone, so re-initializing in flight keeps the reference the flight began with; a filter that never had one takes it from the first altitude, read against the seeded position (see fuse_baro_altitude). origin is left alone for the same reason, and state.position is taken as relative to it — or, with no origin yet, to the one the first fuse_gnss_geodetic will place around it.

Errors

InitError::NotFinite if state or covariance carries a value that is not a number, and InitError::InvalidVariance for a variance on the diagonal that is not strictly positive — the bar every fuse_* puts on R, applied here because a seed is the one path that writes a covariance in whole. A rejected seed leaves the filter uninitialized rather than poisoned.

The bar is the floor of (42′) rather than zero, because zero is the tidy member of the class it is there to catch. A diagonal that was never populated arrives as exactly zero from a memset; the same accident with an exponent left in it — dropped in deserialization, or a variance scaled by its own units twice — arrives at 1e-30 and does the identical damage, since 1e-30 / (1e-30 + R) is zero in f32 and validity calls the quantity good on the first read. Refusing one and accepting the other would catch the legible failure and wave through the messy ones. The floor is three to five decades below anything the filter itself reaches, so no seed anybody meant is near it.

Whether the seed counts as aligned is the covariance’s answer, not this one’s: a confident seed reports Status::Healthy straight away, a coarse one Status::Aligning until it converges.

time is when the seed is valid, and starts the clock the next predict is differenced against.

impl Eskf

pub fn attitude_variance(&self) -> AttitudeVariance

How uncertain the attitude is about each navigation axis: tilt about north and east, heading about down.

The numbers validity tests against Accuracy, and the ones to plot beside a tilt or a heading. The attitude rows of covariance are in body axes and mean tilt and heading only while the vehicle is level; see AttitudeVariance. All zero before the filter is initialized, which is the covariance it then holds.

pub const fn is_aligned(&self) -> bool

Whether the attitude has ever met ALIGNED_TILT and ALIGNED_HEADING since initialization — the test behind Status::Aligning.

Promotion is measured, not timed: it reads the covariance against those two bars, and holds heading to the rule Validity does, so it stays false on a vehicle with no magnetometer — a heading nothing ever observed is not aligned however tight sigma_yaw is. It does not read Config::accuracy: how good the attitude must be for the mission and whether the start has been resolved are two questions, and a bar serving both has to be wrong for one of them. What it does not do is fall back: alignment is an event, “the start has been resolved”, where validity is the live question, “is tilt good enough right now”.

Conflating the two is what a corpus replay showed costs. Read live, the bar is crossed 703 times on 2c42096b, a grounded vehicle under a poor sky view whose tilt σ sits above ALIGNED_TILT for 79 % of two hours, and 7592c9b2 and f16771dd start aligned and end Aligning. The counts are the same with tilt read on body axes or on navigation ones. That is honest about tilt right now, which is Validity’s job, and useless as a report that the filter has not finished starting up. PX4 and ArduPilot both latch it for the same reason: tilt_align and tiltAlignComplete are only ever tested while false (src/modules/ekf2/EKF/control.cpp:73-78 at c4e4ef98e9, libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp:520-525 at 368dc0c428).

pub fn validity(&self) -> Validity

Which parts of the estimate are good enough to use, right now.

Also carried on State::validity, which is where most callers will meet it.

pub fn predicted_validity(&self) -> Validity

Which parts of the estimate the filter expects to be good if the vehicle left the ground now — the arming question, rather than the current one.

Sitting still, some states are simply unobservable: heading without a magnetometer, horizontal position before the first fix is fused. Asking validity at that moment says no, and says it about a filter that would in fact be navigating a second after takeoff. A vehicle that refused to arm on that answer would never arm at all.

So a quantity counts here if it survives the horizon, or if a source that constrains it is currently being accepted. The two halves answer the two ways an arming check can be wrong. The projection is the pessimistic half: P is propagated Accuracy::horizon forward with nothing fusing, by the (16)–(22) predict itself runs, and each quantity is tested at the far end — so a tilt that is inside its bar now and will not be in a second reads false here and true from validity. The aiding clause is the optimistic half, and it is what a projection cannot supply: before the first fix, horizontal position has no estimate to propagate and the fact that fixes are arriving is the whole answer. pred_horiz_pos_rel in ArduPilot’s status word is that clause; PX4 has no equivalent, and neither publishes the projection.

Tilt is where the projection earns its place, because nothing aids it: a static window brings it in and the gyroscope’s noise and bias take it back out through (20), on a schedule the covariance knows and no acceptance timer does. At ImuNoise’s defaults an unaided start holds tilt for 10.3 s from a window that measured its gyroscope and 4.84 s from one whose gyroscope never scattered, so a horizon under that arms and one over it does not, which the current value alone cannot say. The projection is of propagation alone: the position hold of Config::hold, which engages at that bar, is not projected.

The projection is (22′) in one exact step, so it is neither optimistic nor longer for a longer horizon: one Φ P Φᵀ and its noise, about an IMU epoch’s arithmetic.

impl Eskf

pub fn new(config: Config) -> Result<Self, ConfigError>

Create an uninitialized filter, or refuse a Config with a value outside its bound (Config::validate). Measurements are refused with Fusion::NotInitialized until initialize succeeds.

pub const fn config(&self) -> &Config

The configuration this filter was built with.

pub const fn is_initialized(&self) -> bool

Whether initialize has succeeded.

pub const fn time(&self) -> Option<Timestamp>

The filter’s clock: the time of the last sample predict was handed, or of the window or seed the filter started from. None until initialized.

Every later sample is differenced against it and every measurement’s age read from it. Only Propagation::InvalidStep, a sample not after it, leaves it where it was; every other outcome moves it, integrated or not, since the time did pass. So it is when the estimate is valid only while the samples are being integrated: after a refused one the state is as old as the last that was.

pub fn state(&self) -> State

The current estimate, including its Status.

Status and validity are derived here rather than cached: they are pure functions of diagnostics, the covariance, and Config, so computing them on read means there is no invariant for the mutating methods to maintain. The cost is two rotations of the attitude block, one each for tilt and heading, six other covariance entries and six source timers, compared.

pub const fn angular_rate(&self) -> Option<AngularRate<Body>>

The body angular rate with the estimated gyroscope bias removed, ω of equation (9), from the last step predict integrated from a sample.

For moving the estimate to a point other than the IMU. The GNSS fuse_* take their antenna’s offset and refer the fix to the IMU themselves, (28′) and (29′); the estimate they correct stays the IMU’s, and a point at r in body axes (the center of mass a controller wants, a payload, a sensor this crate does not fuse) is at the IMU’s position plus R r and moves at its velocity plus R (ω × r). PX4 reports its estimate at the IMU the same way, and corrects each aiding source with this bias-corrected rate (EKF/aid_sources/gnss/gps_control.cpp:313-318 at c4e4ef98).

The algebra is the application’s own; nalgebra here, reached through arrays so that its version is the application’s too.

use fusion_nav::prelude::*;
use nalgebra::{Quaternion, UnitQuaternion, Vector3};
// The center of mass, 0.2 m behind the IMU: measured on the airframe.
let r = Vector3::from(Position::body(-0.2, 0.0, 0.0).to_array());

// None before the first step and across a gap, where no sample says how fast the
// vehicle turned.
if let Some(omega) = filter.angular_rate() {
    let state = filter.state();
    let q = state.attitude.body_to_ned();
    let rotation = UnitQuaternion::new_normalize(Quaternion::new(q.w, q.x, q.y, q.z));
    let position = Vector3::from(state.position.to_array()) + rotation * r;
    let omega = Vector3::from(omega.to_array());
    let velocity = Vector3::from(state.velocity.to_array()) + rotation * omega.cross(&r);
    // Yawing at 0.5 rad/s, a point 0.2 m aft swings sideways at 0.1 m/s.
    assert!((velocity.norm() - 0.1).abs() < 1e-3);
}

Committed with the state, and only with it. None until a step is integrated, after any initialization, and after a gap coasted past Config::max_predict_dt, until the next sample: a coast reads no sample, and the rate before the gap is not the rate after it. A refused step leaves the last rate in place, as it leaves the state.

pub const fn diagnostics(&self) -> &Diagnostics

Per-source health. Off the hot path.

pub const fn covariance(&self) -> &Covariance

The 15 x 15 error covariance, in the error-state ordering.

Trait Implementations

impl Clone for Eskf

fn clone(&self) -> Self

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for Eskf

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Default for Eskf

fn default() -> Self

A filter on Config::default, which validates (a test holds it to that).