Inertial Odometry: IMU to 3D Tracking

Inertial odometry is the art of estimating where you are using only motion sensors and no external infrastructure. Just accelerometers and gyroscopes, and a lot of math to fight the inevitable drift. This post walks through building an inertial odometry system from first principles using a phone's IMU, covering the sensor model, double integration, quaternion orientation, drift correction techniques, and the Madgwick fusion filter. The Taken 2 Problem In Taken 2, Bryan Mills is kidnapped, blindfolded, and thrown in the trunk of a car. He can't see anything. But he can feel the accelerations, the turns, the stops. He counts seconds between turns, estimates speeds from how hard he's thrown around, and mentally reconstructs the route.

This is inertial odometry. Estimating trajectory using only what you feel: linear acceleration and angular velocity. No external references. The sensors are your inner ear — or in our case, a phone's IMU. What Does an IMU Actually Measure? An Inertial Measurement Unit (IMU) contains two sensors: Accelerometer: measures specific force along three axes (X, Y, Z) in m/s². This is not pure acceleration. When the phone sits still on a table, it reads approximately $[0, 0, 9.81]$ — the reaction force to gravity. The accelerometer cannot distinguish between gravity and real acceleration. This ambiguity is the central challenge of inertial odometry.

Fig 1: Raw IMU data at rest. The accelerometer reads gravity along Z. The norm is close to 9.81.

Gyroscope: measures angular velocity around three axes in rad/s. When the phone is still, it reads approximately $[0, 0, 0]$. When rotating, it tells you how fast and around which axis.

Fig 2: The gyroscope reads near-zero, but fluctuates a lot (i.e., has noise).

At rest, the readings aren't perfectly constant — they fluctuate around a mean value. This fluctuation is noise, and the mean offset from the expected value is bias. Characterizing the Sensor: Bias and Noise

Before tracking anything, we need to know how wrong our sensors are. We do this by collecting data while the phone is perfectly still and analyzing the statistics.

The same is done for the gyroscope (where the expected reading at rest is $[0, 0, 0]$). This gives us a calibration file:

Fig 3: Accelerometer and Gyro at rest: the mean is our bias estimate, the spread is sensor noise.

The bias gets subtracted from every future reading. The std tells us the noise floor — any reading smaller than this is indistinguishable from noise. Even a tiny uncorrected bias of 0.01 m/s² in the accelerometer, when double-integrated, produces 0.5 meters of position drift in just 10 seconds. Calibration isn't optional. From Acceleration to Position: Double Integration With calibrated sensors, the simplest tracking pipeline is: 1. Subtract bias from accelerometer 2. Subtract gravity (assuming phone is flat: gravity is along Z) 3. Integrate acceleration → velocity 4. Integrate velocity → position

Fig 4: The double integration pipeline, assuming the phone is flat on a surface. Simple, but fragile. This works surprisingly well for short motions on a flat table — slide the phone left, the visualization tracks it left. But it breaks down quickly: Problem 1: Bias residuals integrate to drift. Even after calibration, a residual bias of 0.005 m/s² (half the noise floor) produces:

velocity after 10s:  0.005 × 10  = 0.05 m/s
position after 10s:  ½ × 0.005 × 10² = 0.25 m
position after 60s:  ½ × 0.005 × 60² = 9.0 m

Error grows as t². Double integration is an error amplifier. Problem 2: The flat phone assumption. We subtracted gravity along sensor Z, but this only works if the phone is perfectly flat. Tilt it 5° and gravity leaks into X/Y:

gravity_leak_xy = 9.81 × sin(5°) ≈ 0.86 m/s²

That's 100x larger than the bias residual. After 10 seconds, it would produce ~43 meters of drift. The flat phone assumption is not just limiting — it's catastrophically wrong for any real use. Adding Rotation: Gyroscope and Quaternions To handle arbitrary phone orientation, we need to know which way the phone is pointing. The gyroscope gives us angular velocity, and by integrating it, we can track orientation. Quaternions in 30 Seconds We represent orientation as a quaternion $q = [w, x, y, z]$ — a 4-component mathematical object that encodes a 3D rotation without the singularities (gimbal lock) that plague Euler angles. Key operations:

Right-multiplying ($q × dq$) applies the rotation in the body (sensor) frame, which is correct since the gyroscope measures angular velocity in its own frame. Rotation-Aware Gravity Removal With a tracked quaternion, we can finally remove gravity correctly at any orientation:

# 1. Bias-correct the accelerometer (in sensor frame)
accel_corrected = accel_raw - accel_bias

# 2. Rotate from sensor frame to world frame using the quaternion
accel_world = rotate_vector(accel_corrected, q)

# 3. In world frame, gravity is ALWAYS [0, 0, g] — subtract it cleanly
accel_linear = accel_world - np.array([0, 0, gravity])

Fig 5: The quaternion rotates the sensor reading into the world frame, where gravity subtraction is trivial. This is the key insight: the gyroscope solves the gravity problem. No matter how the phone is oriented, we can always isolate the linear acceleration. The Gravity Leakage Problem But the quaternion is only as good as the gyroscope that drives it. Gyro bias, even after calibration, accumulates into orientation error:

After 30s: error ≈ 0.001 rad/s × 30s = 0.03 rad ≈ 1.7°
gravity_leak = 9.81 × sin(1.7°) ≈ 0.29 m/s²
position_drift in 10s = ½ × 0.29 × 100 = 14.5 meters

Fig 6: With the Gyro-only orientation, the angles drift because there's nothing to anchor them. The gyroscope tells us how the phone is changing orientation, but it has no sense of absolute "down". Small errors in "down" cause gravity to leak into the horizontal plane, producing phantom acceleration that integrates into runaway position drift. This is the fundamental limitation of gyro-only orientation tracking. Taming the Drift: Practical Corrections Pure double integration with gyro-only orientation drifts hopelessly. We can't eliminate drift entirely with IMU alone, but several techniques make it manageable. Knowing When You're Still: Zero Velocity Update (ZUPT) The single most effective correction. The idea: if the phone isn't moving, velocity must be zero. Detect stationary periods and reset velocity.

All three conditions must be true. Low variance alone isn't enough — constant gravity leak has low variance but non-zero magnitude. Low accel alone isn't enough — the phone could be rotating. We need the conjunction of all three. When stationary is detected, velocity is aggressively decayed:

if is_stationary:
    if stationary_count > 10:   # stationary for >0.1s
        velocity *= 0.3         # aggressive kill
    else:
        velocity *= 0.7         # gentle decay (might start moving again)

A subtle trap: it's tempting to also require low velocity for ZUPT (surely a stationary phone has low velocity?). But this creates a deadlock — phantom velocity from drift prevents ZUPT from triggering, and ZUPT is the mechanism needed to kill that phantom velocity. ZUPT must be allowed to fire regardless of current velocity. Sanity-Checking Acceleration: The Gravity Magnitude Check A clever heuristic based on a physical constraint: when the phone is not accelerating, the accelerometer magnitude must equal $g$. When it is accelerating, the magnitude deviates from $g$.

This elegantly suppresses phantom acceleration from quaternion drift during stationary periods (when $|accel| ≈ g$), while letting real acceleration through during motion (when $|accel|$ deviates significantly from $g$). Safety Nets: Adaptive Drag and Velocity Clamping Even with ZUPT and gravity check, some phantom velocity leaks through during motion windows. Two last-resort mechanisms:

The adaptive drag is the more interesting one: it uses the gravity check as a real-time indicator. When $gravity\_scale < 0.5$, the phone is probably not accelerating, so any current velocity is likely phantom — apply strong drag. When $gravity\_scale$ is high, the phone is genuinely accelerating — preserve the velocity with gentle drag. These are all symptomatic treatments. They manage the consequences of orientation drift but cannot fix the root cause: the quaternion slowly rotating away from truth with no way to correct itself. Complementary Filters and the Madgwick Filter The Problem: Gyro Drift Has No Anchor The fundamental limitation of above steps is: the gyroscope integration is open-loop. It tracks changes in orientation but has no reference for absolute orientation. Over time, the quaternion drifts, gravity leaks into XY, and position runs away. None of our fixes above can solve this. ZUPT only works when stationary. The gravity check only suppresses phantom acceleration, it doesn't fix the quaternion. We need a way to correct the orientation itself. Two Sensors, Complementary Errors: Translator and Filters The Gyroscope and Accelerometer speak two different languages.

• Gyroscope: Speaks in "Degrees per second" (Angular Speed).
• Accelerometer: Speaks in "Meters per second squared" (Force).

We can't add speed to force. The trigonometry is the translator. It converts the accelerometer's force readings into an Angle, which is the same language the gyroscope uses once its data is integrated. Here's the key insight: the accelerometer — the same sensor we use for position — also tells us about orientation. When the phone isn't accelerating, the accelerometer reads pure gravity. From the gravity direction in sensor frame, we can compute which way is "down":

roll  = atan2(a_y, a_z)
pitch = atan2(-a_x, sqrt(a_y² + a_z²))

But this only works when stationary. During motion, the accelerometer reads gravity + linear acceleration, and the "down" estimate is wrong.

If we only used the gyroscope, our orientation would slowly drift away due to mathematical "noise" accumulating over time. If we only used the accelerometer, our orientation would jitter wildly every time we took a step or hit a bump. We solve this by looking at their "cleanest" frequency ranges:

The two sensors have complementary error characteristics:

This is where the frequency insight becomes the hero of our story. We have two ways to calculate our current orientation:

• The Fast Path (Gyro): Start with the previous angle and add the current rotation speed. It's smooth and handles quick movements perfectly, but it slowly drifts away from the truth.
• The Steady Path (Accel): Use the trig above to find "down." It’s shaky and vibrates during movement, but it never forgets where the ground is.

The Complementary Filter is the weighted bridge between them. It trusts the High-Frequency signals (the quick twists) from the Gyro and the Low-Frequency signals (the long-term gravity) from the Accel.

In code, this "frequency blending" looks surprisingly simple. We give the Gyroscope most of the power (e.g., 98%) and let the Accelerometer "nudge" the result back to center (2%):

# The Complementary Filter
# alpha is usually 0.95 to 0.98
angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * (accel_angle)

By doing this, the high-frequency "noise" from your walking is mostly ignored (because it only has a 2% impact), while the low-frequency "drift" from the gyro is constantly corrected by the 2% anchor of gravity.

At this specific orientation, the math cannot distinguish between "Roll" and "Yaw." They effectively collapse into the same axis. This is Gimbal Lock.

The complementary filter relies on a steady "nudge" from the accelerometer to keep the gyroscope in check. When you hit gimbal lock:

• Discontinuous Jumps: As you approach $90^\circ$ pitch, the Roll calculation becomes extremely sensitive to tiny bits of noise. A tiny vibration can cause the calculated Roll to flip from $0^\circ$ to $180^\circ$ instantly.
• The "Nudge" Becomes a "Shove": The filter sees this $180^\circ$ jump and thinks, "Oh no, we are way off! I need to correct this." It then applies that 2% correction based on a garbage value, causing your orientation estimate to spin or glitch wildly.
• Loss of Degree of Freedom: Since the filter is operating in Euler space (treating Roll, Pitch, and Yaw as independent numbers), it doesn't realize that two of its axes have merged. It tries to update them independently, leading to a "trapped" state where the math can't accurately track a smooth rotation through the vertical.

Madgwick: Gradient Descent on Quaternions The Madgwick filter implements this complementary principle in quaternion space using gradient descent, avoiding the gimbal lock issues of Euler angle approaches. The objective: find the quaternion $q$ that correctly explains the measured gravity direction. If $q$ is our orientation estimate, then rotating the world gravity $[0,0,1]$ into sensor frame should match the normalized accelerometer reading $â$

f(q, â) = q* ⊗ [0,0,0,1] ⊗ q - [0, â_x, â_y, â_z]

The Catch: This math assumes the accelerometer is sensing only gravity. If you are shaking the device, the "Down" vector becomes corrupted by your movement. Madgwick solves this by making the accelerometer's influence very "slow" — it uses gradient descent to gently nudge the orientation toward gravity over time, while letting the gyroscope handle the fast, jerky movements where linear acceleration is highest. It assumes that, on average and in the long term, the only constant force being measured is gravity.

Total acceleration measured by the sensor is: $\vec{a}_{total} = \vec{g}_{gravity} + \vec{a}_{linear}$ . The objective function, $f(q, \hat{a})$, effectively pretends that $\vec{a}_{linear} = 0$. If you are sprinting with the sensor or mounting it on a racing drone, that formula will actually fight the gyroscope and pull your orientation estimate toward the direction of your linear acceleration.

However, the Madgwick filter doesn't jump to the gradient descent solution instantly. It uses a very small step size (a gain parameter called $\beta$).

• High-frequency linear accelerations (like footsteps or vibrations) are usually zero-mean; they oscillate back and forth.
• Because the filter is "slow" to react to the accelerometer, these quick spikes wash out, leaving only the constant pull of gravity as the dominant influence.

Note: High-end "Inertial Navigation Systems" (INS) use GPS or wheel encoders to estimate $\vec{a}_{linear}$ and subtract it from the accelerometer reading before giving it to the filter. Madgwick, in its purest form, just trusts that the movements are "noisy" and gravity is "steady."

Now, expanding the above formula with $q = [w, x, y, z]$:

f₁ = 2(xz - wy) - â_x
f₂ = 2(wx + yz) - â_y
f₃ = 2(0.5 - x² - y²) - â_z

When $f = [0,0,0]$, the quaternion perfectly explains the accelerometer as pure gravity. The gradient: to minimize the squared error $½|f|²$, we compute $∇E = Jᵀ × f$ (see image for the derivation of formula), where $J$ is the Jacobian (partial derivatives of $f$ with respect to $q$):

The fusion: each timestep, combine the gyro prediction with the accelerometer correction

The $\beta$ parameter controls the balance:

Higher $\beta$ corrects drift faster but also reacts to linear acceleration during motion (temporarily corrupting orientation). Lower $\beta$ is more robust during motion but slower to correct drift. The default of 0.04 is a good compromise.

One Last Detail: Adaptive Gravity Estimation The Madgwick filter fixes orientation drift, which fixes gravity direction. But if the gravity magnitude stored in calibration is even slightly wrong, a constant Z residual remains:

residual_z = actual_gravity - calibrated_gravity
position_z_drift = ½ × residual_z × t²

Even 0.003 m/s² error → 1.35m Z drift in 30 seconds. We fix this with a two-stage approach:

# 1. At startup: 2-second precise gravity measurement
gravity_samples = [np.linalg.norm(accel - bias) for accel in startup_samples]
gravity = np.mean(gravity_samples)

# 2. During tracking: slow continuous adaptation when quasi-static
if gravity_deviation < threshold:     # phone not accelerating
    gravity = 0.999 * gravity + 0.001 * np.linalg.norm(accel_corrected)

The continuous estimator uses a very slow EMA ($\alpha = 0.001$) so it only updates when the phone isn't accelerating, and takes ~10 seconds to converge — slow enough that brief motion can't corrupt it.

Fig 7: The final tracker visualized in Rerun — 6-DOF position and orientation from raw IMU data. What's Still Missing This system works well for short-duration, small-area tracking — sliding a phone on a table, rotating it in hand. But it degrades quickly over longer distances and durations. For walking-scale tracking, the phone is in continuous motion, so ZUPT rarely triggers and the gravity check stays at full pass-through. Double integration error grows unchecked. Real pedestrian systems solve this with step detection — recognizing the periodic gait pattern and applying ZUPT at each footstep, giving an error correction anchor every ~0.5 seconds. Yaw drift is unsolvable with accelerometer + gyroscope alone. The Madgwick filter corrects roll and pitch (tilt relative to gravity), but rotation around the gravity axis (yaw) has no accelerometer-observable reference. Fixing yaw requires a magnetometer (compass) or visual features. The fundamental limitation of inertial odometry is that position comes from double-integrating a noisy signal. No amount of filtering can prevent errors from growing over time. This is why production systems (phones, VR headsets, autonomous vehicles) always fuse IMU data with external references: GPS, cameras, LiDAR, or UWB beacons.