ESP32 Sensor Fusion: Combine IMU, Baro and GPS Data

Building an accurate navigation or orientation system with ESP32 sensor fusion using IMU and GPS data is one of the most challenging yet rewarding projects in embedded systems. Raw sensor data from a gyroscope, accelerometer, barometer, or GPS module is never perfect — each has its own noise characteristics, drift, and blind spots. Sensor fusion algorithms combine these imperfect data streams to produce estimates that are better than any single sensor alone. In this comprehensive guide, we’ll explore how to implement sensor fusion on the ESP32, covering complementary filters, Kalman filters, and the Mahony AHRS algorithm — from theory to working code.

Why Sensor Fusion? Limitations of Individual Sensors

To understand why sensor fusion is necessary, consider the weaknesses of each sensor type used in navigation systems:

Gyroscope

Gyroscopes measure angular velocity (degrees per second). Integrating this gives angle, but gyroscopes suffer from drift — tiny errors accumulate over time. Leave an MPU6050 gyroscope running for 10 minutes and your angle estimate can drift 5–20 degrees even at room temperature. Gyroscopes are excellent for short-term, high-frequency rotation measurements but terrible for absolute orientation over time.

Accelerometer

Accelerometers measure linear acceleration. In a stationary device, they measure gravity — allowing calculation of tilt angle relative to Earth’s vertical. However, any motion adds acceleration to the gravity vector, causing large transient errors during movement. Accelerometers give a stable long-term reference but are very noisy in dynamic situations.

GPS

GPS gives absolute position in latitude/longitude (and altitude), but with 2–5 metre horizontal accuracy and 5–15 metre vertical accuracy under open sky. GPS update rates are typically 1–10 Hz, making it too slow for real-time control applications. GPS also fails completely indoors and in urban canyons with multipath reflections.

Barometer

Barometric pressure sensors like the BMP280 or BME280 give excellent relative altitude accuracy (±0.1 m) but drift with weather and temperature changes. They cannot give absolute altitude without a reference pressure reading.

Sensor fusion combines the strengths of each sensor while suppressing their individual weaknesses. A complementary filter uses the gyroscope for fast dynamics and the accelerometer for slow drift correction. A Kalman filter fuses GPS altitude and barometric altitude with optimal statistical weighting.

BMP280 Barometric Pressure and Altitude Sensor I2C/SPI Module

The BMP280 provides ±0.1 m relative altitude precision — an essential sensor for barometric altitude data in a GPS+Baro sensor fusion system on ESP32.

View on Zbotic

Hardware Setup: IMU, Barometer, and GPS on ESP32

For this tutorial we use:

• MPU6050 (or MPU9250 for magnetometer support) — 6-axis IMU via I2C (SDA=GPIO21, SCL=GPIO22)
• BMP280 or BME280 — barometric pressure sensor via I2C (same bus as IMU)
• NEO-6M or NEO-M8N GPS module — UART at 9600 baud (RX=GPIO16, TX=GPIO17)
• ESP32 DevKit — dual-core 240 MHz, sufficient for all fusion algorithms at 100 Hz

I2C Bus Architecture

Connect both the MPU6050 (I2C address 0x68) and BMP280 (I2C address 0x76 or 0x77) to the same I2C bus. The ESP32 I2C peripheral supports 400 kHz fast mode — use this for the IMU to achieve 100–200 Hz sampling rates needed for good gyro integration. The BMP280 only needs to be read at 25–50 Hz for altitude fusion.

GY-BME280-3.3 Precision Altimeter Atmospheric Pressure Sensor

The BME280 adds temperature and humidity sensing on top of BMP280’s barometric capability — one module for altitude, temperature, and humidity in your ESP32 sensor fusion project.

View on Zbotic

Complementary Filter: Simple and Effective

The complementary filter is the simplest sensor fusion algorithm for IMU-based angle estimation. It combines the gyroscope’s high-frequency accuracy with the accelerometer’s low-frequency stability:

// Complementary filter for pitch and roll
float alpha = 0.98;  // Trust gyro 98%, accel 2%
float dt = 0.01;     // 100 Hz loop, 10ms period

// Accelerometer angle
float accel_pitch = atan2(ay, sqrt(ax*ax + az*az)) * 180.0 / M_PI;
float accel_roll  = atan2(-ax, az) * 180.0 / M_PI;

// Complementary filter
pitch = alpha * (pitch + gx * dt) + (1.0 - alpha) * accel_pitch;
roll  = alpha * (roll  + gy * dt) + (1.0 - alpha) * accel_roll;

The alpha coefficient of 0.98 means: trust the gyroscope 98% of the time for fast dynamics, but let the accelerometer correct drift slowly over ~50 update cycles (0.5 seconds at 100 Hz). This simple 4-line algorithm gives surprisingly good results for quadcopter attitude estimation, robot balance control, and gesture recognition — all achievable on an ESP32 with minimal CPU load.

Kalman Filter for Altitude Fusion (Baro + GPS)

The Kalman filter is the mathematically optimal estimator for linear systems with Gaussian noise. For altitude fusion — combining barometric altitude (high frequency, noisy, drifts with weather) and GPS altitude (low frequency, lower accuracy, absolute) — a 1D Kalman filter produces excellent results.

// 1D Kalman filter state
float kf_altitude = 0.0;  // Estimated altitude (m)
float kf_velocity = 0.0;  // Estimated vertical velocity (m/s)
float kf_P[2][2] = {{1,0},{0,1}};  // Error covariance matrix

// Process noise (baro measurement noise: 0.3m)
const float Q_altitude = 0.001;
const float Q_velocity = 0.01;
// Measurement noise
const float R_baro = 0.1;   // BMP280 noise
const float R_gps  = 5.0;   // GPS altitude noise

void kalman_predict(float accel_z, float dt) {
    kf_altitude += kf_velocity * dt + 0.5 * accel_z * dt * dt;
    kf_velocity += accel_z * dt;
    // Update covariance...
}

void kalman_update_baro(float baro_alt) {
    float innovation = baro_alt - kf_altitude;
    float S = kf_P[0][0] + R_baro;
    float K = kf_P[0][0] / S;   // Kalman gain
    kf_altitude += K * innovation;
    kf_P[0][0] *= (1 - K);
}

At 100 Hz IMU updates with 1 Hz GPS updates and 25 Hz baro updates, this Kalman filter delivers altitude estimates accurate to ±0.2 m vertically — dramatically better than raw GPS (±5–15 m).

Mahony AHRS: Full 3D Orientation Estimation

For full 3D orientation (pitch, roll, yaw) — needed for drone flight controllers, robot orientation, or head-tracking — the Mahony AHRS (Attitude and Heading Reference System) is the most popular algorithm in the maker community. It’s computationally lightweight enough to run at 1000 Hz on an ESP32 while producing drift-free attitude estimates.

Mahony AHRS uses a proportional-integral feedback loop to correct gyroscope drift using the cross-product between the measured acceleration vector and the reference gravity vector. When a magnetometer (MPU9250 or separate HMC5883) is available, yaw drift is also corrected using Earth’s magnetic field reference.

Use the Mahony library by Johann Kraus (available in Arduino Library Manager) or the MadgwickAHRS library as an alternative. Both are heavily optimised for embedded targets and produce quaternion output (preferred over Euler angles for gimbal-lock-free computation).

GPS + IMU Dead Reckoning on ESP32

Dead reckoning means estimating position from velocity and direction when GPS is unavailable (indoors, tunnels, dense urban areas). The ESP32 can implement a simple 2D dead reckoning system:

1. Get initial position fix from GPS.
2. When GPS signal is lost, integrate accelerometer data (corrected by gyroscope fusion) to estimate displacement in North-East coordinates.
3. Apply drift correction when GPS is re-acquired by comparing estimated position to GPS position and resetting the state estimate.

Accuracy degrades rapidly over time (accelerometer double-integration error grows as t²), but for short GPS outages of 10–30 seconds, dead reckoning typically maintains position error under 5–10 metres — sufficient for vehicle navigation through underpasses or parking garages common in Indian metros.

FreeRTOS Task Architecture for Multi-Sensor Systems

Running three sensors on ESP32 requires careful task architecture to avoid priority inversions and missed samples. A proven design:

• IMU Task (Core 1, Priority 5): Reads MPU6050 at 100 Hz via I2C. Runs complementary/Mahony filter. Writes orientation to shared data structure protected by a FreeRTOS mutex.
• Barometer Task (Core 1, Priority 4): Reads BMP280 at 25 Hz. Posts baro altitude to Kalman filter via a queue.
• GPS Task (Core 0, Priority 3): Reads NMEA stream at 9600 baud via UART interrupt. Parses GGA sentences for position and altitude. Posts to GPS data queue at 1 Hz.
• Fusion Task (Core 0, Priority 6): Wakes when new data is available on any queue. Runs Kalman update, computes final fused state, publishes to MQTT or displays on screen.

Ai Thinker NodeMCU-32S-ESP32 Development Board – IPEX Version

The dual-core ESP32 is essential for sensor fusion — assign IMU processing to Core 1 and GPS/MQTT tasks to Core 0 without any scheduling conflicts, using this well-supported NodeMCU-32S board.

View on Zbotic

GY-BME280-5V Temperature, Humidity and Pressure Sensor

The 5V BME280 variant is great for ESP32 sensor fusion projects powered from a 5V supply — no separate 3.3V regulator needed, and you get pressure, temperature, and humidity in one chip.

View on Zbotic

Frequently Asked Questions

What is the best IMU for ESP32 sensor fusion in India?

The MPU6050 (6-axis) is the most commonly available and cheapest at ~₹100–150 in India, and it’s excellent for complementary filter and Mahony AHRS. For projects needing magnetometer-corrected yaw (heading), the MPU9250 (9-axis) or ICM-42688-P are better choices. Avoid the GY-87 combination boards for high-speed applications as they add I2C address conflicts.

How do I handle the gimbal lock problem in sensor fusion?

Gimbal lock occurs when using Euler angles (pitch/roll/yaw) at 90-degree singularities — common in aircraft attitude estimation. The solution is to use quaternions internally for all AHRS calculations and only convert to Euler angles for display/output. Both Mahony and Madgwick algorithms output quaternions natively.

Can I run a Kalman filter on ESP32 in real-time at 100 Hz?

Yes, comfortably. A 2-state 1D Kalman filter (altitude + velocity) takes about 5–10 µs per update on the ESP32 at 240 MHz. Even a 6-state Extended Kalman Filter (EKF) for full 3D position runs well under 100 µs — leaving the ESP32 with ample CPU for WiFi, MQTT, and display tasks.

My GPS altitude is very inaccurate. Should I use barometric altitude instead?

Neither alone is ideal — this is exactly why fusion exists. Barometric altitude gives ±0.1 m relative precision but drifts with atmospheric pressure changes. GPS altitude has ±5–15 m absolute accuracy but doesn’t drift. Fusing them with a Kalman filter gives you the best of both: good short-term precision from baro and long-term accuracy anchoring from GPS.