5-State EKF — Tight-Coupled GPS Pseudoranges

This demo runs a tightly-coupled 2D Extended Kalman Filter that consumes raw GPS pseudoranges directly, one satellite at a time. It is the structural extension of the 4D linear KF from Block 5: same predict-update rhythm, same per-state error panels, same animated trajectory with a 95% covariance ellipse — the only new ingredient is that the measurement Jacobian is recomputed at every pseudorange update from the current receiver-to-satellite geometry.

The headline contrast with the GPS demo from Block 6 is that the receiver here is not a black box. There, an NLLS solver pre-processed all the pseudoranges into a single position estimate, which a downstream filter would then ingest as a position measurement (loose coupling). Here, every individual pseudorange is its own scalar measurement and the filter processes them one at a time. The advantages are operational, not just theoretical:

• The filter still works with 1–3 satellites. The Block 6 black-box solver collapses below 4 satellites in 3D (3 in 2D); the EKF degrades gracefully.

• Each pseudorange brings its own innovation \(\nu_i = \tilde{\rho}_i - h_i(\hat{\mathbf{x}}^-)\). That residual is a per-sat indicator the filter can monitor — the foundation of fault detection in Block 8.

• The receiver clock bias becomes a state, learned over time alongside position and velocity instead of re-solved from scratch every update.

• Sensor cadences mix freely. Each satellite can deliver pseudoranges at its own rate; the filter handles them in the order they arrive.

The 5-state model

The state vector adds the receiver clock bias \(b\) to the constant-velocity 4D model:

\[ \mathbf{x} = \begin{bmatrix} p_x & p_y & v_x & v_y & b \end{bmatrix}^{\top}. \]

The transition matrix is the constant-velocity block plus a random-walk row for \(b\):

\[\begin{split} \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & \Delta t & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}, \qquad \mathbf{Q}_d = \begin{bmatrix} \mathbf{Q}_{\text{nav}} & \mathbf{0} \\ \mathbf{0}^{\top} & \sigma_b^2\,\Delta t \end{bmatrix}. \end{split}\]

\(\mathbf{Q}_{\text{nav}}\) is the 4D constant-velocity process-noise matrix from Block 5; the clock-bias channel evolves as a random walk with rate \(\sigma_b\).

The pseudorange measurement function for satellite \(i\) is the same nonlinear \(h_i\) from the Block 6 reading:

\[ h_i(\mathbf{x}) = \lVert \mathbf{p} - \mathbf{s}^{(i)} \rVert + b. \]

Linearizing at the current prior \(\hat{\mathbf{x}}^-\) gives the \(1 \times 5\) Jacobian

\[ \mathbf{H}_i = \begin{bmatrix} \hat{e}_x & \hat{e}_y & 0 & 0 & 1 \end{bmatrix}, \qquad \hat{e} = \frac{\hat{\mathbf{p}}^- - \mathbf{s}^{(i)}}{\lVert \hat{\mathbf{p}}^- - \mathbf{s}^{(i)} \rVert}. \]

Three things to read off this row:

• Position entries are the unit vector from satellite \(i\) toward the receiver — same line-of-sight direction the Block 6 reading derived.

• Velocity entries are zero. Pseudoranges don’t depend instantaneously on velocity, so a single pseudorange update touches velocity only through the cross-covariance.

• Clock-bias entry is exactly 1. \(b\) adds linearly to every pseudorange, so a unit change in \(b\) produces a unit change in the predicted measurement.

Interactive demo

Open in full screen

The layout matches the multi-state KF demos: live 2D trajectory with the 95% position-covariance ellipse, five stacked per-state error panels with \(\pm 95\%\) bounds, animation control bar with scrubber, and synchronized “now” cursors across all panels. The new UI element is the sky compass in the top-right fieldset — a small unit-direction circle showing each satellite’s bearing from the receiver. Drag any satellite around the compass to change geometry. Each satellite has its own color, on/off toggle, and update-interval slider so you can mix sensor cadences freely.

Walkthrough

The demo opens at a default scenario: 4 satellites evenly spread, \(\sigma_\rho = 10\) m, all sats updating at 1 Hz, \(\sigma_a = 2\) m/s², \(\sigma_b = 3\) m/√s, \(T = 300\) s. The filter starts with deliberate position uncertainty (\(\sigma_p = 10\) m), velocity uncertainty (\(\sigma_v = 1\) m/s), and clock uncertainty (\(\sigma_b = 50\) m); truth is sampled around that prior. Try the following:

1. Read the headline metrics. Position estimate within a few meters of truth, velocity within a fraction of a m/s, clock bias estimate tracking truth within a couple of meters. The cov ellipse on the trajectory plot stabilizes into a small near-circular shape; all five error panels stay inside their bounds. The filter has learned the clock bias as a state, alongside position and velocity, from purely scalar pseudorange measurements.

2. Watch the per-satellite update flashes. Four colored dots in the top-left of the trajectory panel — they fade in when their satellite’s measurement is processed and decay over a couple of seconds. Each flash corresponds to one scalar EKF update with a 1×5 Jacobian, applied in sequence inside the same time step.

3. Toggle off SAT 2, 3, and 4. Leave only SAT 1. This is the headline contrast with the Block 6 demo. The black-box NLLS solver would refuse to converge with one satellite. The EKF keeps producing an estimate — bounds on the components perpendicular to SAT 1’s line of sight widen visibly, but the filter never breaks. With time, even one sat at 1 Hz keeps the position channel observable through the cross-covariance and the prior’s continuing predictions.

4. Re-enable a second satellite at 90° from the first. Position bounds collapse along that newly-observed direction. Two complementary lines of sight are enough to constrain 2D position firmly — and the filter never had to know that ahead of time, it just used the measurements as they arrived.

5. Slide individual satellite intervals. Pull SAT 1 to 0.1 s and SAT 2–4 to 5 s. SAT 1 dominates the update cadence; the cov ellipse tightens along its LoS direction much faster than along the others. Tight coupling lets sensors at different cadences contribute proportional amounts of information without any pre-aggregation step.

6. Click “Clustered (bad)” preset. All four satellites collapse into a 30° arc. The cov ellipse balloons along the cluster’s LoS direction (same anisotropic stretch as the GPS demo from Block 6 — geometry still matters in the EKF). HDOP isn’t displayed numerically, but you can see it visually in the ellipse’s elongation. Drag a single satellite away from the cluster: the ellipse snaps tighter immediately.

7. Slide \(\sigma_\rho\) up to 30 m. Bounds widen across all states; the per-step Kalman gain on each scalar update shrinks (filter trusts the prior more when the measurement is noisy). The geometric ellipse anisotropy stays unchanged — that’s structurally separate from per-sat noise.

8. Slide \(\sigma_b\) up to 10 m/√s. The clock-bias error panel shows wider bounds; the bias channel converges less tightly because the filter expects more drift between updates. The clock-bias state’s \(\mathbf{Q}\) entry is a tuning knob just like any other process noise.

9. Pause and scrub. Drag the time slider; the cov ellipse, all five error-panel cursors, sky compass, and stats card all snap to that time so you can inspect filter state at any moment.

Key observations

• Same recursion as the linear KF. The predict-update equations are unchanged from Block 5; only the measurement model gets re-linearized at every update. Predict moves the state forward; update fuses the measurement with the prior using inverse-variance weighting; covariance shrinks under each update and grows under each predict.

• The 1×5 Jacobian carries everything. Position entries point along the line of sight; velocity entries are zero (no instantaneous velocity sensitivity in pseudoranges); clock-bias entry is unity. The Jacobian re-evaluates as \(\hat{\mathbf{x}}^-\) moves, but its structure stays the same.

• Cross-covariance picks up velocity. Pseudorange Jacobians have zero velocity entries, but the filter still produces a velocity estimate because \(\mathbf{P}\) has non-zero \(p\)-\(v\) off-diagonals from \(\mathbf{F}\)’s \(\Delta t\) couplings. Watch the velocity bounds in the per-state panel: they tighten over a few cycles even though no measurement directly observes velocity.

• Tight coupling separates sensor cadence from filter cadence. The filter runs at \(\Delta t = 0.1\) s; each satellite delivers its measurement on its own schedule (1 Hz, 5 Hz, 10 s, whatever). The filter handles them as they arrive without any pre-aggregation.

• Geometry still controls observability. Cluster the satellites and the cov ellipse balloons along the cluster’s LoS, exactly as in the Block 6 black-box demo. The EKF formulation does not magic away DOP; it just makes every other operational property of the receiver more graceful.

• Per-satellite innovations are now available. Each scalar update produces an innovation \(\nu_i\) that the filter could test for outliers (does it exceed some multiple of its expected \(\sigma\)?). Block 8 builds exactly that test, and the per-sat structure of the EKF is the reason it can be done at all.

Source

MATLAB · code/ExtendedKalmanFilter5D.m↓

With the 2D covariance ellipse helper in code/navutils/Draw2DErrorBounds.m.