Edge Geospatial IoT Gateway Processing

Kalman filtering noisy GPS fixes on a gateway

A constant-velocity Kalman filter over a four-state position/velocity model is the standard answer to GNSS jitter, and it is small enough to run per-fix on a Cortex-A gateway without noticing. What is not small is the tuning: two covariance numbers that decide whether the filter tracks a manoeuvre or smooths it away, and which cannot be derived from first principles because they encode assumptions about how the asset moves. This guide implements the filter, then spends most of its length on those two numbers. It sits under trajectory simplification and smoothing in the Local Spatial Processing Patterns guide.

The model, and why this one

The filter maintains a state of four numbers — easting, northing, and the velocity in each — and a 4×4 covariance describing how confident it is. Each cycle it predicts the state forward by dt using a constant-velocity model, inflates the covariance by the process noise, then corrects toward the measurement in proportion to how the covariances compare.

Constant velocity is the right model for almost every field asset. Constant acceleration adds two states and helps only where acceleration is sustained and smooth, which describes aircraft and almost nothing that drives. A coordinated-turn model helps where heading changes dominate, at the cost of a nonlinearity that turns the filter into an EKF and the tuning problem into a research project. For vehicles, containers, plant and people, constant velocity with well-chosen process noise is the honest choice.

Run it in projected metres, on the frame established in coordinate reference systems at the edge. A filter tuned in degrees encodes the latitude it was tuned at into its own noise model.

One filter cycle: predict, inflate, correct A cycle over three panels. Predict moves the state forward by dt under constant velocity, producing a prior estimate whose uncertainty ellipse has grown along the direction of travel. Inflate adds the process noise, growing the ellipse further to reflect unmodelled acceleration. Correct combines the prior with the measurement, whose own uncertainty is a circle of radius equal to the reported accuracy; the posterior sits between them, closer to whichever was more confident, with a smaller ellipse than either. The Kalman gain is just "which of these two do I believe more?" 1 · predict state moves by v·dt, ellipse elongates 2 · inflate by Q process noise: what the model does not know 3 · correct with R posterior is tighter than either input Q says how much the asset can surprise the model; R says how much the receiver lies. Their ratio is the entire behaviour of the filter.
Every tuning question reduces to the third panel: whether the amber ellipse or the blue circle should dominate the result.

The implementation 

# gps_kalman.py — constant-velocity Kalman filter over projected coordinates.
# State: [east, north, v_east, v_north]. Fixed 4×4 arithmetic written out
# longhand: no numpy import, no allocation per cycle, deterministic timing.
# Threading: one instance per tracked asset, owned by one task.
import math


class TrackFilter:
    __slots__ = ("x", "P", "q", "primed", "t")

    def __init__(self, process_noise: float = 0.35):
        # q is the acceleration spectral density in m²/s³ — see the tuning table.
        self.q = process_noise
        self.x = [0.0, 0.0, 0.0, 0.0]
        # Covariance stored row-major as 16 floats; only the blocks we touch
        # are updated, and the matrix stays symmetric by construction.
        self.P = [0.0] * 16
        self.primed = False
        self.t = 0.0

    def update(self, east: float, north: float, accuracy_m: float, t: float):
        """One measurement. `accuracy_m` is the receiver's own 1-sigma estimate
        (from HDOP × UERE, or the receiver's accuracy field). Returns the
        filtered position."""
        if not self.primed:
            self.x = [east, north, 0.0, 0.0]
            r = accuracy_m * accuracy_m
            self.P = [r, 0, 0, 0,  0, r, 0, 0,  0, 0, 25.0, 0,  0, 0, 0, 25.0]
            self.primed, self.t = True, t
            return east, north

        dt = t - self.t
        if dt <= 0.0 or dt > 60.0:
            self.primed = False                    # gap or clock step: re-prime
            return self.update(east, north, accuracy_m, t)
        self.t = t

        # --- predict: x = F x, P = F P Fᵀ + Q -----------------------------
        self.x[0] += self.x[2] * dt
        self.x[1] += self.x[3] * dt

        P = self.P
        # F P Fᵀ for a constant-velocity model, written out per block.
        for i in (0, 1):
            p, v = i, i + 2
            pp, pv, vv = P[p * 4 + p], P[p * 4 + v], P[v * 4 + v]
            P[p * 4 + p] = pp + 2 * dt * pv + dt * dt * vv
            P[p * 4 + v] = P[v * 4 + p] = pv + dt * vv
            # Q for continuous white-noise acceleration.
            P[p * 4 + p] += self.q * dt ** 3 / 3.0
            P[p * 4 + v] += self.q * dt ** 2 / 2.0
            P[v * 4 + p] = P[p * 4 + v]
            P[v * 4 + v] = vv + self.q * dt

        # --- correct: one axis at a time (measurements are independent) ----
        r = accuracy_m * accuracy_m
        for i, z in ((0, east), (1, north)):
            p, v = i, i + 2
            s = P[p * 4 + p] + r                   # innovation covariance
            k_p = P[p * 4 + p] / s                 # Kalman gain, position
            k_v = P[v * 4 + p] / s                 # Kalman gain, velocity
            residual = z - self.x[p]
            self.x[p] += k_p * residual
            self.x[v] += k_v * residual
            pp, pv, vv = P[p * 4 + p], P[p * 4 + v], P[v * 4 + v]
            P[p * 4 + p] = (1 - k_p) * pp
            P[p * 4 + v] = P[v * 4 + p] = (1 - k_p) * pv
            P[v * 4 + v] = vv - k_v * pv

        return self.x[0], self.x[1]

    def speed(self) -> float:
        return math.hypot(self.x[2], self.x[3])

    def position_sigma(self) -> float:
        return math.sqrt(max(self.P[0], self.P[5]))

Writing the matrix arithmetic out longhand rather than importing numpy is deliberate on this class of device. A 4×4 numpy operation is dominated by call overhead, not by the arithmetic, and importing numpy costs 25 MB of resident memory that a gateway running one filter per asset cannot justify. Longhand runs in about 6 µs per fix and allocates nothing.

Tuning: the two numbers that matter

R, the measurement noise, is the easy one because the receiver reports it. Use the accuracy estimate directly, squared. If the receiver reports only HDOP, multiply it by the user-equivalent range error — typically 4–6 m for consumer GNSS without correction — and use that. Feeding a constant R when the receiver’s own accuracy is varying is the single most common tuning error, because it makes the filter equally trusting of a fix taken with nine satellites and one taken with four.

Q, the process noise, encodes how much the asset can accelerate without the model expecting it. It cannot be measured from the fixes; it is a statement about the asset.

Asset class Typical q (m²/s³) Behaviour
Container, parked plant 0.01 Heavily smoothed; a genuine move takes several fixes to register
Person walking 0.1 Follows direction changes at a walking pace; noise mostly removed
Delivery vehicle, urban 0.35 Tracks stop-start driving; some jitter survives
Emergency vehicle, off-road plant 2.0 Follows aggressive manoeuvres; smoothing is light
Unknown / mixed fleet 0.35 Safe default; err toward tracking rather than smoothing

The failure modes are symmetric and both look like a broken filter. Too small a q produces a lag: the estimate trails the asset through every turn, and the residual grows every time the asset does something. Too large a q produces no smoothing at all — the filter follows the measurement, jitter included, because it believes the asset could have moved that way.

The same turn filtered at three process-noise settings A vehicle taking a right-angle turn, plotted three ways against the raw noisy fixes. With q of 0.01 the filtered path cuts the corner by 14 metres and takes six fixes to rejoin the true path. With q of 0.35 it follows the corner within 2 metres while removing most of the jitter. With q of 5 it reproduces the raw track almost exactly, jitter included, providing no smoothing at all. One turn, three values of q q = 0.01 — corner cut by 14 m q = 0.35 — within 2 m, jitter gone q = 5 — follows the noise Dashed grey is the raw fix sequence in all three panels.
Neither extreme announces itself. The left panel looks smooth and is wrong; the right panel looks responsive and is unfiltered.

Constraint validation

Constraint Expected impact Mitigation built into the code
RAM A numpy-based filter costs 25 MB before the first fix Longhand 4×4 arithmetic; the whole filter is 20 floats per tracked asset
CPU Per-fix cost multiplies by asset count ~6 µs per update on a Cortex-A53; a hundred assets at 1 Hz is 0.06% of a core
Latency The filter sits before the decision stage Causal and constant-time; no buffering, no lookahead
Numerical stability Covariance can lose symmetry and go negative over long runs Symmetric assignment on every off-diagonal write; per-axis scalar update avoids a matrix inverse entirely
Power Duty-cycled nodes produce long gaps 60 s gap guard re-primes instead of extrapolating

Gotchas and edge cases

  • Re-prime on a teleport, not just on a gap. A receiver that reacquires after an outage can jump hundreds of metres legitimately. If the residual exceeds several times the innovation covariance, the model is wrong about reality — reset rather than dragging the estimate across the gap over the next twenty fixes.
  • A stationary asset makes velocity drift. With no real motion, the velocity states random-walk under process noise, and a filter left running overnight on a parked vehicle can report a slow phantom crawl. Clamp velocity to zero when the position variance stops shrinking and the speed stays under a threshold.
  • The filter’s output is not a measurement. Downstream consumers computing distance travelled from filtered positions get a different — usually smaller and more accurate — answer than from raw fixes. Be explicit about which one the reported total uses, because the two will be compared eventually.
  • Do not filter, then feed the filtered position back as a measurement. It happens accidentally when the smoothed track is spooled and later re-processed. Each pass shrinks the covariance further and the filter becomes over-confident, eventually ignoring genuine motion.
  • Per-axis correction assumes independent errors. GNSS position errors are correlated between axes under poor geometry. The scalar update is still an excellent approximation and it avoids a matrix inversion on hardware without an FPU — but if HDOP is extreme, expect the filter to be slightly over-confident.
Diagnosing a filter from its residual Four residual signatures and what each means. A residual whose mean is near zero and whose magnitude tracks the reported accuracy indicates a correctly tuned filter. A residual that spikes on every turn and settles between them indicates process noise set too low. A residual as large as the raw noise indicates process noise set too high, so the filter is following the measurement. A residual with a steady non-zero mean indicates a systematic offset such as an uncorrected datum shift, which no amount of tuning fixes. The residual is the filter's own error report — four signatures worth knowing zero mean, magnitude ≈ reported accuracy correctly tuned — leave it alone spikes on every turn, settles between q too low — the filter lags manoeuvres as large as the raw noise q too high — no smoothing is happening steady non-zero mean a systematic offset — a datum shift, not a tuning problem
Only the bottom row is unfixable by tuning, and it is the one most often met with more tuning.

Integration## Integration

Place the filter after the quality gate and before every decision, exactly as the parent guide’s pipeline shows. Export position_sigma() alongside the position: it is the filter’s own statement of how much it should be trusted, and it is what lets a geofence test widen its band when confidence is poor rather than producing a crossing it cannot support. That coupling — uncertainty into the threshold logic — is what turns two independently sensible components into one that behaves correctly at the moment the sky view degrades.

Related in Trajectory Simplification & Smoothing