Sensor Fusion: Combine Accelerometer Gyroscope with Kalman Filter

If you have ever built a drone, a self-balancing robot, or a robotic arm, you know the frustration: the gyroscope drifts over time, and the accelerometer is too noisy for fast motion. The solution is sensor fusion — mathematically combining the two sensors so their weaknesses cancel out. The Kalman filter is the gold-standard algorithm that makes this possible.

This tutorial walks you through the theory of sensor fusion, explains exactly how a Kalman filter works, and gives you working Arduino code that fuses MPU-6050 accelerometer and gyroscope data into a clean, stable orientation estimate.

Why Do You Need Sensor Fusion?

No single sensor perfectly measures orientation. An accelerometer measures the direction of gravitational acceleration — giving an absolute tilt angle — but it is extremely sensitive to linear acceleration (vibration, movement), causing large spikes during motion. A gyroscope measures angular rate (degrees per second) and can be integrated over time to give angle, but integration accumulates errors (known as gyroscope drift), causing the angle estimate to slowly wander even when the sensor is stationary.

Sensor fusion is the process of mathematically combining multiple sensor readings to produce an estimate that is better than any individual sensor. The Kalman filter is the optimal linear estimator for this task when sensor noise is Gaussian.

Accelerometer and Gyroscope Fundamentals

Accelerometer

A MEMS accelerometer measures force per unit mass (acceleration in m/s²). At rest, it measures only gravity (9.81 m/s² downward). By computing the angle of the gravity vector relative to the sensor axes, you can determine tilt:

θ_accel = atan2(Ay, Az)   // pitch angle (X-axis tilt)
φ_accel = atan2(Ax, Az)   // roll angle (Y-axis tilt)

Strengths: Absolute angle reference, no long-term drift.
Weaknesses: Noisy, sensitive to vibration and linear acceleration, cannot measure yaw (rotation around vertical axis).

Gyroscope

A MEMS gyroscope measures angular rate (°/s or rad/s). Integrating the angular rate gives angle:

θ_gyro += gyro_rate × dt   // Euler integration

Strengths: Very smooth, fast response, not sensitive to linear acceleration.
Weaknesses: Drifts over time due to bias, temperature sensitivity, and integration of quantisation noise.

The Two Core Problems

Gyroscope Drift

A typical MEMS gyroscope has a bias drift of 0.1–10 °/s. Over 60 seconds, even a 1 °/s drift accumulates to 60° of error — making the sensor useless for long-duration angle tracking without correction.

Accelerometer Noise

A vibrating drone or moving robot introduces linear accelerations that can be 10–50× stronger than gravity. The accelerometer cannot distinguish these from gravitational acceleration, producing wildly incorrect tilt angles during motion.

The insight behind sensor fusion: The accelerometer is accurate over the long term (gives the correct average angle) but noisy in the short term. The gyroscope is accurate in the short term (smooth, fast response) but drifts in the long term. Combining them — trusting the gyroscope at high frequencies and the accelerometer at low frequencies — gives you the best of both worlds.

Complementary Filter: The Simple Start

Before tackling the Kalman filter, it is worth understanding the complementary filter — a simpler, less computationally expensive fusion method.

angle = alpha × (angle + gyro_rate × dt) + (1 - alpha) × accel_angle

Where alpha is typically 0.95–0.98

This is essentially a high-pass filter on the gyroscope data combined with a low-pass filter on the accelerometer data. It works well for many applications (balancing robots, phone screen orientation) and requires very little code. However, it does not optimally handle varying noise levels and cannot provide uncertainty estimates.

Kalman Filter Theory

The Kalman filter, developed by Rudolf Kálmán in 1960, is a recursive algorithm that estimates the state of a linear dynamic system from noisy measurements. For sensor fusion, the “state” is our angle estimate and the gyroscope bias.

The key insight is that the Kalman filter maintains two pieces of information at all times:

1. The state estimate (x̂) — our best guess of the true angle
2. The estimation uncertainty (P) — how confident we are in our estimate

At every time step, the algorithm performs two phases:

• Predict phase: Use the gyroscope (the motion model) to propagate the estimate forward in time. Uncertainty grows because we are not certain the gyroscope is perfect.
• Update phase: Use the accelerometer measurement to correct the prediction. The Kalman Gain (K) determines how much we trust the measurement versus our prediction — a key advantage over the complementary filter.

Kalman Filter Equations Step by Step

For IMU fusion, the state vector is:

x = [θ,  b]ᵀ
     angle  gyro_bias

Step 1: Predict (Time Update)

// Predicted angle (gyro integration, bias corrected)
θ_pred = θ + dt × (gyro_rate - b)

// Predicted bias (assumed constant)
b_pred = b

// Predicted error covariance
P_pred = A × P × Aᵀ + Q

Step 2: Update (Measurement Update)

// Innovation (measurement residual)
y = θ_accel - θ_pred

// Kalman Gain
K = P_pred × Hᵀ × (H × P_pred × Hᵀ + R)⁻¹

// Updated state estimate
θ_est = θ_pred + K[0] × y
b_est = b_pred + K[1] × y

// Updated error covariance
P = (I - K × H) × P_pred

The matrices Q (process noise covariance) and R (measurement noise covariance) are the tuning parameters that govern filter behaviour. Higher Q → trust measurements more. Higher R → trust predictions more.

MPU-6050 Wiring with Arduino

Arduino Kalman Filter Code

#include <Wire.h>

const int MPU = 0x68;
double gyroX_rate, accX_angle;

// Kalman state
double Q_angle   = 0.001;  // Process noise: angle
double Q_bias    = 0.003;  // Process noise: gyro bias
double R_measure = 0.03;   // Measurement noise

double angle = 0, bias = 0;
double P[2][2] = {{0,0},{0,0}};
unsigned long lastTime;

double kalmanUpdate(double newAngle, double newRate, double dt) {
  // Predict
  double rate = newRate - bias;
  angle += dt * rate;
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  // Update
  double S = P[0][0] + R_measure;
  double K[2] = { P[0][0] / S, P[1][0] / S };
  double y = newAngle - angle;
  angle += K[0] * y;
  bias  += K[1] * y;
  P[0][0] -= K[0] * P[0][0];
  P[0][1] -= K[0] * P[0][1];
  P[1][0] -= K[1] * P[0][0];
  P[1][1] -= K[1] * P[0][1];
  return angle;
}

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0); // Wake up MPU-6050
  Wire.endTransmission();
  Serial.begin(115200);
  lastTime = millis();
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Starting register
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true);

  int16_t AcX = Wire.read()<<8 | Wire.read();
  int16_t AcY = Wire.read()<<8 | Wire.read();
  int16_t AcZ = Wire.read()<<8 | Wire.read();
  Wire.read(); Wire.read(); // Temperature (skip)
  int16_t GyX = Wire.read()<<8 | Wire.read();

  double dt = (millis() - lastTime) / 1000.0;
  lastTime = millis();

  accX_angle = atan2(AcY, AcZ) * 180.0 / PI;
  gyroX_rate = GyX / 131.0; // 131 LSB/°/s at ±250°/s range

  double pitch = kalmanUpdate(accX_angle, gyroX_rate, dt);
  Serial.print("Pitch: "); Serial.println(pitch);
  delay(10);
}

Tuning Q, R, and P

Getting the filter to perform well requires tuning three noise covariance parameters:

• Q_angle (process noise for angle): Increase if the gyroscope is very noisy. Decrease if the system dynamics are predictable. Typical range: 0.0001–0.01.
• Q_bias (process noise for gyro bias): Increase if the bias drifts quickly (warm-up, temperature changes). Decrease for a very stable gyroscope. Typical range: 0.001–0.01.
• R_measure (measurement noise covariance): Increase if the accelerometer is very noisy (vibrating platform). Decrease if the accelerometer is clean. Typical range: 0.01–0.5.

Practical tuning tip: Start with the default values above. If the output is too laggy, reduce R_measure. If it shakes too much, increase R_measure. If it drifts, reduce Q_bias.

Advanced: Madgwick and Mahony Filters

For 3D orientation (quaternion-based), the Madgwick filter and Mahony filter are computationally efficient alternatives that handle full 6-DOF or 9-DOF (with magnetometer) fusion.

• Madgwick: Single tuning parameter (beta = 0.033 typical). Runs at 1 kHz on Cortex-M3. Uses gradient descent to minimise sensor error. Very popular in drone flight controllers.
• Mahony: Uses proportional-integral feedback instead of gradient descent. Slightly simpler, also widely used in embedded systems.
• EKF (Extended Kalman Filter): Handles non-linear systems. Used in professional navigation systems, autonomous vehicles, and robotics where accuracy is paramount.

BMP280 Barometric Pressure and Altitude Sensor

Add barometric altitude to your IMU fusion stack for 9-DOF navigation. Fuse with MPU-6050 for altitude-hold in drones.

View on Zbotic

Applications of IMU Sensor Fusion

• Self-balancing robots: The classic application — a 2-wheeled inverted pendulum robot uses Kalman-filtered pitch angle to keep the PID controller stable.
• Drone flight controllers: ArduPilot and Betaflight both implement EKF-based sensor fusion for attitude estimation. Stable flight is impossible without it.
• VR headsets: Low-latency Kalman fusion of gyroscope and accelerometer (and magnetometer) tracks head orientation with sub-millisecond latency.
• Gesture recognition: Wearable fitness trackers and smartwatches use complementary/Kalman filters to classify arm gestures (step counting, exercise detection).
• Gimbal stabilisation: Camera gimbals use 3-axis Kalman filtering to compute motor corrections that keep the camera horizon-level.
• Agricultural robots: Tractor auto-steering systems fuse GPS with IMU using Kalman filters to achieve centimetre-level path tracking.

Frequently Asked Questions

Conclusion

Sensor fusion with a Kalman filter transforms the complementary weaknesses of accelerometers and gyroscopes into a single, accurate, low-noise orientation estimate. The filter’s ability to simultaneously estimate angle and gyroscope bias — and to weight each sensor contribution optimally based on noise characteristics — makes it far superior to simple averaging or complementary filtering in demanding applications.

With the Arduino code in this tutorial, you can immediately start experimenting with MPU-6050 fusion. Once comfortable with the 1D Kalman filter shown here, you can extend it to full 3D quaternion-based fusion using the Madgwick or Mahony filter libraries that are freely available for Arduino and STM32.