Sensor Fusion with Kalman Filter on Arduino: IMU Example

If you have ever built a drone, a self-balancing robot, or any motion-tracking project with an Arduino and an IMU (Inertial Measurement Unit), you already know the frustration: accelerometer readings are noisy and jittery, while gyroscope readings drift over time. Neither sensor is reliable on its own. The solution is sensor fusion — combining data from multiple sensors to get a result that is better than any single sensor alone.

The Kalman filter is the gold standard algorithm for sensor fusion. Originally developed for NASA’s Apollo program, it is now used everywhere from aircraft navigation to smartphone orientation tracking. In this tutorial, you will learn how the Kalman filter works conceptually and implement it on an Arduino to fuse accelerometer and gyroscope data from an IMU and get a stable, accurate tilt angle.

What Is Sensor Fusion?

Sensor fusion is the process of combining data from two or more sensors so that the fused result has less uncertainty than any individual sensor. Think of it like asking two witnesses to describe an event — one has perfect short-term memory but forgets over time (gyroscope), and the other has a long-term reliable view but is easily distracted by noise (accelerometer). A smart judge weighs each witness’s testimony appropriately to arrive at the truth.

In an IMU context:

• Accelerometer: Measures linear acceleration including gravity. Can compute absolute tilt angle but is sensitive to vibration and movement noise.
• Gyroscope: Measures angular velocity. Integrating it gives angle, but integration accumulates error (drift) over time.

By fusing these two, you get an angle estimate that is both noise-resistant (thanks to the gyro’s smooth response) and drift-free (thanks to the accelerometer’s absolute reference).

Why Use a Kalman Filter?

There are simpler fusion approaches like the complementary filter, which is essentially a weighted average: angle = 0.98 * (angle + gyro * dt) + 0.02 * accel_angle. This works reasonably well and is very easy to implement.

The Kalman filter, however, is superior because:

• It dynamically adjusts weights based on the estimated uncertainty of each measurement — not a fixed coefficient.
• It is optimal in the sense that it minimizes the mean squared error of the estimate.
• It handles varying noise conditions better than a fixed complementary filter.
• It provides a covariance estimate — you always know how confident the filter is in its answer.

For most hobby projects, both work. But once you understand the Kalman filter, it opens the door to far more complex multi-sensor, multi-variable estimation problems.

Kalman Filter Explained Simply

The Kalman filter operates in two alternating steps: Predict and Update.

Step 1: Predict

Use the gyroscope to predict the current angle based on the previous estimate:

angle_predicted = angle_previous + gyro_rate * dt
P_predicted = P_previous + Q

Here, P is the error covariance (how uncertain we are) and Q is the process noise covariance (how much we trust the gyro model).

Step 2: Update (Correct)

Use the accelerometer measurement to correct the prediction:

K = P_predicted / (P_predicted + R)          // Kalman Gain
angle = angle_predicted + K * (accel_angle - angle_predicted)
P = (1 - K) * P_predicted

Here, R is the measurement noise covariance (how much we trust the accelerometer) and K is the Kalman Gain — the key variable that decides how much weight to give the new measurement.

When the gyro is reliable (P is small), K is small and the new measurement barely changes the estimate. When the gyro has drifted (P is large), K grows and the accelerometer pulls the estimate back toward truth. This is the elegance of the Kalman filter.

Hardware Required

• Arduino Uno, Nano, or compatible board
• MPU-6050 IMU module (6-axis: 3-axis accelerometer + 3-axis gyroscope on a single chip)
• Breadboard and jumper wires
• USB cable

The MPU-6050 communicates over I2C, making it very easy to interface with Arduino. It is one of the most popular IMUs for hobby projects due to its low cost and built-in Digital Motion Processor (DMP).

BMP280 Barometric Pressure and Altitude Sensor I2C/SPI Module

Add barometric altitude to your sensor fusion setup. Combine with IMU data for full 3D navigation state estimation.

View on Zbotic

IMU Setup and Raw Data

Wiring the MPU-6050 to Arduino

Understanding Raw IMU Values

The MPU-6050 returns raw 16-bit integers for each axis. You need to convert them:

• Accelerometer: Divide raw value by 16384 (for ±2g range) to get g-force. Then use atan2() to compute tilt angle.
• Gyroscope: Divide raw value by 131 (for ±250°/s range) to get degrees per second.

The tilt angle from the accelerometer for roll is:

roll_accel = atan2(Ay, Az) * 180 / PI

Implementing the Kalman Filter on Arduino

We implement a simple 1D Kalman filter for the roll angle. The state vector is [angle, gyro_bias], but for clarity we will use a simplified single-variable version here. This is sometimes called a “scalar Kalman filter” and is entirely appropriate for this application.

Key Variables

• Q_angle: Process noise variance for the angle (tune lower to trust gyro more)
• Q_bias: Process noise variance for the gyro bias
• R_measure: Measurement noise variance (tune higher to trust accelerometer less)
• P[2][2]: Error covariance matrix
• K[2]: Kalman gain

Typical Starting Values

Q_angle   = 0.001
Q_bias    = 0.003
R_measure = 0.03

These are not magic numbers — you tune them based on your specific sensor and application. Higher R_measure means you trust the accelerometer less (smoother but slower to correct drift). Lower Q_angle means you trust the gyro model more.

Full Arduino Sketch

#include <Wire.h>

// MPU-6050 I2C address
#define MPU_ADDR 0x68

// Kalman filter variables
float Q_angle  = 0.001f;
float Q_bias   = 0.003f;
float R_measure = 0.03f;

float angle = 0.0f;        // filtered angle
float bias  = 0.0f;        // gyro bias estimate
float P[2][2] = {{0,0},{0,0}};

unsigned long timer;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Wake up MPU-6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Wake up
  Wire.endTransmission(true);

  timer = micros();
  Serial.println("Roll angle (Kalman filtered):");
}

// Read raw data from MPU-6050
struct ImuData {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
};

ImuData readMPU() {
  ImuData d;
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // Starting register for accel data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);
  d.ax = Wire.read()<<8 | Wire.read();
  d.ay = Wire.read()<<8 | Wire.read();
  d.az = Wire.read()<<8 | Wire.read();
  Wire.read(); Wire.read(); // temperature - skip
  d.gx = Wire.read()<<8 | Wire.read();
  d.gy = Wire.read()<<8 | Wire.read();
  d.gz = Wire.read()<<8 | Wire.read();
  return d;
}

// Kalman filter update function
float kalmanUpdate(float newAngle, float newRate, float dt) {
  // --- Predict ---
  float 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 ---
  float S = P[0][0] + R_measure;
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;

  float y = newAngle - angle;
  angle += K[0] * y;
  bias  += K[1] * y;

  float P00_temp = P[0][0];
  float P01_temp = P[0][1];
  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
}

void loop() {
  ImuData imu = readMPU();

  // Convert to physical units
  float Ax = imu.ax / 16384.0f;
  float Ay = imu.ay / 16384.0f;
  float Az = imu.az / 16384.0f;
  float Gx = imu.gx / 131.0f;   // degrees/sec

  // Angle from accelerometer (roll)
  float accelAngle = atan2(Ay, Az) * 180.0f / PI;

  // Elapsed time in seconds
  unsigned long now = micros();
  float dt = (now - timer) / 1000000.0f;
  timer = now;

  // Run Kalman filter
  float kalmanAngle = kalmanUpdate(accelAngle, Gx, dt);

  // Print results
  Serial.print("Accel: ");
  Serial.print(accelAngle, 2);
  Serial.print(" deg  |  Kalman: ");
  Serial.print(kalmanAngle, 2);
  Serial.println(" deg");

  delay(10);
}

Upload this sketch with your Arduino IDE. Open the Serial Monitor at 115200 baud and tilt your MPU-6050. You will immediately see the difference: the Accel reading jumps around with noise, while the Kalman reading is smooth and tracks the true angle faithfully.

Reading Results and Tuning

What to Expect

At rest on a flat surface, both angles should read approximately 0°. As you tilt the board:

• The accel angle will jump immediately but be noisy.
• The Kalman angle will follow smoothly with minimal overshoot.
• After stopping, the Kalman angle will converge to the correct resting angle within a second or two.

Tuning Guidelines

Gyroscope Bias Calibration

At startup, keep the IMU still for 1-2 seconds and average 100 gyro readings to get the initial bias offset. Subtract this from subsequent readings for better accuracy.

Real-World Applications

1. Self-Balancing Robot

A two-wheeled self-balancing robot (like a Segway) uses the Kalman-filtered roll angle as the setpoint error for a PID controller. Without sensor fusion, the robot would oscillate uncontrollably due to noisy angle readings.

2. Drone Flight Controller

Flight controllers like Betaflight fuse gyroscope and accelerometer data (and often barometer/magnetometer too) using Kalman or complementary filters to compute stable attitude estimates for the motor mixing algorithm.

3. Camera Gimbal Stabilization

A 3-axis gimbal keeps a camera level regardless of vehicle motion. The Kalman filter provides jerk-free angle estimates to the servo controllers.

4. Robotic Arm Joint Feedback

IMUs mounted on each link of a robotic arm provide angle feedback to the controller. Kalman filtering smooths encoder noise and compensates for vibration during fast moves.

5. Human Motion Tracking

Wearable health devices use IMUs on the body to track motion for step counting, fall detection, and rehabilitation monitoring. Kalman filtering makes these measurements reliable enough for clinical use.

GY-BME280-3.3 Precision Altimeter Atmospheric Pressure Sensor Module

Add altitude/pressure to your sensor fusion pipeline. Ideal for drones and UAVs needing full position-state estimation.

View on Zbotic

Frequently Asked Questions

Q: Can I use the Kalman filter on an Arduino Nano?

Yes. The Nano has the same ATmega328P as the Uno and can run this code comfortably. The floating-point math runs fast enough at the ~100Hz update rate used here.

Q: Is the Kalman filter better than the complementary filter?

In theory yes — the Kalman filter is mathematically optimal. In practice, for simple angle estimation with a single IMU, a well-tuned complementary filter often gives nearly identical results at lower computational cost. Use Kalman when you need dynamic noise adaptation or are fusing more than two sensors.

Q: My angle drifts even with the Kalman filter. Why?

Check that you are running the update loop fast enough (at least 50Hz is recommended). Also verify your gyro bias calibration. If the bias is not subtracted at startup, even the Kalman filter cannot fully correct for it.

Q: How do I extend this to pitch and yaw as well?

Run the same Kalman filter independently on the Y and Z gyro axes. For yaw you will also need a magnetometer (compass), since neither the accelerometer nor gyroscope alone can provide an absolute yaw reference.

Q: Can the Kalman filter fuse more than two sensors?

Yes. A full multi-dimensional Kalman filter (often called an Extended Kalman Filter or EKF for non-linear systems) can fuse GPS, accelerometer, gyroscope, barometer, and magnetometer together. This is what modern drone autopilots and smartphone navigation chips do.

Q: What library should I use for Kalman filtering in Arduino?

The SimpleKalmanFilter library on Arduino IDE is a great starting point. Alternatively, the code in this tutorial is complete and self-contained — no library required.

Ready to build your own sensor fusion project? Explore Zbotic’s range of sensors and modules to get started. From IMUs to environmental sensors, we stock everything you need for your next Arduino project — fast shipping, competitive prices, and expert support.

Conclusion

The Kalman filter is one of the most powerful tools in an embedded engineer’s arsenal. By combining the best properties of accelerometer and gyroscope data, you can achieve stable, accurate, and drift-free angle estimation on an Arduino with just a few dozen lines of code. Once you understand the predict-update cycle and have tuned Q_angle, Q_bias, and R_measure for your hardware, you will find this algorithm applicable to an enormous range of sensing problems.

Start with the code provided here, experiment with the tuning parameters, and observe how the filter adapts. Once comfortable, extend it to a full 2D/3D state vector and add more sensors. Sensor fusion is a journey — and the Kalman filter is your most reliable guide.