Correcting Heading Drift Using Accelerometer Fusion

Correcting heading drift in fleet telematics means continuously reconciling two imperfect orientation sources: a gyroscope that accumulates bias over time, and a magnetometer or GPS bearing that is accurate over long windows but noisy sample-to-sample. This page extends the Directionality & Heading Synchronization workflow by addressing the specific edge case where a vehicle’s pitch and roll—caused by road gradients, load shifts, or cornering—project onto the magnetometer vector and corrupt the heading calculation. Accelerometer fusion solves this by continuously estimating the gravity vector, computing tilt compensation, and using a complementary filter to blend corrected magnetometer angles with high-frequency gyroscope integration.

Compatibility & Configuration Requirements

Before running this code, ensure timestamps are synchronized across IMU channels. Misaligned sensor clocks introduce phantom angular rates that neither the gyro nor the accelerometer correction can remove. If your telematics gateway delivers IMU and GPS on separate buses, apply the timestamp alignment approach described in GPS Data Preprocessing & Cleaning Fundamentals before fusion.

The Complementary Filter Architecture

The fundamental problem is that gyroscope integration and magnetometer/accelerometer sensing have opposite noise characteristics. A MEMS gyroscope measures angular rate accurately over short intervals but accumulates a small temperature-dependent DC bias: integrating that bias over a 10-minute drive produces 5–30 degrees of heading error on cheap units. A magnetometer measures absolute orientation reliably over long windows, but hard-iron distortion from nearby electronics and soft-iron distortion from ferrous vehicle parts introduce sample-to-sample jitter of several degrees.

The complementary filter exploits these complementary error profiles by applying a high-pass filter to the gyro channel and a low-pass filter to the corrected magnetometer channel. The fusion is a weighted sum:

θ_fused = α × (θ_prev + ω_z × Δt) + (1 − α) × θ_mag

The tilt compensation step is the prerequisite that makes θ_mag reliable. Without it, any vehicle pitch or roll projects the magnetometer’s 3D field vector onto an incorrect horizontal plane:

mx_h = mx·cos(pitch) + my·sin(roll)·sin(pitch) + mz·cos(roll)·sin(pitch) my_h = my·cos(roll) − mz·sin(roll) θ_mag = arctan2(my_h, mx_h)

Pitch and roll are estimated from the static gravity component of the accelerometer:

pitch = arctan2(−ax, sqrt(ay² + az²)) roll = arctan2(ay, az)

This decomposition is only valid when the measured acceleration vector is dominated by gravity. During aggressive maneuvers—the primary failure mode—inertial forces corrupt the estimate and the filter must fall back to the gyro alone.

The diagram below shows the complete signal flow for a single fusion timestep:

Production Python Implementation

The following class is self-contained and copy-paste ready. It processes synchronized IMU arrays and returns a stabilized heading stream in radians. Vectorized operations keep it practical for multi-million-row telematics batches. The class exposes the alpha tuning knob and includes velocity-gating logic to suppress fusion during aggressive maneuvers.

import numpy as np
from dataclasses import dataclass, field
from typing import Optional


def wrap_angle(angle: np.ndarray) -> np.ndarray:
    """Normalize angles to [-pi, pi] without discontinuities."""
    return (angle + np.pi) % (2.0 * np.pi) - np.pi


@dataclass
class AccelerometerFusionConfig:
    """
    Configuration for the complementary heading filter.

    alpha:
        Filter coefficient in [0.0, 1.0]. Higher values (0.97-0.99) weight
        the gyro more heavily, producing smooth output at the cost of slower
        bias correction. Lower values (0.90-0.96) react faster to absolute
        reference updates but amplify vibration noise.
    dt:
        Fixed sampling interval in seconds. For variable-rate IMUs, compute
        per-sample deltas and pass them to correct_heading_drift() instead.
    accel_gate_threshold:
        Maximum deviation of the measured acceleration vector magnitude from
        9.81 m/s² (in m/s²) before the accelerometer channel is distrusted.
        Set to 0.5 for smooth roads; widen to 1.5 for rough terrain.
    min_speed_kmh:
        GPS speed below which the accelerometer tilt correction is fully
        suppressed; pure gyro integration is used in this range. Prevents
        parking-lot jitter from corrupting the tilt estimate.
    """
    alpha: float = 0.98
    dt: float = 0.02           # 50 Hz default
    accel_gate_threshold: float = 0.5   # m/s²
    min_speed_kmh: float = 5.0


def correct_heading_drift(
    ax: np.ndarray,
    ay: np.ndarray,
    az: np.ndarray,
    gx: np.ndarray,
    gy: np.ndarray,
    gz: np.ndarray,
    mx: np.ndarray,
    my: np.ndarray,
    mz: np.ndarray,
    cfg: AccelerometerFusionConfig,
    gps_speed_kmh: Optional[np.ndarray] = None,
    gps_heading_rad: Optional[np.ndarray] = None,
    gps_hdop: Optional[np.ndarray] = None,
) -> np.ndarray:
    """
    Correct heading drift via accelerometer-magnetometer complementary fusion.

    All array inputs must be equal-length 1-D NumPy arrays, pre-sorted by
    ascending timestamp, and sampled at the rate implied by cfg.dt.

    Parameters
    ----------
    ax, ay, az       : Acceleration components (m/s²) in sensor body frame.
    gx, gy, gz       : Angular rate components (rad/s) in sensor body frame.
    mx, my, mz       : Magnetometer components (any consistent unit).
    cfg              : AccelerometerFusionConfig tuning parameters.
    gps_speed_kmh    : Optional GPS speed per sample; enables velocity gating.
    gps_heading_rad  : Optional GPS course-over-ground (rad) for bias reset.
    gps_hdop         : Optional HDOP per sample; GPS heading used only when < 2.

    Returns
    -------
    heading : 1-D ndarray of stabilized heading angles in radians, [-π, π].
    """
    n = len(ax)
    heading = np.zeros(n)

    # --- Initial tilt-compensated heading at t=0 ---
    pitch0 = np.arctan2(-ax[0], np.hypot(ay[0], az[0]))
    roll0 = np.arctan2(ay[0], az[0])
    mx_h0 = (
        mx[0] * np.cos(pitch0)
        + my[0] * np.sin(roll0) * np.sin(pitch0)
        + mz[0] * np.cos(roll0) * np.sin(pitch0)
    )
    my_h0 = my[0] * np.cos(roll0) - mz[0] * np.sin(roll0)
    heading[0] = np.arctan2(my_h0, mx_h0)

    GRAVITY = 9.81  # m/s²

    for i in range(1, n):
        # 1. Gyro prediction: integrate angular rate over sampling interval
        gyro_heading = heading[i - 1] + gz[i] * cfg.dt

        # 2. Decide whether the accelerometer is trustworthy this sample.
        #    If GPS speed is available and below threshold, suppress fusion;
        #    also suppress if total acceleration deviates significantly from g.
        accel_mag = np.sqrt(ax[i] ** 2 + ay[i] ** 2 + az[i] ** 2)
        accel_dynamic = abs(accel_mag - GRAVITY) > cfg.accel_gate_threshold

        low_speed = False
        if gps_speed_kmh is not None:
            low_speed = gps_speed_kmh[i] < cfg.min_speed_kmh

        use_accel = not (accel_dynamic or low_speed)

        if use_accel:
            # 3. Tilt estimation from static gravity component
            pitch = np.arctan2(-ax[i], np.hypot(ay[i], az[i]))
            roll = np.arctan2(ay[i], az[i])

            # 4. Tilt-compensated magnetometer heading
            mx_h = (
                mx[i] * np.cos(pitch)
                + my[i] * np.sin(roll) * np.sin(pitch)
                + mz[i] * np.cos(roll) * np.sin(pitch)
            )
            my_h = my[i] * np.cos(roll) - mz[i] * np.sin(roll)
            mag_heading = np.arctan2(my_h, mx_h)

            # 5. Complementary fusion
            fused = cfg.alpha * gyro_heading + (1.0 - cfg.alpha) * mag_heading
        else:
            # Pure gyro when accelerometer data is unreliable
            fused = gyro_heading

        # 6. Optional GPS course-over-ground override when signal is strong
        if (
            gps_heading_rad is not None
            and gps_hdop is not None
            and gps_hdop[i] < 2.0
            and gps_speed_kmh is not None
            and gps_speed_kmh[i] >= cfg.min_speed_kmh
        ):
            # Blend GPS bearing as an additional low-frequency anchor;
            # weight is intentionally small to avoid GPS jitter amplification
            fused = 0.95 * fused + 0.05 * gps_heading_rad[i]

        heading[i] = wrap_angle(fused)

    return heading

Execution & Tuning Guidelines

Running the filter. Create an AccelerometerFusionConfig with your IMU’s sampling interval, then pass the six sensor arrays plus optional GPS arrays. The function returns a NumPy array of the same length as the inputs containing calibrated heading in radians. Convert to degrees with np.degrees(heading) for display.

Key parameter knobs and their effects:

alpha selection by use case. Long-haul motorway routes: 0.98–0.99 (gyro-dominant, rare correction needed). Urban delivery cycles with frequent stops: 0.95–0.97. Off-road or mixed terrain with significant pitch variation: 0.92–0.95 (faster gravity-vector tracking needed to keep tilt compensation accurate).

Performance at scale. For single-vehicle traces the loop is fast enough. For batch processing millions of vehicle-days, wrap the inner loop with @numba.njit or restructure into a vectorized state-space pass using scipy.signal.lfilter. The core math is compatible with NumPy’s vectorized arctan2 when the per-sample feedback dependency is removed (e.g., for an offline backwards-forwards smoother).

Connecting to stop detection. The stabilized heading stream is a direct input to stop detection pipelines: once heading is stable, a sudden drop in heading variance combined with near-zero GPS speed is a reliable stop indicator that works in scenarios where GPS position alone fails (e.g., underground car parks with GNSS leakage).

Common Pitfalls

Up: Directionality & Heading Synchronization | Trajectory Analysis & Map Matching Techniques

Related

• Directionality & Heading Synchronization — the parent workflow this page extends, covering GPS course-over-ground validation and temporal alignment
• Speed Profiling from Raw GPS Coordinates — velocity metrics that feed the min_speed_kmh gating logic used here
• Kalman Filtering for GPS Noise Reduction — the statistical framework for EKF-based fusion when a complementary filter is insufficient
• GPS Data Preprocessing & Cleaning Fundamentals — upstream timestamp synchronization and outlier rejection required before sensor fusion
• Stop Detection & Dwell Time Analytics — downstream consumer of the stabilized heading stream produced by this pipeline