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.

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 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):

surface 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.

Surface 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}. \]

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]. \]

For two-way range the EKF doubles the geometric range and Jacobian while zeroing the clock columns, i.e. \(\partial h/\partial b_c = 0\) and \(\partial h/\partial \dot b_c = 0\).

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 doubles the geometric Doppler while removing 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 radial constraint

When estimation.dem.enabled is set, the filter adds a scalar pseudo-measurement that constrains the radial distance to the Martian surface using a Digital Elevation Model (DEM). The DEM height is sampled at the estimated latitude/longitude with src.dem.mars_dem.mars_elevation, and the measurement is constructed as

\[ \rho_{\text{DEM}} = h_{\text{ell}}(\phi,\lambda) + h_{\text{DEM}}(\phi,\lambda), \]

with the predicted value

\[ \hat{\rho}_{\text{DEM}} = \lVert \mathbf{r} \rVert. \]

The Jacobian matches the radial unit vector,

\[ \mathbf{H}_{\text{DEM}} = \left[\frac{\mathbf{r}^\top}{\lVert \mathbf{r} \rVert}\ \ \mathbf{0}_{1\times3}\ \ 0\ \ 0\right]. \]

The measurement noise follows the Melman et al. (2024) guidance: surface users use \(\sigma_{\text{DEM}} = n\,\sqrt{\sigma_h^2 + \sigma_{\text{pos}}^2}\), while lander cases default to lander_sigma_3sigma_m/3 (20 m at 3-sigma). When estimation.dem.enable_below_m is set for a lander, the filter uses the lander noise below the threshold and falls back to the surface DEM noise above it (representing the altimeter being disabled). 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. The DEM observation is computed from the predicted position at that epoch (so height is never treated as a state), then included in the combined measurement vector. When estimation.dem.reference_radius_m/estimation.dem.flattening are omitted, the DEM constraint uses user.body_radius_m when available. If planet_shape is set, sphere uses the mean radius and zero flattening while ellipsoid uses the semimajor axis and flattening from configs/environment/constants.yaml. If no user radius or constants are available, set estimation.dem.reference_radius_m explicitly and estimation.dem.flattening as needed. Set estimation.dem.lander_use_1sigma_in_filter to true (default) to convert the 20 m (3-sigma) lander accuracy into a 1-sigma value; set it to false to use the 20 m directly. For estimation.dem.mode, use surface (rover) or lander.

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: surface
    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

Lander-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: lander
    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

Run the CLI with --log-level TRACE to inspect what the EKF is doing internally. Rather than printing every epoch, the filter aggregates roughly four-hour windows and reports a concise summary: average/minimum satellite counts, mean/range of ‖x‖ and tr(P), plus innovation statistics for each measurement type (count, mean/rms residuals, predicted σ, thermal σ). 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.