July 2026
In part 1, we built a Kalman filter that fuses GPS position with GPS Doppler speed and course to smooth out a wiggly raw trajectory. It worked. But we ended on three open items:
This post works through all three, plus two lessons that only showed up once we started testing the filter against real-world recordings instead of a single clean track.
Our conclusion in part 1 floated using heading instead of course, with sensor fusion, as a future improvement. Before doing the sensor fusion part properly, we tried the naive version first: swap locationTrueHeading in for locationCourse directly, no fusion, see what happens.
It made things measurably worse. Checking a recording with both fields present:
$$ \overline{|\psi_{heading}-\psi_{course}|} \approx 36°,\quad \sigma \approx 29°,\ \text{max} > 120° $$
The reason is that course and heading measure different things. Course is derived from consecutive GPS fixes — by construction, it is the direction of travel. Heading is derived from the magnetometer — it's the direction the phone is physically pointing, which has nothing to do with the direction of travel unless the phone happens to be mounted in a fixed, known orientation relative to the vehicle. Swapping one for the other without fusing them first injects a velocity vector rotated by tens of degrees off the true heading, at every step. This is a good demonstration of why the part 1 conclusion specifically said "with sensor fusion" and not just "heading."
As a lighter-weight middle ground — short of a full INS-style fusion — we found the offset between the two isn't fixed (a phone gets picked up, reoriented, put down differently), so a single calibration constant doesn't hold across a whole recording. Instead we track it with a slow-adapting circular running average, using the standard trick for averaging angles (accumulating $\sin$ and $\cos$ separately to avoid the wraparound problem at 0°/360°):
$$ \begin{align*} \bar s_k &= \alpha \bar s_{k-1} + (1-\alpha)\sin(\psi_{course,k}-\psi_{heading,k})\ \bar c_k &= \alpha \bar c_{k-1} + (1-\alpha)\cos(\psi_{course,k}-\psi_{heading,k})\ \Delta\psi_k &= \operatorname{atan2}(\bar s_k, \bar c_k) \end{align*}\tag{9} $$
updated only when course itself is confident. When course drops out or fails its accuracy check, we fall back to $\psi_{heading} + \Delta\psi$ instead of holding the last known course stale indefinitely. It's a patch, not a fusion — a proper solution would integrate gyro rate to track heading continuously and use course fixes to correct drift, the way an INS/GPS filter would. That's still on the list.
Equation (7)'s $\bm Q_k$ was computed once, from about a minute of stationary sampling, and applied identically every iteration regardless of how much time had actually passed since the last update. That's fine if your sample rate is perfectly uniform. Phone GPS isn't — the gap between fixes can be anywhere from a fraction of a second to several seconds depending on signal conditions.
A fixed $\bm Q$ applies the same absolute dose of process noise to a 0.1-second gap as to a 5-second gap. That's wrong in both directions: over a short gap, barely anything can have changed, so the filter distrusts a still-good prediction more than it should; over a long gap, a lot could have changed, so the filter clings to a stale prediction longer than it should.
The standard fix is the discretized white noise acceleration model — treat the unmodeled acceleration as continuous white noise of power spectral density $q$, and integrate its effect on position and velocity over the actual elapsed time $\Delta t$:
$$ \boldsymbol{Q_k} = q\begin{pmatrix} \Delta t^3/3 & 0 & \Delta t^2/2 & 0 \\ 0 & \Delta t^3/3 & 0 & \Delta t^2/2 \\ \Delta t^2/2 & 0 & \Delta t & 0 \\ 0 & \Delta t^2/2 & 0 & \Delta t \end{pmatrix} $$
where $q = \sigma_s^2$ from equation (7). Velocity variance grows linearly with $\Delta t$ since it's a single integral of the noise; position variance grows with $\Delta t^3$ since it's a double integral; and the $\Delta t^2/2$ off-diagonal terms capture the fact that position and velocity errors from the same unmodeled acceleration are correlated, not independent — something the purely diagonal $\bm Q_k$ in equation (7) missed entirely.