Zero-velocity updates and pedestrian dead reckoning

"You told me to integrate. So I integrated. The building grew a third floor that does not exist, and by the lobby I was standing in the car park across the street."

An Unbounded AI Agent

This section builds on inertial navigation and its drift (Section 24.4) and on the stance-phase detection developed for gait (Chapter 23). The correction machinery is an error-state Kalman filter, so if the extended Kalman filter feels shaky, revisit the Kalman family in Chapter 9. We assume orientation is already tracked (Sections 24.1 to 24.3), so specific force can be rotated into the navigation frame and gravity subtracted.

The zero-velocity update as a pseudo-measurement

Mount an IMU on the foot, at the instep or heel. Strap-down integration propagates the standard mechanization: rotate body-frame specific force \(\mathbf{f}^b\) into the navigation frame, subtract gravity, integrate once for velocity and twice for position. Left alone, this drifts. But during the stance phase, from foot-flat to toe-off, the foot velocity is genuinely zero for roughly 0.2 to 0.5 seconds each step. When a stance instant is detected, we assert a measurement

where \(\hat{\mathbf{v}}_k\) is the filter's current velocity estimate and \(\mathbf{R}\) encodes how close to zero we believe the foot really is. The measurement residual \(\mathbf{z}_k - \mathbf{0}\) is simply the drifted velocity, and the Kalman gain distributes that correction back not only onto velocity but, through the error-state covariance, onto the accelerometer and gyroscope biases and onto orientation. This last part is the quiet magic: ZUPT does not merely zero the velocity, it observes and trims the biases that caused the drift in the first place, so the position error between steps shrinks too. ZUPT turns unbounded, accelerating drift into a bill charged per meter walked, not per second elapsed: a well-tuned foot-mounted system holds position error to roughly 0.2 to 1 percent of distance travelled, down from an \(O(t^3)\) blow-up with no correction at all.

Detecting the zero-velocity interval

Everything hinges on correctly deciding when the foot is still. Declare stance too often and you inject false zeros during the swing, corrupting the estimate; declare it too rarely and drift creeps back. The classic family of detectors thresholds a windowed statistic. The acceleration-magnitude detector flags stance when \(\lVert \mathbf{f}^b \rVert\) sits near \(g\); the angular-rate energy detector flags it when gyroscope energy is low; the widely used SHOE detector (stance hypothesis optimal estimation) is a generalized likelihood-ratio test that fuses both, comparing accelerometer and gyroscope energy in a short window against thresholds. Because these thresholds are gait- and speed-dependent, a fixed setting that is right for a walk fires late during a run and chatters during a slow shuffle, which is why learned and adaptive stance detectors are an active research thread.

The snippet below implements a SHOE-style detector and applies the resulting zero-velocity flag as a hard velocity reset, the pedagogical minimal version of a ZUPT. A production system feeds the same flag into an error-state EKF instead of hard-resetting.

import numpy as np

def shoe_detector(acc, gyr, fs, g=9.81,
                  sigma_a=0.5, sigma_w=0.1, win=15, gamma=3.5e5):
    # acc: (N,3) m/s^2, gyr: (N,3) rad/s from a foot-mounted IMU.
    N = len(acc)
    zv = np.zeros(N, dtype=bool)
    half = win // 2
    for k in range(half, N - half):
        sl = slice(k - half, k + half + 1)
        a_win = acc[sl]
        w_win = gyr[sl]
        a_mean = a_win.mean(axis=0)
        ahat = g * a_mean / np.linalg.norm(a_mean)   # expected gravity direction
        # Generalized likelihood ratio: accel deviation from g + gyro energy.
        T = (np.sum((a_win - ahat) ** 2) / sigma_a**2 +
             np.sum(w_win ** 2) / sigma_w**2) / win
        zv[sl] = zv[sl] | (T < gamma)                # below threshold => stance
    return zv

def zupt_track(acc_nav, zv, fs):
    # acc_nav: (N,3) gravity-removed acceleration already in the nav frame.
    dt = 1.0 / fs
    vel = np.zeros_like(acc_nav)
    pos = np.zeros_like(acc_nav)
    for k in range(1, len(acc_nav)):
        vel[k] = vel[k-1] + acc_nav[k] * dt
        if zv[k]:
            vel[k] = 0.0                             # zero-velocity update
        pos[k] = pos[k-1] + vel[k] * dt
    return pos, vel

Pedestrian dead reckoning when the foot is out of reach

Foot-mounted ZUPT is superb but impractical for a consumer phone or a wristband, where there is no crisp stance signal. The alternative, pedestrian dead reckoning in the step-and-heading form, discards continuous integration entirely. It detects each step as an event (Chapter 23), estimates a scalar step length \(l_i\), reads a heading \(\psi_i\), and advances the position one stride at a time:

Step length is not measured directly; it is inferred from step frequency and acceleration variance, since no single foot-strike profile reveals distance on its own. The Weinberg model, \(l_i = K\,(a_{\max}-a_{\min})^{1/4}\), uses the spread between peak and trough vertical acceleration as a proxy for stride vigor: the fourth root compresses that spread's dynamic range so the fit stays roughly linear from a walk to a jog, and breaks down outside it, since a shuffle barely varies acceleration while a sprint saturates it. The linear frequency model \(l_i = a + b f_{\text{step}}\) instead ties stride length to cadence and needs \(b\) calibrated per user; uncalibrated, it can misjudge stride by 10 to 20 percent, which is why deployed systems open with a short calibration walk. Because the model stays coarse even after calibration, step-and-heading drift is dominated by two coupled errors: step-length scale error, which stretches or shrinks the whole trajectory, and heading error, which curls it. Heading is the more dangerous of the two, since a slow gyroscope yaw drift, unobservable without an external reference, rotates the entire subsequent path.

Heading, and the errors ZUPT cannot fix

ZUPT makes velocity error and, through it, tilt and accelerometer bias observable, but it says nothing about yaw: rotation about the gravity vector leaves specific force unchanged, so heading drift is fundamentally unobservable from a zero-velocity update alone. Two partial remedies help. ZARU, the zero-angular-rate update, exploits instants when the whole body, not just the foot, is still, observing gyroscope bias and slowing yaw drift. Heuristic drift reduction assumes indoor walking follows dominant building headings (corridors meet at right angles) and gently snaps heading toward the nearest cardinal-ish direction. Both are crutches; the durable fix is fusing an absolute reference, a clean-field magnetometer, a map constraint, or an RF fix, which is why pedestrian dead reckoning is almost never deployed alone. Propagating heading uncertainty rigorously (Chapter 4) is what lets a downstream fusion stage weight the track against those references.

What's Next

In Section 24.6, we replace the hand-built detector-plus-filter pipeline with a learned inertial odometry network in the IONet style, which regresses displacement directly from raw IMU windows and sidesteps the brittle stance-threshold that this section leaned on so heavily.