Skip to content

Delayed Two-Way Measurements (TWM)

This note summarizes how delayed two-way measurements are handled in the EKF, following twm_implementation_codex_optimized.txt (Comellini/Larsen/Silvestrini).

Packet lifecycle

At arrival time, each two-way measurement constructs a delayed packet using the stored state history and its acquisition timestamp t_s:

  • measurement_id - unique identifier
  • t_s / t_k - acquisition and arrival times
  • x_s, P_s - reconstructed state/covariance at t_s (Spec 3.2)
  • H_s - Jacobian at acquisition (Spec 3.2)
  • R_s - measurement variance
  • M - correction matrix, initialized to I (Spec 5.2)

Fractional delays are preserved via two_way_delay_seconds, and the correction matrix uses partial Phi segments instead of rounding.

For delayed-TWM runs, the estimator expects the _delayed two-way columns and the two_way_delay_seconds field to be consistent with each other.

Update ordering

At each epoch k, the filter enforces the required order (Spec 6):

1) Predict -> x_hat_{k|k-1}, P_{k|k-1} 2) Fast measurements -> x_hat_{k|k}, P_{k|k} 3) Delayed TWM arrivals -> x_hat_{k|k;delta}, P_{k|k;delta}

Correction matrix recursion

For every in-flight packet, update:

M_j = (I - K_j H_j) Phi_j M_{j-1}

This is implemented in update_M_for_inflight_packets (Spec 5.2).

Larsen / extrapolation update

At arrival epoch k, the delayed update uses:

y_k^ext = y_s_delta - C_s x_hat_{s|s-1} + C_k x_hat_{k|k-1}

and applies the gain/state/covariance update exactly as in Spec 5.4.

Baseline (no delayed compensation)

Set estimation.use_delayed_two_way_measurements: true to force the standard EKF to ingest the *_delayed two-way columns directly (no Larsen correction). This provides a fair baseline for comparing delayed fusion vs. “delay ignored”.

Implementation entry points:

  • predict_step
  • fast_update_step
  • update_M_for_inflight_packets
  • larsen_delayed_update

All covariance updates are symmetrized and checked for PSD (Spec 8).