Navigation Filter

The Extended Kalman Filter (EKF) implemented in src/filtering.py produces position, velocity, and clock solutions from synthetic measurements (measurements.json). This page documents the state, prediction model, and measurement linearisation used by the simulator.

Running the estimator

Invoke the CLI via

poetry run acons estimate --config configs/scenarios/....yaml --run-dir outputs/<scenario> \
    [--measurements-path /path/to/measurements.json] \
    [--output-subdir custom_tag]

By default the estimator loads measurements.json from <run-dir>/simulate/, but --measurements-path lets you point to an external catalogue (useful when you copy measurements between runs or produce them out-of-band). When overriding the measurements file the estimator writes its outputs next to that data set: the parent directory of the measurements file gains a sibling estimate/<output-subdir>/ folder containing the EKF logs, parquet files, and plots. If --output-subdir is omitted the artefacts go directly under estimate/. With the default layout the outputs therefore land in <run-dir>/estimate/<output-subdir>/.

The estimator now loads catalogues through the helper measurement_file_manipulation.load_measurement_catalogue so switching to alternative formats (chunked JSON, parquet, etc.) only requires swapping the reader instead of refactoring the CLI or EKF entry points. Before grouped EKF updates, the estimator drops heavy DataFrame metadata attributes from the loaded measurement table to avoid pandas deep-copy overhead when moving-user trajectory metadata is large.

When the estimation stage starts the CLI now prints a short bullet list summarising the measurement types, two-way contact cadence/selection strategy, injected delay (and whether simulation toggles it), and whether the delayed two-way measurement (TWM) state is being estimated. This trace log keeps the two-way assumptions visible alongside the EKF configuration so scenario tweaks are easy to audit in estimate/estimate.log.

State vector

The EKF maintains the eight-dimensional state

\[ \mathbf{x} = \begin{bmatrix} \mathbf{r}^\top & \mathbf{v}^\top & b_c & \dot b_c \end{bmatrix}^\top, \]

where \(\mathbf{r}\) and \(\mathbf{v}\) are the user position and velocity in metres and metres per second, while \(b_c\) (m) and \(\dot b_c\) (m/s) represent the receiver clock bias and drift.

Prediction model

ACONS uses an 8-state shared EKF loop, but the prediction block now depends on user.type.

For static and rover, the filter uses constant-velocity kinematics. With sampling interval \(\Delta t\), the state transition matrix is

\[ \mathbf{F} = \begin{bmatrix} \mathbf{I}_3 & \Delta t\,\mathbf{I}_3 & \mathbf{0}_{3\times1} & \mathbf{0}_{3\times1} \\ \mathbf{0}_{3\times3} & \mathbf{I}_3 & \mathbf{0}_{3\times1} & \mathbf{0}_{3\times1} \\ \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & 1 & \Delta t \\ \mathbf{0}_{1\times3} & \mathbf{0}_{1\times3} & 0 & 1 \end{bmatrix}. \]

The process noise covariance depends on the configured user type (user.type):

static and rover users apply a constant diagonal covariance sourced from estimation.process_noise_diag.
orbiter and edl users rely on the dynamic random-walk model governed by estimation.process_noise.

For orbiters and EDL profiles the covariance is block diagonal and derived from the estimation.process_noise entries. With sampling interval \(\Delta T\) the covariance is

\[ \mathbf{Q}_k = \begin{bmatrix} \sigma_a\,\Sigma_a & \mathbf{0}_{6\times2} \\ \mathbf{0}_{2\times6} & \sigma_{clk}\,\Sigma_{clk} \end{bmatrix}, \]

with

\[ \Sigma_a = \begin{bmatrix} \tfrac{\Delta T^3}{3} & 0 & 0 & \tfrac{\Delta T^2}{2} & 0 & 0 \\ 0 & \tfrac{\Delta T^3}{3} & 0 & 0 & \tfrac{\Delta T^2}{2} & 0 \\ 0 & 0 & \tfrac{\Delta T^3}{3} & 0 & 0 & \tfrac{\Delta T^2}{2} \\ \tfrac{\Delta T^2}{2} & 0 & 0 & \Delta T & 0 & 0 \\ 0 & \tfrac{\Delta T^2}{2} & 0 & 0 & \Delta T & 0 \\ 0 & 0 & \tfrac{\Delta T^2}{2} & 0 & 0 & \Delta T \end{bmatrix}, \qquad \Sigma_{clk} = \begin{bmatrix} \tfrac{\Delta T^3}{3} & \tfrac{\Delta T^2}{2} \\ \tfrac{\Delta T^2}{2} & \Delta T \end{bmatrix}. \]

Here \(\sigma_a\) represents the acceleration driving noise shared by the three position/velocity axes, while \(\sigma_{clk}\) controls the common bias/drift random walk. Both parameters are interpreted as one-sigma spectral densities (units \(m^2/s^3\) and \(m^2/s\) respectively) and should be tuned for each scenario.

Static/rover users instead specify estimation.process_noise_diag with four entries (position, velocity, clock_bias, clock_drift). These form the diagonal elements of \(\mathbf{Q}\) and remain constant regardless of the sampling interval, matching the stationary rover use case.

These terms model position/velocity random walks and clock bias/drift evolution. The prediction step follows the standard EKF recursion:

\[ \mathbf{x}_{k|k-1} = \mathbf{F}\,\mathbf{x}_{k-1|k-1},\qquad \mathbf{P}_{k|k-1} = \mathbf{F}\,\mathbf{P}_{k-1|k-1}\,\mathbf{F}^\top + \mathbf{Q}. \]

Configurable mode blocks (shared EKF loop)

The estimator now selects propagation and process noise through a mode block (src/ekf_models.py) while keeping one shared EKF/update pipeline (src/filtering.py, src/ekf_core.py):

static
constant-velocity propagation
diagonal process noise
rover
- constant-velocity propagation
constant diagonal process noise taken directly from estimation.process_noise_diag
orbiter
gravity-driven RK4 propagation in Mars monopole gravity
orbiter DMC process-noise model
edl
gravity-aware RK4 propagation without IMU coupling
adaptive process noise that increases with estimated speed

Current runtime semantics treat edl as the canonical descent user type. The legacy lander scenario label is still accepted for backward compatibility and is normalized internally to edl.

Moving-receiver truth handling

For moving rover and edl users, estimation reporting reconstructs receiver truth per epoch from measurements.json before computing state errors. This avoids comparing a trajectory-driven solution against one fixed receiver position.

The reconstructed truth uses the per-epoch fields:

receiver_x_m
receiver_y_m
receiver_z_m
receiver_vx_mps
receiver_vy_mps
receiver_vz_mps

grouped by jd.

static: constant-velocity propagation + static diagonal \(Q\).
rover: constant-velocity propagation + diagonal \(Q\) taken directly from estimation.process_noise_diag.
orbiter: RK4 position/velocity propagation with monopole Mars gravity + DMC coupled position/velocity \(Q\).
edl: currently uses the shared 8-state runner with adaptive descent-style \(Q\) scaling by speed.

This preserves one estimator entry point while allowing runtime mode selection via user.type.

Canonical descent semantics:

edl is the primary descent runtime mode.
lander remains accepted only as a backward-compatible alias that is normalized to edl.

Rover moving-batch update

For user.type: rover, the default runtime path is the normal per-epoch EKF update. An optional rolling measurement batch (MovingBatchEKF) remains available behind configuration for future rover studies that want to stack measurements across a short time window before each update.

You can switch rover behaviour between normal EKF and moving-batch EKF in config:

estimation:
  rover:
    batch_enabled: true      # true: moving-batch EKF, false: normal per-epoch EKF
    batch_seconds: 60        # rolling window length when batch_enabled is true

Default is batch_enabled: false, so rover scenarios use the standard EKF unless the batch mode is explicitly enabled.

Current rover IMU scope

The codebase does not yet ingest rover IMU streams in the estimate stage. To keep the rover interface stable for a future upgrade, the estimation config accepts a placeholder IMU block under estimation.rover:

estimation:
  rover:
    batch_enabled: false
    batch_seconds: 60
    enable_imu: false
    imu_source: null

Current behaviour:

enable_imu: false (default): estimator runs the current rover EKF path.
enable_imu: true: estimate stage stops with a clear error because rover IMU fusion is not yet enabled.

Moving-receiver truth alignment in estimation outputs

For user.type: rover and user.type: edl, estimation post-processing now derives the receiver truth trajectory directly from the simulated measurement catalogue (receiver_x_m/y_m/z_m, receiver_vx_mps/vy_mps/vz_mps) and aligns it with EKF states by jd. As a result, state_errors.csv and state_error_stats.csv no longer compare moving-user EKF results against a single fixed reference point; they compare each epoch against the corresponding truth sample.

Current EDL scope

The codebase currently does not ingest IMU streams in the estimate stage, so the full 14-state INS-coupled EDL implementation (attitude, accelerometer/gyro bias states, 100 Hz inner propagation) is not yet active in production runs. The current edl mode therefore keeps the shared 8-state measurement/update footprint and applies an adaptive process-noise model aligned with entry dynamics.

To keep the interface stable for a future upgrade, the estimation config accepts an IMU block:

estimation:
  edl:
    enable_imu: false
    imu_source: null

The legacy compatibility alias

estimation:
  lander:
    enable_imu: false
    imu_source: null

is still accepted, but new scenarios should use estimation.edl.

Current behaviour:

enable_imu: false (default): estimator runs the current 8-state EDL EKF path.
enable_imu: true: estimate stage stops with a clear error because IMU fusion is not yet enabled.

The repository EDL baseline seeds initial_state.velocity_mps with a value close to the first sample of the configured descent trajectory instead of starting from zero velocity.

Planned IMU file contract for future activation:

timestamp_s (seconds, monotonically increasing)
accel_x_mps2, accel_y_mps2, accel_z_mps2 (body frame)
gyro_x_radps, gyro_y_radps, gyro_z_radps (body frame)

Measurement model

For each measurement the EKF evaluates the predicted observable \(h(\mathbf{x})\), the Jacobian \(\mathbf{H} = \partial h/\partial \mathbf{x}\), and the innovation covariance

\[ S = \mathbf{H}\,\mathbf{P}_{k|k-1}\,\mathbf{H}^\top + \sigma^2, \]

where \(\sigma\) combines the thermal noise reported in measurements.json (range_noise_std_m, range_rate_noise_std_mps, etc.) with the per-measurement SISE variances (sise_range_variance_m2, sise_range_rate_variance_mps2, and their two-way equivalents). The Kalman gain is

\[ \mathbf{K} = \mathbf{P}_{k|k-1}\,\mathbf{H}^\top\,S^{-1}, \]

and the state/covariance update are

\[ \mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + \mathbf{K}\,\big(z - h(\mathbf{x}_{k|k-1})\big),\qquad \mathbf{P}_{k|k} = (\mathbf{I}-\mathbf{K}\mathbf{H})\,\mathbf{P}_{k|k-1}\,(\mathbf{I}-\mathbf{K}\mathbf{H})^\top + \mathbf{K}\sigma^2\mathbf{K}^\top. \]

Range measurements

Let \(\boldsymbol{x}_u = \boldsymbol{s}-\boldsymbol{r}\) be the line-of-sight vector from user to satellite, \(\rho = \lVert\boldsymbol{x}_u\rVert\), and \(\hat{\boldsymbol{u}} = \boldsymbol{x}_u / \rho\). A one-way range obeys

\[ h_\rho = \rho + b_c,\qquad \mathbf{H}_\rho = \left[-\frac{\boldsymbol{x}_u^\top}{\rho}\ \ \mathbf{0}_{1\times3}\ \ 1\ \ 0\right]. \]

Two-way range observables reuse the same geometric term but omit the user clock, so the Jacobian remains identical except the last two columns stay at zero (the EKF still accounts for the simulator’s two-way calibration bias through the measurement value).

Delayed two-way range (TWM)

When estimation.delayed_twm_enabled is set, the filter applies a Larsen-style delayed-measurement update for two-way range. Two-way observations are stored at acquisition time, the correction matrix is propagated across intermediate epochs, and the delayed update is applied upon arrival using the extrapolated measurement

\[ y_{k,s}^{ext} = y_s + h(\hat{x}_k^-) - h(\hat{x}_s^+). \]

Delay timing is configured under the measurement block. The simulator writes a two_way_delay_seconds column (per measurement) and the delayed EKF uses the exact fractional delay without rounding; the correction matrix includes partial Phi segments when needed.

measurement:
  two_way_delay_seconds: 5.0      # per-measurement column is populated with this value
  two_way_delay_simulate: true

Enable the delayed-TWM EKF path under estimation:

estimation:
  delayed_twm_enabled: true

To simulate delayed two-way data but still use the standard EKF (no delayed-measurement update), leave delayed_twm_enabled: false. When delayed two-way columns are present, the estimate stage swaps the _delayed two-way columns into the base columns before filtering.

Because delayed updates can introduce numerical asymmetry in the covariance, the filter symmetrizes the delayed-TWM covariance update and checks PSD without silently fixing it.

Doppler measurements

Define the relative velocity \(\dot{\boldsymbol{x}}_r = \dot{\boldsymbol{s}}-\dot{\boldsymbol{r}}\). Its projection along the line of sight is \(\dot{\rho} = \dot{\boldsymbol{x}}_r^\top \hat{\boldsymbol{u}}\) and the perpendicular component is

\[ \boldsymbol{p} = \dot{\boldsymbol{x}}_r - (\dot{\boldsymbol{x}}_r^\top \hat{\boldsymbol{u}})\,\hat{\boldsymbol{u}}. \]

The one-way range-rate model implemented in filtering.py is

\[ h_{\dot\rho} = \dot{\rho} + \dot b_c,\qquad \mathbf{H}_{\dot{\rho}} = \left[-\frac{\boldsymbol{p}^\top}{\rho}\ \ -\hat{\boldsymbol{u}}^\top\ \ 0\ \ 1\right]. \]

Two-way range-rate uses the same Doppler projection but removes the clock terms by setting \(\partial h/\partial b_c = \partial h/\partial \dot b_c = 0\). All Jacobians are evaluated per satellite before forming the innovation statistics described above.

DEM terrain constraint

When estimation.dem.enabled is set, the filter adds a scalar pseudo-measurement that constrains the predicted vehicle altitude above the terrain.

For static/rover, the DEM update uses direct terrain height matching (no AGL undulation model):

convert predicted body-fixed XYZ to geodetic (lat, lon, h) on the configured planet shape
sample DEM height h_terrain at (lat, lon)
form the residual model as h - h_terrain

\[ r_{\text{DEM,static}} = 0 - \left(\hat{h} - h_{\text{terrain}}\right). \]

The Jacobian for static/rover is linearised numerically from this same height residual model. The default implementation uses a faster local DEM-gradient Jacobian path for static/rover (with a legacy XYZ finite-difference fallback kept behind an internal flag for rollback/testing).

For edl, the filter keeps the shared planetary AGL model (with undulation) and linearises it numerically around the current state position:

\[ \mathbf{H}_{\text{DEM,edl}} = \left[ \frac{\partial h_{\text{AGL}}}{\partial x}\ \ \frac{\partial h_{\text{AGL}}}{\partial y}\ \ \frac{\partial h_{\text{AGL}}}{\partial z}\ \ \mathbf{0}_{1\times3}\ 0\ 0 \right]. \]

This keeps the EDL DEM update consistent with the same local terrain/undulation model used to predict AGL, rather than forcing a purely radial correction direction.

The measurement noise follows the Melman et al. (2024) guidance: static/rover users use \(\sigma_{\text{DEM}} = n\,\sqrt{\sigma_h^2 + \sigma_{\text{pos}}^2}\), while EDL cases default to lander_sigma_3sigma_m/3 (20 m at 3-sigma). When estimation.dem.enable_below_m is set for an EDL case, the filter uses the descent noise below the threshold and falls back to the surface DEM noise above it. If the DEM lookup returns NaN, the update is skipped. The DEM update is applied once per epoch by stacking the DEM row alongside the range and range-rate rows in a single EKF update.

For estimation.dem.mode, use static (or rover) or edl. The legacy alias lander is still accepted. Repository scenarios use static as the fixed-user DEM mode convention.

Altimeter (EDL only)

When measurement.altimeter is enabled for an EDL user, the simulator writes altimeter_agl_m, altimeter_true_agl_m, altimeter_noise_std_m, altimeter_vehicle_height_datum_m, and altimeter_undulation_m into the measurement catalogue. The EKF treats the altimeter as its own AGL measurement (not merged into the DEM constraint) and uses the same predicted planetary AGL as the DEM update:

\[ \hat{h}_{\text{AGL}} = \left(h_{\text{reference}} - N\right) - h_{\text{terrain,datum}}. \]

For edl, the altimeter uses the same numerically linearised planetary-AGL Jacobian as the DEM constraint, so the update follows the local terrain-height model rather than a purely radial approximation. The update is only applied when the estimated AGL is below measurement.altimeter.enable_below_m (default 1000 m), and the measurement noise is taken from altimeter_noise_std_m written by the simulator. If that column is unavailable, the EKF falls back to the nominal sensor noise derived from the configured altimeter model. The estimator requires estimation.dem.mode: edl; the legacy alias lander is still accepted for backward compatibility.

Example configuration:

estimation:
  dem:
    enabled: true
    base_dir: data/mars_dem/global
    global_filename: Mars_HRSC_MOLA_BlendDEM_Global_200mp_v2.tif
    method: bilinear
    mode: static
    sigma_h_m: 10.0
    n_sigma: 3.0
    sigma_pos_m: 0.0

DEM sweep helper:

The repo includes run_dem_sigma_sweep.sh, which updates estimation.dem.sigma_h_m in configs/scenarios/estimation/ekf_mars.yaml and runs the full pipeline for each value:

./run_dem_sigma_sweep.sh 10 20 40

EDL-mode example with an altitude gate:

estimation:
  dem:
    enabled: true
    base_dir: data/mars_dem/global
    global_filename: Mars_HRSC_MOLA_BlendDEM_Global_200mp_v2.tif
    method: bilinear
    mode: edl
    enable_below_m: 1000.0
    lander_sigma_3sigma_m: 20.0
    lander_use_1sigma_in_filter: true

Measurement set

During each epoch the EKF processes the entries from measurements.json in chronological order. Supported types are currently range and range_rate. Each record carries its own noise standard deviation (range_noise_std_m, range_rate_noise_std_mps), allowing heterogeneous observables to coexist inside the same update step. The reporting layer applies a robust MAD-based filter to residuals and state errors before producing summary statistics and plots.

Visibility-conditioned accuracy

state_error_by_satellite_count.csv captures how solution quality changes with the number of visible satellites. Each bin aggregates the EKF epochs that shared the same visibility count and reports the horizontal, vertical, and full 3D position error statistics (RMS, 95th, 99.7th percentiles). The horizontal component is evaluated in the local tangent plane, the vertical term is aligned with the receiver up-axis, and the 3D column reuses the radial error magnitude saved in the state-error timeseries. This makes it easy to spot thresholds where sparse geometry starts to blow up the solution.

Trace logging

poetry run acons estimate --log-level TRACE … now emits structured context before the EKF starts: the resolved receiver longitude/latitude/height/source, the measurement configuration (observable types, carrier/chip rates, two-way cadence/delay, RNG seed), and the estimator setup (initial position/ velocity/clock state plus process-noise scalars, DEM configuration, latency flags, downsampling). Once the EKF begins ingesting measurements, TRACE waits until the filter finishes and then reports a single summary covering the entire run: average/minimum satellite counts, covariance-derived σ statistics for every state component (mean, RMS, and max of the per-epoch standard deviations), plus innovation statistics for each measurement type (count, mean/rms residuals, predicted σ). The initialisation record still captures which observables are enabled and the starting covariance. These summaries appear inline with the textual log so you can quickly verify that the filter is using the expected measurements and noise settings without wading through thousands of lines.

Each reported σ statistic comes straight from the EKF covariance: at every epoch we read the relevant diagonal entry, take its square root to recover the one-sigma standard deviation, and add that value to a running list per state component. The TRACE summary then reports the mean, RMS, and maximum of those σ samples so you can see how the covariance-driven uncertainty evolves without digging into ekf_covariances.parquet. We also compute root-sum-square position and velocity σ values (combining the X/Y/Z or Vx/Vy/Vz entries) so you have a single scalar describing the 3D bounds for those state groups; their mean/RMS/max are logged alongside the per-component stats.

For each measurement type, TRACE additionally prints the mean σ_pred, which represents the EKF innovation standard deviation (the square root of the innovation covariance that combines thermal noise with the propagated state covariance). Comparing the residual RMS against σ_pred makes it easy to spot whether the EKF is optimistic (residuals ≫ σ_pred) or if the measurement settings are too noisy.

Once the EKF finishes and the state/residual outputs are written, the CLI appends another TRACE block summarising the realised 3D position error, 3D velocity error, and clock-bias error (each reported as mean/RMS/max|x|/P95/P99, derived from the difference between the estimated state and the configured truth). This provides an immediate “how did the solution perform?” snapshot without waiting to inspect state_error_stats.csv.