# 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$:
$$
\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}.
$$
$\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`.