Struct NavFilter 

pub struct NavFilter<R: RealField> { /* private fields */ }

A 17-state error-state Kalman filter: the error-state estimate + its covariance.

Implementations

impl<R: RealField> NavFilter<R>

pub fn new( state: InsErrorState<R>, cov_diag: [R; 17], ) -> Result<Self, PhysicsError>

Build the filter from an initial error-state estimate and an initial covariance diagonal.

cov_diag is indexed in the canonical 17-state order [pos(3), vel(3), att(3), accel_bias(3), gyro_bias(3), clock_bias, clock_drift], the order InsErrorState::to_array packs. Entry i is the initial variance of state i.

Errors

Rejects a cov_diag that is not a covariance diagonal: a non-finite entry, or a negative variance. A diagonal is symmetric by construction, so only finiteness and non-negativity are checked here (see validate_covariance). Admitting a zero or negative variance here is what makes the degenerate measurement-update path reachable, so it is refused at the entry point.

pub fn predict( &mut self, dt: R, specific_force: [R; 3], process_noise_diag: [R; 17], ) -> Result<(), PhysicsError>

Predict one step: propagate the error state and P ← F·P·Fᵀ + Q_d, where the process noise is the first-order discretisation Q_d = Q_c·dt of the caller’s continuous-time input.

process_noise_diag is indexed in the canonical 17-state order [pos(3), vel(3), att(3), accel_bias(3), gyro_bias(3), clock_bias, clock_drift], the order InsErrorState::to_array packs.

It is a continuous-time process-noise spectral density (units: state²/s, e.g. m²/s on the position block, (m/s)²/s on velocity), not an already-discretised per-step covariance. Scaling it by dt is what makes the accumulated covariance a function of elapsed time rather than step count: over a fixed horizon T = N·dt, the additive noise is N·(Q_c·dt) = T·Q_c, independent of dt, so the filter’s tuning survives a change of step size. Without the dt factor (the pre-2026-07-24 behaviour) halving dt over a fixed horizon doubled the accumulated process noise and silently re-tuned the filter.

Q_d = Q_c·dt is the standard first-order (Euler–Maruyama) discretisation of the random-walk and white-noise terms this filter carries — IMU bias random walk and clock noise (Groves 2013, §14.2.4). The within-step cross-coupling the transition matrix induces (a Van Loan discretisation would capture it) is deliberately not modelled here: it buys accuracy the filter’s other Tier-A approximations (C ≈ I, no Earth rotation) do not warrant.

Errors

Refuses a step it cannot discretise, rather than reporting success while writing a NaN or a negative variance into the covariance:

• a non-finite or non-positive dt. dt = NaN poisons every entry of F and therefore all of P; dt < 0 runs the discretisation backwards and subtracts |dt|·Q_c from the diagonal, leaving negative variances that no later check inside the filter looks at; dt = 0 is a step that does not advance and adds nothing, so it is refused as a caller error rather than silently absorbed.
• a negative or non-finite process-noise entry. A spectral density is non-negative by definition, and a negative one shrinks the covariance on a predict — uncertainty falling while dead-reckoning is the wrong direction to be wrong for a GNSS-denied estimate.

Rejection is atomic: both checks precede any mutation, so a refused predict leaves the state and covariance exactly as they were and the caller can retry the step. The ReentryNavEngine::predict that drives this filter already returns a Result, so the refusal reaches the marcher.

pub fn update_scalar( &mut self, h: [R; 17], z: R, r: R, ) -> Result<(), PhysicsError>

Fold in one scalar measurement z = h·δx + noise with measurement variance r (a sequential scalar update; S = h·P·hᵀ + r is a scalar, so no inversion). Corrects the estimate and shrinks the covariance.

h is the measurement row in the canonical 17-state order [pos(3), vel(3), att(3), accel_bias(3), gyro_bias(3), clock_bias, clock_drift], the order InsErrorState::to_array packs. One axis of a position fix sets h[i] = 1 for i in 0..3; the clock bias is index 15 and the clock drift index 16.

The covariance update is the Joseph form P ← (I−K·h)·P·(I−K·h)ᵀ + r·K⊗K, followed by a re-symmetrization. The simple form P − K⊗(h·P) loses symmetry and positive-definiteness under long sequences of near-unity-gain folds (a precise receiver folded every step), after which the cross-term gains change sign and the injected corrections diverge; Joseph is PSD-preserving unconditionally (Groves 2013, §3.4.3).

What the re-symmetrization guarantees, and what it does not. Entries [i][j] and [j][i] are written from the same two summands, so P is symmetric when this method returns. That is a per-update property, not a running one: predict forms F·P·Fᵀ + Q_d and does not re-symmetrize, and the two triple products accumulate their sums in different orders, so float-level asymmetry re-enters at every predict step. Nothing here bounds how far asymmetry or the positive-semi-definiteness margin drifts over a long run, and Joseph’s PSD preservation is an exact-arithmetic result. In floating point it is the robust form, not a proof. The standing checks are the guards below (a refused update mutates nothing) and the validate_covariance screen, which runs only at construction and restore.

What is measured, and what is still unpinned. The predict half now is: a 5000-step predict-only coast is held against the √ε band restore admits, and the measured residual sits around 3e-7 of it — sub-linear in step count (10× the steps moved it by 2.6×, the signature of a random walk rather than amplification), so a snapshot taken after a long dead-reckoning coast is not a rejection hazard. The fold half is not: the longest sequence in the suite is 60 position fixes (closed_loop_tests), and those tests assert position error and variance collapse; none reads covariance after a long near-unity-gain sequence to bound max|P[i][j] − P[j][i]| or to check vᵀPv ≥ 0. A refactor back to the simple form would still pass CI.

Errors

Refuses a measurement it cannot fold, rather than writing a NaN into the state and covariance:

• a non-finite measurement z (a garbage sensor fix): k·z would poison every state component, including the zero-gain ones (0·NaN = NaN), while the covariance stays finite — an invisible corruption. z is not read until after this check, so the rejection stays atomic;
• a negative or non-finite measurement variance r (a variance is non-negative by definition);
• a non-positive or non-finite innovation covariance s = h·P·hᵀ + r, which the gain divides by.

Rejection is atomic: every check precedes any mutation, so a refused update leaves the state and covariance exactly as they were. This matters for the sequential per-axis folds in ReentryNavEngine::correct_position, where a rejection on one axis must not leave an earlier axis half-applied.

pub fn covariance(&self) -> &[[R; 17]; 17]

The full error-state covariance matrix (snapshot access; diagnostics use position_variance / covariance_trace).

pub fn restore( state: InsErrorState<R>, cov: [[R; 17]; 17], ) -> Result<Self, PhysicsError>

Rebuild a filter from snapshotted state and covariance: the exact inverse of reading state and covariance. Exists for the state-snapshot resume path.

Errors

Rejects a cov that is not a covariance: a non-finite entry, an asymmetry beyond the validate_covariance tolerance, or a negative variance on the diagonal. A snapshot carrying such a matrix was already broken; failing loudly at restore is better than continuing from it (a non-symmetric or negative-variance covariance drives the measurement update to a NaN).

pub fn state(&self) -> &InsErrorState<R>

The current error-state estimate.

pub fn reset_navigation_error(&mut self)

Apply the ESKF feedback reset: zero the navigation-error part of the estimate (position, velocity, attitude) after it has been injected into the nominal trajectory. The learned bias and clock states persist. (The covariance is unchanged — the reset moves the mean, not the spread.)

pub fn position_variance(&self) -> R

The position-error variance (trace of the 3×3 position block) — the reacquisition witness.

pub fn covariance_trace(&self) -> R

The full covariance trace (total filter uncertainty).

Trait Implementations

impl<R: Clone + RealField> Clone for NavFilter<R>

fn clone(&self) -> NavFilter<R>

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl<R: Debug + RealField> Debug for NavFilter<R>

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

Formats the value using the given formatter.