# Block 7 — Extended Kalman Filter for GPS

By the end of this block you should be able to:

1. Distinguish between **loose-coupling** and **tight-coupling** GPS/INS integration.
2. Apply the standard linear Kalman filter when GPS provides a position measurement.
3. Explain why pseudorange measurements require an Extended Kalman Filter.
4. Compute the pseudorange Jacobian $\mathbf{H}$ from the receiver-to-satellite geometry.
5. Trace through one complete EKF predict-update cycle with GPS pseudoranges.

## Two Ways to Fuse GPS into a Filter

There are two architectural choices for integrating GPS into a navigation filter, and the difference comes down to whether the filter sees the GPS solver's output or the raw pseudoranges.

![Side-by-side architecture diagram. Left: pseudoranges → GPS solver → position → linear KF. Right: pseudoranges → EKF directly → 5-state output.](_media/ekf_loose_vs_tight.png)

*Fig. 1.1. The two GPS/filter integration patterns. Loose coupling pre-solves the four-unknown GPS problem and feeds the result to a linear KF. Tight coupling skips the pre-solve and lets the EKF process raw pseudoranges, taking over the clock-bias estimation as part of its state.*

### Loose coupling

In the loose-coupling pattern, a GPS solver runs first (the same NLLS routine from Block 6), produces a solved position $\hat{\mathbf{p}}$ and a covariance, and the filter ingests that as a linear position measurement:

$$
\mathbf{z}_k = \hat{\mathbf{p}}_k + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}).
$$

The observation matrix $\mathbf{H} = [\mathbf{I}\ |\ \mathbf{0}]$ is the same one we used for the position-only sensor in Block 5. No extra math, no Jacobians: it is a standard linear Kalman filter update. $\mathbf{R}$ is set from the GPS accuracy specification, $\sigma_{\text{GPS}}^2 \approx (\mathrm{HDOP}\times\sigma_\rho)^2$.

### Tight coupling

In the tight-coupling pattern, the filter ingests raw pseudoranges directly:

$$
\tilde{\rho}_i = \lVert \mathbf{p} - \mathbf{s}^{(i)} \rVert + b + \epsilon_i.
$$

This measurement model is **nonlinear in user position**, so the standard KF, which requires $\mathbf{z} = \mathbf{H}\mathbf{x} + \mathbf{v}$, cannot be applied directly. The fix is the Extended Kalman Filter (EKF): linearize $h_i(\mathbf{x})$ around the current best estimate, and run the standard KF update with that linearized $\mathbf{H}$.

Tight coupling is preferred in practice because:

- The filter degrades gracefully when only one or two satellites are visible. A loose-coupling GPS solver needs four satellites just to produce an output; an EKF can update on whatever satellites it sees.
- The clock bias becomes part of the navigation state, so the filter learns the clock dynamics over time.
- The error model is more honest: the filter uses the real pseudorange uncertainty rather than the post-solve position uncertainty, which has been smoothed by the NLLS solver.

The MATLAB demo in this block, `ExtendedKalmanFilter5D.m`, runs the tight-coupling case. The 4-state Block 5 demo with a position-only sensor IS the loose-coupling case, just relabeled.

## The Tight-Coupling Challenge

The standard Kalman update equation requires a linear measurement model:

$$
\mathbf{z} = \mathbf{H}\mathbf{x} + \mathbf{v}.
$$

There is no matrix $\mathbf{H}$ that exactly represents a Euclidean range — that is what "nonlinear" means. The way out is to approximate the nonlinear function with a tangent hyperplane at every step and let the filter run on the approximation. As long as the approximation error is small relative to the measurement noise, the filter behaves nearly identically to the linear case.

## EKF Key Idea: Linearization

Replace $h(\mathbf{x})$ with its first-order Taylor expansion around the current predicted state:

$$
h(\mathbf{x}) \approx h(\hat{\mathbf{x}}^-) + \underbrace{\frac{\partial h}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}^-}}_{\mathbf{H}} \big(\mathbf{x} - \hat{\mathbf{x}}^-\big).
$$

The matrix $\mathbf{H}$ is the Jacobian of $h$ evaluated at $\hat{\mathbf{x}}^-$. The innovation now uses the **nonlinear** function while the gain and covariance updates use the **linearized** $\mathbf{H}$:

$$
\boldsymbol{\nu}_k = \tilde{\rho}_k - h(\hat{\mathbf{x}}_k^-).
$$

After that, every other equation is identical to the linear KF of Block 5. The only addition is that the Jacobian must be re-evaluated at every step, because $\hat{\mathbf{x}}^-$ changes from cycle to cycle. That re-evaluation is what people mean when they say the EKF "re-linearizes" at every step.

![A nonlinear curve h(x) with a tangent line drawn at the operating point x-hat. The slope of the tangent is labeled H.](_media/ekf_linearization.png)

*Fig. 2.1. The EKF replaces $h(x)$ by its tangent line at $\hat{x}^-$. The slope of the tangent is the Jacobian $\mathbf{H}$. As the estimate moves between cycles, the tangent point moves with it; this is why the EKF is non-optimal in general (it ignores higher-order terms) and why filter divergence is a real failure mode if the linearization is bad.*

:::{admonition} Key Concept
:class: key-concept

The EKF reuses the entire Kalman filter machinery; the only change is that $\mathbf{H}$ is recomputed at every step from a Jacobian, and the innovation uses the nonlinear measurement function.
:::

## Extending the State for GPS

To use pseudoranges directly, we have to add the receiver clock bias $b$ as a state. Building on the 4-state filter from Block 5, the 2D tight-coupling state vector becomes

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

The clock bias $b$ has units of meters (range-equivalent receiver timing error). We typically model it as a random walk or a slow Gauss-Markov process — the time constant of receiver-clock drift is much longer than the navigation update rate, so the bias is essentially constant from one fix to the next but drifts measurably over minutes to hours.

Every pseudorange equation includes $b$ in the same way: $\tilde{\rho}_i = \lVert \mathbf{p} - \mathbf{s}^{(i)} \rVert + b + \epsilon_i$. With one extra state and the same predict-update cycle, the EKF estimates $b$ automatically alongside position and velocity. The full 3D version uses a 7-state vector $[p_x, p_y, p_z, v_x, v_y, v_z, b]^{\top}$ — same idea, bigger matrices.

## The Pseudorange Jacobian

The measurement function for satellite $i$ is

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

Differentiate row by row to get the $1\times 5$ Jacobian:

$$
\mathbf{H}_i = \frac{\partial h_i}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}^-}
=
\begin{bmatrix}
\hat{e}_x & \hat{e}_y & 0 & 0 & 1
\end{bmatrix},
$$

where the unit vector from receiver to satellite is

$$
\hat{e}_x = \frac{\hat{p}_x - s_x^{(i)}}{\rho_i},\qquad
\hat{e}_y = \frac{\hat{p}_y - s_y^{(i)}}{\rho_i}.
$$

The structure is clean and operationally meaningful:

- The first two entries are the **unit line-of-sight vector** from the receiver to satellite $i$. They tell the filter how a small move in user position changes the predicted pseudorange.
- The velocity entries are zero. Pseudoranges do not depend instantaneously on velocity, so the Jacobian has no velocity sensitivity.
- The clock-bias entry is exactly 1, because the clock bias adds linearly to every pseudorange.

The 3D version extends the unit-vector block to three components and pads the rest with zeros: $\mathbf{H}_i = [\hat{e}_x, \hat{e}_y, \hat{e}_z, 0, 0, 0, 1]$.

This is the same Jacobian the GPS NLLS solver in Block 6 uses internally; the EKF is just running it inside a recursive predict-update loop instead of inside a batch optimizer.

## The EKF Predict-and-Update Loop

The propagation step is unchanged from the linear KF:

$$
\hat{\mathbf{x}}^- = \mathbf{F}\,\hat{\mathbf{x}}^+,
\qquad
\mathbf{P}^- = \mathbf{F}\,\mathbf{P}^+\,\mathbf{F}^{\top} + \mathbf{Q}.
$$

The update step runs **once per visible satellite**. For satellite $i$:

$$
\nu_i = \tilde{\rho}_i - h_i(\hat{\mathbf{x}}^-) \qquad \text{(nonlinear innovation)}
$$

$$
\mathbf{K}_i = \mathbf{P}^-\,\mathbf{H}_i^{\top}\,\big(\mathbf{H}_i\,\mathbf{P}^-\,\mathbf{H}_i^{\top} + \sigma_\rho^2\big)^{-1}
$$

$$
\hat{\mathbf{x}}^+ = \hat{\mathbf{x}}^- + \mathbf{K}_i\,\nu_i,
\qquad
\mathbf{P}^+ = \mathbf{P}^- - \mathbf{K}_i\,\mathbf{H}_i\,\mathbf{P}^-.
$$

Notice the innovation uses $h_i$ (the full nonlinear function) but the gain and covariance updates use $\mathbf{H}_i$ (the linearized Jacobian). After processing satellite $i$, the posterior $\hat{\mathbf{x}}^+$ becomes the prior for satellite $i+1$, with the Jacobian re-evaluated at the new estimate. Sequential one-satellite-at-a-time processing is much cleaner than stacking all satellites into one matrix update, and it generalizes naturally to mixed-rate sensors (each satellite can update at its own cadence).

::::{admonition} Quick Exercise
:class: quick-exercise

Given a 5-state EKF in 2D with

$$
\hat{\mathbf{x}}^- = [0,\ 0,\ 10,\ -5,\ 20]^{\top}\ \text{m},\quad
\mathbf{P}^- = 100\,\mathbf{I}_5,\quad
\mathbf{s} = [3000,\ 4000]^{\top}\ \text{m},\quad
\tilde{\rho} = 5035\ \text{m},\quad
\sigma_\rho^2 = 100\ \text{m}^2,
$$

walk through one full update step.

:::{admonition} Solution
:class: dropdown

**Linearize.** Compute the predicted range:

$$
\rho = \lVert \hat{\mathbf{p}}^- - \mathbf{s} \rVert = \sqrt{(0 - 3000)^2 + (0 - 4000)^2} = 5000\ \text{m}.
$$

Predicted pseudorange (with the prior clock bias $b^- = 20$ m):

$$
h(\hat{\mathbf{x}}^-) = \rho + b^- = 5020\ \text{m}.
$$

Unit line-of-sight components:

$$
\hat{e}_x = \frac{0 - 3000}{5000} = -0.6, \qquad
\hat{e}_y = \frac{0 - 4000}{5000} = -0.8.
$$

So $\mathbf{H} = [-0.6,\ -0.8,\ 0,\ 0,\ 1]$.

**Innovate.**

$$
\nu = \tilde{\rho} - h(\hat{\mathbf{x}}^-) = 5035 - 5020 = +15\ \text{m}.
$$

**Innovation variance.** With $\mathbf{P}^- = 100\,\mathbf{I}_5$:

$$
\mathbf{H}\,\mathbf{P}^-\,\mathbf{H}^{\top} = 100\,(\hat{e}_x^2 + \hat{e}_y^2 + 0 + 0 + 1) = 100\,(0.36 + 0.64 + 1) = 200.
$$

$$
S = \mathbf{H}\,\mathbf{P}^-\,\mathbf{H}^{\top} + \sigma_\rho^2 = 200 + 100 = 300\ \text{m}^2.
$$

**Gain.**

$$
\mathbf{K} = \mathbf{P}^-\,\mathbf{H}^{\top}/S = \frac{100}{300}\,[-0.6,\ -0.8,\ 0,\ 0,\ 1]^{\top}
= [-0.20,\ -0.267,\ 0,\ 0,\ 0.333]^{\top}.
$$

**Update state.**

$$
\hat{\mathbf{x}}^+ = \hat{\mathbf{x}}^- + \mathbf{K}\,\nu
= [0,\ 0,\ 10,\ -5,\ 20]^{\top} + 15\,[-0.20,\ -0.267,\ 0,\ 0,\ 0.333]^{\top}
= [-3.0,\ -4.0,\ 10,\ -5,\ 25]^{\top}\ \text{m}.
$$

**Which states changed and why?** Position $p_x$ and $p_y$ moved along the negative line-of-sight direction (toward where the satellite is *not*) because the measured range was longer than expected; the filter explains that by moving the receiver away from the satellite. The clock bias also moved up by 5 m, because increasing $b$ would equally explain a longer-than-expected pseudorange. Velocity stayed put — pseudoranges have no velocity content in their Jacobian, so a single pseudorange measurement updates only position and clock bias directly.

This is exactly why we need pseudorange-rate (Doppler) measurements to update velocity directly. Block 9 talks about that pattern in the context of the F-47 capstone.
:::

::::

## EKF / GPS Pseudorange Demo

The **5-state EKF demo** in this block runs the full tight-coupling filter on a 2D scenario: 5-state $[p_x, p_y, v_x, v_y, b]^{\top}$ with constant-velocity kinematics, a random-walk clock-bias model, and four GPS satellites delivering scalar pseudorange measurements at individually-controllable cadences. A sky compass lets you drag satellites around to change the geometry; per-satellite toggles let you turn individual satellites off and watch the filter degrade gracefully — including the operationally critical "fewer than 4 satellites" case where the Block 6 black-box solver would collapse. Each pseudorange goes through one scalar EKF update with a 1×5 Jacobian, and all the standard live-trajectory + per-state error panels follow the same animation pattern as the Block 5 KF demos. A MATLAB implementation is also included at `code/ExtendedKalmanFilter5D.m`.

## Wrap-Up

Loose coupling pre-solves GPS into a position estimate and feeds that to a linear KF. Tight coupling feeds raw pseudoranges into the filter directly, which forces an Extended Kalman Filter to handle the nonlinear range equation. The EKF replaces $h(\mathbf{x})$ with its tangent at the current best estimate, runs the standard KF update with that linearized Jacobian, and re-linearizes at every cycle. Adding the clock bias as a state gives the EKF a self-contained tightly-coupled GPS/INS framework. The Jacobian's first two entries are the unit line-of-sight to the satellite, the velocity entries are zero, and the clock-bias entry is one — that compact structure is the whole reason the EKF works on pseudoranges.

The next block confronts the assumption every filter we have built so far has quietly relied on: that the sensors are well-behaved. What happens when a satellite is spoofed, when the pseudorange grows a steady bias, when a measurement starts to lie? That is fault detection, integrity, and HMI — the topic of Block 8.