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

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 (receiver.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 uses the same geometric sensitivity while zeroing the clock columns, i.e. \(\partial h/\partial b_c = 0\) and \(\partial h/\partial \dot b_c = 0\).

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

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.

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.