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 directly 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 that 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. The result turns an \(O(t^3)\) position-error growth into an error that grows roughly with distance travelled, at typically 0.2 to 1 percent of path length for a well-tuned foot-mounted system.

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 through empirical models. The Weinberg model, \(l_i = K\,(a_{\max}-a_{\min})^{1/4}\), and the linear frequency model \(l_i = a + b f_{\text{step}}\) are both common, with the gain calibrated per user. Because the model is coarse, 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 the specific-force measurement unchanged, so heading drift is fundamentally unobservable from a zero-velocity update alone. Two partial remedies are standard. The zero-angular-rate update (ZARU) exploits stance instants when the whole body is still, not just the foot, to observe gyroscope bias, which slows yaw drift. Heuristic drift reduction (HDR) 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 solution is fusing an absolute heading or position reference, whether a magnetometer where the field is clean, a map constraint, or an RF fix, which is why pedestrian dead reckoning is almost never deployed alone. Estimating and propagating the heading uncertainty honestly (Chapter 4) is what lets a downstream fusion stage weight the dead-reckoned track correctly 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.