Skip to content

Estimators

Error-state Kalman filter fusing inertial, GNSS and visual data.

Formulation

Nominal state x = (R, p, v, b_a, b_g). The filter carries a 21-dimensional error state in the order

``[dtheta (0:3), dp (3:6), dv (6:9), db_g (9:12), db_a (12:15),
   c_p (15:18), c_t (18:21)]``

The first fifteen are the usual inertial states. c_p and c_t are the errors of the stored visual anchor position and attitude: nuisance parameters, not physical states, needed because an anchor is a pose the filter itself produced and its error is therefore not independent evidence. They are what makes the visual channel sound to enable, and the module docstring below explains both the failure they fix and the one they do not.

The convention that the true state relates to the nominal one as R_true = R Exp(dtheta), p_true = p + dp, v_true = v + dv, and the biases additively. Gravity is a known constant, not a filter state.

This is the textbook first-order error-state formulation, not an invariant one: the update equations are linearised around the current estimate and the transport is not enforced through the group. That choice is deliberate and documented in docs/architecture.md -- an invariant EKF gives marginally better behaviour in fast rotation, and buys it with algebra that a reader cannot check by inspection.

Process model (discrete, first order), with a = accel - b_a:

dp'    = dp + dv dt
dv'    = dv - R [a]x dt dp_theta - R dt db_a
dtheta' = Exp(-omega dt) dtheta - dt db_g
Measurement models
  • GNSS position: z = p, H = [0 I 0 0 0].
  • Visual relative rotation R_rel = R_{i-1}^T R_i: the residual Log(R_meas R_pred^T), which equals the attitude error exactly, with H = [I 0 0 0 0] and an isotropic noise covariance.
  • Visual relative translation t_rel = R_{i-1}^T (p_i - p_{i-1}): with h(x) = R_prev^T (p_cur - p_prev) the residual is t_meas - h(x_nom) and H = [0 R_prev^T 0 0 0].

The previous side of a visual constraint is the filter state saved at the previous visual update, not at the previous inertial tick. At 200 Hz inertial against 20 Hz vision those are different poses, and using the inertial tick would make the relative constraint compare the current frame with itself.

A gate rejects updates whose normalised residual exceeds gate_sigma times the predicted standard deviation, and the rejection count is reported. Silent gating would hide a divergence, so it is counted rather than merely applied.

Fusing vision without corrupting the filter

A relative-pose visual measurement constrains how the vehicle moved, not where it is. Fusing one by re-deriving the visual anchor from the filter's own corrected state -- the obvious implementation, and the one this filter started with -- turns the two into the same thing: the filter is handed its own output and averages it as though it were fresh independent evidence. The covariance collapses, and once it has collapsed the filter is certain and wrong.

Measured on the 20 s synthetic fixture (200 Hz inertial, 20 Hz vision at 0.05 deg and 1 cm, 5 Hz GNSS at 0.1 m, 2 mm/s^2 accel noise density, 0.01 m/s gyro noise density), reported as mean squared Mahalanobis distance (NEES, where a calibrated 3D filter reports 3.0):

configuration                  GNSS used   error    1-sigma    NEES
GNSS only (control)               101/101   0.039 m   0.073 m    2.8
vision, anchor folded into R       55/101  16.11 m    0.037 m    5.1e4
vision, anchor folded into R + FDIR 27/101  4.69 m    0.037 m    1.5e4
vision, anchor as a state          101/101   0.033 m   0.068 m    3.2

The middle row is the failure in one line: the claimed 1-sigma is 3.7 cm while the error is 16 m, and 46 of 101 absolute GNSS fixes are discarded as outliers by the innovation gate. A system monitoring only its own gating counters would see this as "vision is noisy", when the actual fault is that vision has silenced the GNSS.

The third row is that same defect with the FDIR gate active (ADR-0005), and it is the clearest statement of what the gate can and cannot do here. FDIR rejects 74 of 101 fixes instead of 46, which looks like an improvement -- the error does drop to 4.69 m. It is not one. Three quarters of the absolute fixes are now gone, and the claimed 1-sigma is unchanged at 3.7 cm, so the NEES only falls from 5.1e4 to 1.5e4. The gate is applying a chi-square test to an innovation covariance that the defect has already made wrong, and a stricter threshold on a wrong denominator buys a smaller number rather than a more correct filter. The run that the regression test measures is the FDIR-off one, on purpose: FDIR partially masks this defect, and a test that measured the masked version would be measuring the mitigation rather than the fault.

The fix is the last row, and it takes two things that are easy to get wrong:

  • The anchor error is carried as six estimated states (c_p, c_t) rather than folded into the innovation covariance. An error common to every visual update is a quantity to be estimated, not noise to be divided down.
  • Those states are given a declared uncertainty, not the filter's own covariance. Deriving the new anchor's uncertainty from the current covariance is circular, and it ratchets the overconfidence downward at every re-commit.
  • The covariance update uses the Joseph form. The textbook shortcut (I - K H) P is symmetric but not positive semidefinite; with the anchor states it drove the smallest eigenvalue to -0.31 within two visual updates, which is a physically meaningless covariance rather than a small inaccuracy.

With GNSS available the filter is calibrated again, and slightly better than the GNSS-only control because the visual channel adds real information. Vision with no GNSS at all is also calibrated (mean NEES 4.2, 1.1 m final error), so the anchor model is doing real work rather than only muting the symptom.

The limit that remains

Under a 15 s GNSS denial the same filter still over-trusts vision (mean NEES 286.2, 2.322 m error against a claimed 0.162 m), and the cause is structural rather than a matter of tuning. The information a visual measurement carries about absolute position is the Schur complement

P_pp - P_pc P_cc^-1 P_cp

Declaring the anchor blocks uncorrelated collapses this to P_pp: the measurement acts as if the anchor did not exist. Declaring them perfectly correlated -- which is the honest description of an anchor copied from the current estimate -- collapses it to zero and makes the measurement model exactly degenerate, which is numerically unusable. The truth lies between those extremes and cannot be represented by one anchor state, because a chain of relative measurements is only jointly informative about position once the whole chain is considered. That requires a pose graph over the visual keyframes, not a single-anchor ESKF, and it is the change this project has not made.

So the visual channel ships disabled by default. The failure above is not a reason to hide it, and the numbers stay in this docstring; it is a reason not to hand a caller a filter that silently reports 0.162 m while being 2.32 m wrong. vision_anchor_modelled is exposed only so the regression tests can reproduce the middle row.

Known simplifications
  • Measurements are applied at the next inertial tick, so a measurement carries a latency of up to one IMU period (5 ms at 200 Hz). This is recorded in the result and is far below the timescales this project studies, but it does put a floor under the effect of the timestamp_offset scenario.
  • With the visual channel disabled there is no constraint tying the bias states to anything but GNSS excitation, so a vision-only run has no absolute reference at all. Its error grows, and the covariance is expected to grow with it; a vision-only run cannot demonstrate a benefit over dead reckoning and must not be reported as one.
  • The camera-to-IMU extrinsic is assumed identity.
  • No motion-constrained zero-velocity or zero-rotation update is applied, so bias observability comes only from GNSS excitation over the sequence.
  • run(t0=...) truncates the inertial stream and cold-starts the filter at that instant with zero position, zero velocity and identity attitude. It does not mean "assist until t0 and then deny". A denial window must be expressed as a scenario on the fix stream, not as a start time.

Bases: TypedDict

The filter's propagated state.

This was annotated dict[str, np.ndarray], which was wrong in both directions: it forbade the integers, the boolean, and the None that the state genuinely holds, so the 12 type errors it produced were being silenced with disable_error_code in pyproject.toml. Naming the members makes a wrong key or a wrong dtype a type error at the point of the mistake, and it lets the silence be removed.

anchor_snapshot is optional in practice -- it is absent until the first GNSS outage opens, and the cross-check falls back to the live anchor when it is missing -- so total=False models that honestly.

Where a run starts, when it does not start at the origin at rest.

The default start is the identity pose at rest with zero biases, which is right for the synthetic benchmark and wrong for any recorded sequence. A caller that knows the pose, velocity and biases at the first sample (from a reference trajectory, or from an initial alignment) passes them here. The covariance is not part of it: the declared initial_*_sigma terms in :class:EskfConfig still say how far from the truth the start is allowed to be.

Tuning and noise for the filter, in physical units.

21-state error-state Kalman filter over a timestamped IMU stream.

Dead-reckoning reference estimator.

Integrates the (possibly degraded) inertial stream with no external updates. It exists for two reasons:

  1. It is the honest lower bound: with no aiding, inertial integration error accumulates without bound. Any aiding scheme must beat it, and showing the gap is more informative than showing one number.
  2. It isolates the replay harness from the fusion algorithm. If dead reckoning diverges by roughly the amount implied by the injected noise and bias, the harness is behaving; the divergence is not a bug in the replay plumbing.

Nominal propagation (world frame, gravity assumed known):

v <- v + (R f + g) dt
p <- p + v dt + 0.5 (R f + g) dt^2
R <- R Exp(omega dt)

Output of any estimator: the estimated trajectory plus run metadata.

Midpoint (constant-acceleration) inertial integrator.