Sensor Fusion Algorithms: Combining IMU, GPS and Magnetometer Data

Combining data from a 9-axis IMU, a GPS receiver, and a magnetometer sounds straightforward on a datasheet: one gives you high-frequency motion, one gives you absolute position, and one gives you heading. In practice, running all three together on an edge device is where most projects stumble. I've deployed sensor fusion stacks on everything from STM32H7-based agricultural rovers to nRF52840 wearables, and the same failure modes repeat: gyroscope bias drifts your yaw 15 degrees in a minute, the magnetometer is hopelessly distorted inside a steel building, and your 10 Hz GPS fix arrives 80-120 ms late over UART, smearing your velocity estimate. This article breaks down the algorithms and embedded implementation patterns I use to make those three sensors agree, with a focus on running an Error-State Kalman Filter reliably on microcontrollers with less than 256 KB RAM and without floating-point luxuries you might expect on a companion computer.

Why Naive IMU, GPS, and Magnetometer Integration Collapses in Real-World Motion

In my experience, the first approach teams try is to integrate the gyroscope for orientation, double-integrate the accelerometer for position, and then snap to GPS when available. That dead-reckoning path diverges within seconds. A typical consumer-grade MEMS gyroscope like the ICM-42688-P has bias instability around 3-5°/hr and significant turn-on bias. Accelerometers give you pitch and roll from gravity, but only when linear acceleration is near zero. The magnetometer, while theoretically an absolute yaw reference, measures a combined field of Earth's ~25-65 µT plus local hard-iron and soft-iron distortions that can exceed 100 µT near motors and batteries.

GPS complicates timing rather than solving it. A u-blox M9N outputs position and velocity at 10 Hz at best, with horizontal accuracy of 1.5-2.5 m in open sky and much worse in urban canyons. The velocity derived from Doppler is far more stable than position differentiation, but it is still delayed. If you simply blend a delayed GPS velocity with a current IMU acceleration, you inject a systematic lag that an optimizer will interpret as acceleration bias. This is why any serious fusion algorithm must model sensor errors, not just sensor values.

The Core Problem: Observability and Error Modeling

A sensor fusion filter does not estimate position and attitude directly. It estimates the errors in those quantities alongside sensor biases. Without estimating gyro bias (3 states), accelerometer bias (3 states), and often magnetometer bias, the filter has no mechanism to prevent drift. GPS makes velocity and position observable, but only intermittently. The magnetometer makes yaw observable, but only if you can trust it. In my field deployments, I have found that treating magnetometer reliability as binary — either trusted or rejected — is far more robust than trying to filter a distorted heading.

On resource-constrained edge hardware, you also face determinism constraints. As described in the FreeRTOS Documentation, interrupt latency and task jitter directly affect IMU sampling regularity. A 200 Hz IMU loop with 2 ms jitter introduces more integration error than a 100 Hz loop with 100 µs jitter. Locking your IMU read to a hardware timer or data-ready interrupt is non-negotiable.

Quaternion Kinematics and the 9-Axis Sensor Fusion State Vector

Euler angles have no place inside a fusion filter. The gimbal lock at ±90° pitch and the discontinuities in yaw will break your covariance propagation. I use unit quaternions for all internal attitude representation and convert to Euler only for logging and control. A quaternion q = [q_w, q_x, q_y, q_z] representing rotation from body to navigation frame propagates with the gyroscope measurement ω as:

q_dot = 0.5 * q ⊗ [0, ω]

where ⊗ is quaternion multiplication. This differential equation is integrated with a first-order or second-order integrator at the IMU rate. I've found that a simple forward Euler with dt < 0.005s is adequate if you renormalize the quaternion every cycle, but for aggressive motion (drones, handheld devices) a second-order Runge-Kutta prevents noticeable energy gain.

Defining a Minimal Yet Sufficient State

For a 9-axis GPS-aided system, my baseline error-state vector is 15 states: attitude error (3), velocity error (3), position error (3), gyro bias error (3), and accel bias error (3). Yaw bias from the magnetometer is handled implicitly through attitude error. If you have sufficient CPU headroom (Cortex-M7 or ESP32-S3), you can extend to 18 states by adding magnetometer hard-iron bias (3). For a Cortex-M4 at 120 MHz, the 15-state version is the sweet spot.

It is tempting to add GPS clock bias or scale factors, but every added state increases covariance matrix operations cubically. On TinyML-enabled edge nodes where you may also be running a neural network for activity classification, that cost matters. If you are planning to co-locate inference and fusion, see TinyML: Running Neural Networks on Microcontrollers with TensorFlow Lite for memory partitioning strategies — the lessons on arena sizing directly apply to reserving DTCM for filter covariances.

// Quaternion propagation at IMU rate (C, single precision)
// Assumes gyro in rad/s, dt in seconds
typedef struct { float w, x, y, z; } quat_t;

void quat_propagation(quat_t *q, float wx, float wy, float wz, float dt) {
    // quaternion derivative = 0.5 * q * omega_quat
    float half_dt = 0.5f * dt;
    quat_t q_dot;
    q_dot.w = (-q->x * wx - q->y * wy - q->z * wz) * half_dt;
    q_dot.x = ( q->w * wx + q->y * wz - q->z * wy) * half_dt;
    q_dot.y = ( q->w * wy - q->x * wz + q->z * wx) * half_dt;
    q_dot.z = ( q->w * wz + q->x * wy - q->y * wx) * half_dt;

    q->w += q_dot.w;
    q->x += q_dot.x;
    q->y += q_dot.y;
    q->z += q_dot.z;

    // Renormalize to unit length - critical
    float norm = sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    q->w /= norm; q->x /= norm; q->y /= norm; q->z /= norm;
}

Error-State Extended Kalman Filter: From Covariance Propagation to Embedded Implementation

The Error-State Extended Kalman Filter (ES-EKF) is my default for IMU-GPS-magnetometer fusion on microcontrollers. Unlike a direct EKF that estimates the full state, the ES-EKF estimates small errors and injects them into a nominal state that is propagated by nonlinear kinematics. The benefit is that the error dynamics are close to linear, so linearization errors remain small even with large attitude motions, and you avoid gimbal issues entirely.

The cycle is two-phase: predict at IMU rate, correct at measurement rate. Prediction propagates the nominal state (quaternion, velocity, position) using bias-corrected IMU data and propagates the covariance P as P = F * P * F^T + Q, where F is the error-state transition matrix and Q is process noise. Correction computes Kalman gain K = P * H^T * (H * P * H^T + R)^-1 and updates the error state.

Making the EKF Fit in 100 KB RAM

A full 15x15 covariance matrix as float32 is 900 bytes, which is fine, but the matrix multiplications are expensive. I store only the upper-triangular part and use symmetric operations. More importantly, I never invert a full matrix. GPS position/velocity updates are 3-DOF or 6-DOF, and magnetometer updates are 3-DOF, so the innovation covariance S = H*P*H^T + R is at most 6x6. I implement a Cholesky decomposition for S inversion rather than a generic Gauss-Jordan.

Process noise Q must be tuned from Allan variance data, not guessed. For an ICM-42688, I typically set gyro noise density to 0.005 dps/√Hz and accelerometer noise to 80 µg/√Hz, then derive continuous-time Q and discretize with dt. Measurement noise R for GPS is taken directly from the u-blox ePV/ePH and sAcc fields, scaled by HDOP. For the magnetometer, I use a fixed R of 5-10 µT squared when undistorted, and inflate it 10x when suspicious.

// EKF correction step for GPS velocity (simplified 6-state example)
// State ordering: [att_err(3), vel_err(3), pos_err(3), bg(3), ba(3)]
void ekf_correct_gps_velocity(float *error_state, float P[15][15],
                              const float gps_vel_ned[3],
                              const float est_vel_ned[3],
                              float R_vel) {
    float y[3]; // innovation
    for(int i=0;i<3;i++) y[i] = gps_vel_ned[i] - est_vel_ned[i];

    // H maps velocity error directly: H = [0  I  0  0  0]
    // Innovation covariance S = H*P*H^T + R = P_vel_vel + R*I
    float S[3][3];
    for(int i=0;i<3;i++) for(int j=0;j<3;j++)
        S[i][j] = P[3+i][3+j] + (i==j ? R_vel : 0.0f);

    // Compute Kalman gain K = P*H^T*inv(S)
    // P*H^T is simply columns 3-5 of P
    float Sinv[3][3];
    invert_3x3(S, Sinv); // Cholesky or analytic

    float K[15][3] = {0};
    for(int i=0;i<15;i++) for(int j=0;j<3;j++)
        for(int k=0;k<3;k++) K[i][j] += P[i][3+k] * Sinv[k][j];

    // Update error state: dx = K * y
    for(int i=0;i<15;i++) {
        float corr = 0.0f;
        for(int j=0;j<3;j++) corr += K[i][j] * y[j];
        error_state[i] += corr;
    }
    // Joseph form covariance update: P = (I-KH)P(I-KH)^T + KRK^T
    // ... omitted for brevity, use symmetric update
}

When you need to run additional workloads alongside the filter, model optimization becomes critical. Quantizing a co-resident anomaly detector from float32 to int8 can free up 60-70% of flash and significantly reduce inference latency, leaving more headroom for deterministic filter execution. The techniques in Edge AI Model Optimization: Pruning, Quantization and Knowledge Distillation are directly applicable when you partition CPU time between the EKF and neural network on the same MCU.

Taming Magnetic Anomalies and GPS Latency with Adaptive Correction Logic

Magnetometers are the weakest link. Indoors, near structural steel, or on a vehicle with high current draw, the local field magnitude ||m|| can deviate from Earth's field by 50% or more. I implement two hard checks before accepting a magnetometer update: magnitude check and dip angle check.

The magnitude check rejects any sample where ||m|| deviates by more than 15% from the calibrated earth field magnitude (typically 45-55 µT depending on latitude). The dip check computes the inclination angle between the measured field and gravity (from the accelerometer) and rejects if it differs by more than 5° from the World Magnetic Model expected dip. If either check fails for more than 500 ms, I stop magnetometer corrections entirely and let yaw drift slowly, constrained only by GPS course-over-ground when velocity exceeds 1.5 m/s.

Handling Delayed GPS Without an Expensive Buffer

GPS latency is not optional to handle. On a Zephyr RTOS system, I've measured UART + parser + DMA delays of 85 ms for NMEA and 40 ms for UBX. If you apply that measurement at the current time, you corrupt the estimate. The correct approach is to keep a short history buffer of IMU-propagated states (typically 100-150 ms at 200 Hz = 20-30 states, each ~64 bytes). When a GPS measurement arrives with timestamp t_gps, you retrieve the state at t_gps, apply the correction there, then re-propagate forward to current time using buffered IMU data. The Zephyr Project Documentation provides good examples of ring buffers and timestamp synchronization using k_cycle_get_32() that I reuse for this purpose.

For stationary or low-speed use cases where GPS course is unavailable, you need another yaw aid. I've found that zero-velocity updates (ZUPT) combined with gyro bias learning during detected stationary periods dramatically limits yaw drift. A simple variance detector on accel and gyro over a 0.5s window can detect stillness with >99% accuracy and allow you to clamp velocity to zero and observe bias.

Time Alignment, Sensor Calibration, and TinyML-Driven Noise Modeling at the Edge

All sensor fusion accuracy is ultimately gated by calibration and time alignment. Factory calibration is insufficient for magnetometers and often for accelerometers. I perform a 6-position accelerometer calibration for scale and bias, and an ellipsoid fit for magnetometer hard/soft iron. The ellipsoid fit can run offline in Python, but I now embed a lightweight recursive ellipsoid estimator that runs during the first 2 minutes of operation when the user is asked to rotate the device in a figure-eight.

Time alignment is more subtle. The IMU data-ready interrupt should timestamp at the ISR, not when the I2C/SPI transaction completes. GPS PPS is ideal for global time, but most low-cost modules don't expose it. Instead, I timestamp the GPS UART RX interrupt and estimate transport delay as a constant learned offline with a logic analyzer.

Adaptive Noise Tuning with TinyML

Static Q and R matrices are a compromise. Vibration on a drone versus walking motion needs different process noise. Instead of hand-tuning mode switches, I've started training a tiny 1D CNN (2 layers, ~4k parameters) that classifies motion context — static, walking, vehicular, high-vibration — from a 1-second window of raw accel/gyro. The classifier runs every 500 ms at <5 ms on a Cortex-M4F using int8 quantization and outputs a scaling factor for Q. This is a clean TinyML for sensor conditioning pattern, not for the fused output itself. If you deploy such models via different frameworks, ONNX Runtime for Edge: Cross-Framework Model Deployment on ARM covers the conversion path that lets you train in PyTorch and deploy with TFLM or ONNX Runtime efficiently.

// Magnetometer acceptance logic with magnitude + dip check
bool is_magnetometer_trusted(const float mag_uT[3], const float accel_g[3],
                             float earth_field_mag, float expected_dip_deg) {
    float mag_norm = sqrtf(mag_uT[0]*mag_uT[0] + mag_uT[1]*mag_uT[1] + mag_uT[2]*mag_uT[2]);
    if (fabsf(mag_norm - earth_field_mag) > 0.15f * earth_field_mag) return false;

    // Normalize vectors
    float mag_n[3] = {mag_uT[0]/mag_norm, mag_uT[1]/mag_norm, mag_uT[2]/mag_norm};
    float acc_norm = sqrtf(accel_g[0]*accel_g[0] + accel_g[1]*accel_g[1] + accel_g[2]*accel_g[2]);
    if (acc_norm < 0.7f || acc_norm > 1.3f) return false; // not in quasi-static
    float acc_n[3] = {accel_g[0]/acc_norm, accel_g[1]/acc_norm, accel_g[2]/acc_norm};

    // Dip = angle between mag and gravity (acc points up when static)
    float dot = mag_n[0]*acc_n[0] + mag_n[1]*acc_n[1] + mag_n[2]*acc_n[2];
    float dip_meas = asinf(fabsf(dot)) * 57.29578f;
    if (fabsf(dip_meas - expected_dip_deg) > 5.0f) return false;

    return true;
}

Bench-Testing Fusion Accuracy: Ground Truth Systems and Long-Duration Drift Metrics

You cannot tune a filter with live GPS alone. I maintain two test rigs: a rate table with an optical encoder (0.01° accuracy) for attitude, and an outdoor dolly with RTK GPS (u-blox F9P with 1-2 cm accuracy) for position. Without ground truth, you are guessing. Log raw sensor data to SD card at full rate and replay through your filter offline in Python or C on a host. This lets you iterate on Q/R 50x faster than field tests.

Key metrics I track on 10-minute datasets: (1) attitude RMSE during dynamic maneuvers, (2) yaw drift during 60-second GPS dropouts, (3) position error after returning to start (loop closure), and (4) velocity error during acceleration. On a well-tuned 15-state ES-EKF on STM32H743 at 200 Hz, I consistently achieve <0.8° roll/pitch RMSE, <2° yaw RMSE with clean mag, <1.5 m position RMSE with standalone GPS, and <0.3 m with RTK aiding. During GPS denial, horizontal position drifts at ~0.5-1.0% of distance traveled with consumer MEMS, which is useful for sizing your dead-reckoning window.

Long-Duration Stability and Temperature

A common oversight is temperature-dependent bias. MEMS bias drifts 0.01-0.05 dps/°C. If you ignore it, a device moving from 25°C indoors to 5°C outdoors will develop a yaw rate offset that looks like motion. I log internal temperature and include a linear thermal model: bg(T) = bg0 + TC * (T - T0). TC is calibrated by cycling the board in a thermal chamber from -10°C to 60°C and fitting bias vs temperature. This single addition cut my long-duration yaw drift by 40% on vehicle-mounted units.

The table reflects my measurements on an STM32F411 and STM32H743. Notice the jump from Madgwick to ES-EKF is 20-30x CPU but buys you bias estimation and GPS integration. For most edge computing platforms handling navigation, that trade is worth it. Reserve particle filters for offline mapping where the distribution is truly multimodal.

Frequently Asked Questions