untitled design

Sensor Fusion in Action: From Noisy MPU6050 Data to Clean Angles with Kalman Filtering

Leave a Comment / Uncategorized / By
Sensor Fusion with MPU6050 and Kalman Filter

Sensor Fusion in Action

From Noisy MPU6050 Data to Clean Angles with Kalman Filtering

Introduction

Have you ever wondered how drones maintain stable flight or how your smartphone knows its orientation? The secret lies in sensor fusion – the art of combining multiple imperfect sensors to create a more accurate and reliable measurement.

In this hands-on tutorial, we’ll explore how to implement sensor fusion using an MPU6050 IMU sensor and the Kalman Filter to estimate orientation angles.

What You’ll Learn: How to combine accelerometer and gyroscope data using a Kalman filter to get smooth, accurate orientation angles without drift.

The Problem: Why We Need Sensor Fusion

The MPU6050 contains two main sensors:

  • Accelerometer: Measures proper acceleration (gravity + movement)
  • Gyroscope: Measures rotational velocity

Individually, both sensors have serious limitations:

Accelerometer Issues

  • Very noisy (jittery readings)
  • Affected by linear acceleration (tilting vs moving)
  • Good for long-term stability but poor for quick changes

Gyroscope Issues

  • Smooth but drifts over time
  • Integrates small errors that accumulate
  • Good for short-term but unreliable for long-term

The Solution: Kalman Filter as a Smart Compromise

The Kalman Filter acts like a smart assistant that constantly asks:

  • “How much should I trust the accelerometer’s noisy but drift-free angle?”
  • “How much should I trust the gyroscope’s smooth but drifting angle?”

It automatically adjusts this trust based on how reliable each sensor is at any given moment!

Kalman Filter Benefits

  • Automatically balances trust between sensors
  • Provides smooth output without drift
  • Responds quickly to actual movement
  • Filters out sensor noise effectively

Hardware Setup

Components Needed

Arduino Uno
MPU6050 IMU sensor
Jumper wires
Breadboard

Connections

MPU6050 Arduino Uno
VCC 5V
GND GND
SCL A5
SDA A4

Understanding the Code

Part 1: Arduino Data Collection

The Arduino code serves as our data collector:

// MPU6050_Adafruit_Final.ino #include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> Adafruit_MPU6050 mpu; void setup(void) { Serial.begin(115200); // Wait for serial monitor while (!Serial) { delay(10); } // Initialize MPU6050 if (!mpu.begin()) { Serial.println(“Failed to find MPU6050 chip”); while (1) { delay(10); } } // Configure for optimal performance mpu.setAccelerometerRange(MPU6050_RANGE_2_G); mpu.setGyroRange(MPU6050_RANGE_250_DEG); mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); Serial.println(“MPU6050 configured with Adafruit library”); delay(100); } void loop() { sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); // Convert gyro from rad/s to deg/s for compatibility with your Python code float gx_deg = g.gyro.x * 57.2958; float gy_deg = g.gyro.y * 57.2958; float gz_deg = g.gyro.z * 57.2958; // Stream data in same format as before: ax,ay,az,gx,gy,gz Serial.print(a.acceleration.x); Serial.print(“,”); Serial.print(a.acceleration.y); Serial.print(“,”); Serial.print(a.acceleration.z); Serial.print(“,”); Serial.print(gx_deg); Serial.print(“,”); Serial.print(gy_deg); Serial.print(“,”); Serial.println(gz_deg); delay(10); // ~100 Hz }

Part 2: Python Kalman Filter Implementation

The Python code implements the Kalman filter and visualization:

# —————— Kalman Filter —————— class KalmanFilter: def __init__(self): self.Q_angle = 0.001 # Process noise covariance for angle self.Q_bias = 0.003 # Process noise covariance for bias self.R_measure = 0.03 # Measurement noise covariance self.angle = 0.0 # Current angle estimate self.bias = 0.0 # Gyro bias estimate self.P = [[0, 0], [0, 0]] # Error covariance matrix def get_angle(self, newRate, newAngle, dt): # Prediction step (using gyroscope) rate = newRate – self.bias self.angle += dt * rate # Update covariance matrix self.P[0][0] += dt * (dt*self.P[1][1] – self.P[0][1] – self.P[1][0] + self.Q_angle) self.P[0][1] -= dt * self.P[1][1] self.P[1][0] -= dt * self.P[1][1] self.P[1][1] += self.Q_bias * dt # Calculate Kalman Gain (the “trust factor”) S = self.P[0][0] + self.R_measure if S == 0: return self.angle K = [self.P[0][0] / S, self.P[1][0] / S] # Update step (using accelerometer) y = newAngle – self.angle # Innovation (difference between measurement and prediction) self.angle += K[0] * y # Update angle estimate self.bias += K[1] * y # Update bias estimate P00_temp, P01_temp = self.P[0][0], self.P[0][1] self.P[0][0] -= K[0] * P00_temp self.P[0][1] -= K[0] * P01_temp self.P[1][0] -= K[1] * P00_temp self.P[1][1] -= K[1] * P01_temp return self.angle

The Kalman Filter Dance: Step by Step

1

Predict (Using Gyroscope)

rate = newRate – self.bias self.angle += dt * rate

We use the gyroscope to predict where the angle should be based on the previous estimate.

2

Calculate Kalman Gain

K = [self.P[0][0] / S, self.P[1][0] / S]

This is the magic sauce – it determines how much we trust the accelerometer vs our prediction.

3

Update (Using Accelerometer)

y = newAngle – self.angle self.angle += K[0] * y

We correct our prediction with the accelerometer measurement, weighted by the Kalman Gain.

Real-World Results: What You’ll See

Live Data Visualization

When you run the code, you’ll see four real-time plots:

Pitch (Accel + Kalman)

Yellow: Noisy accelerometer pitch

Green: Smooth Kalman filtered pitch

Pitch Visualization

Roll (Accel + Kalman)

Same comparison for roll angle

Roll Visualization

Gyro Pitch

Integrated gyroscope pitch (notice the drift!)

Gyro Pitch

Gyro Roll

Integrated gyroscope roll angle

Gyro Roll

Tuning the Kalman Filter

The performance depends on these tuning parameters:

Parameter Default Value Effect Adjustment
Q_angle 0.001 Process noise for angle Increase if filter responds too slowly
Q_bias 0.003 Process noise for bias Increase if bias correction is too slow
R_measure 0.03 Measurement noise Decrease to trust accelerometer more

Pro Tips for Tuning:

  • If the output is too jittery: Decrease R_measure (trust accelerometer less)
  • If the response is too slow: Increase Q_angle (trust model less)
  • If gyro bias correction is slow: Increase Q_bias

Why This Matters in Real Applications

Drones

Without sensor fusion, drones would either be jittery (trusting accelerometer too much) or drift away (trusting gyroscope too much).

Virtual Reality

VR headsets need both quick response (gyroscope) and long-term stability (accelerometer) for immersive experiences.

Robotics

Autonomous robots need accurate orientation estimates for navigation and control.

Common Issues and Solutions

!

Problem: Angles are wrong when sensor is face-down

Solution: The code includes face-down adjustment: az = az (you might need to invert based on your mounting)

!

Problem: Data is noisy

Solution: Check wiring, add capacitors for power filtering, or adjust Kalman parameters

!

Problem: Serial connection drops

Solution: Check COM port, baud rate (115200), and ensure only one program is accessing the port

Extending the Project

Now that you have stable angles, you can:

Build a self-balancing robot using PID control
Create a gesture controller for your computer
Make a camera stabilization system
Develop a head-tracker for flight simulators

Conclusion

The Kalman filter is like having a smart assistant that knows when to trust each sensor. It gives you smooth, responsive angles without drift – the perfect combination for any motion-sensing project.

Key Takeaways:

  • Sensor fusion combines multiple imperfect sensors
  • Kalman filter automatically balances trust between prediction (gyroscope) and measurement (accelerometer)
  • The “Kalman Gain” is the secret sauce that makes it work
  • Tuning is crucial for optimal performance

The beauty of this approach is that it’s widely applicable – the same principles work for GPS navigation, autonomous vehicles, and countless other applications where you need to combine noisy sensors into clean, reliable data.

Try experimenting with the tuning parameters and watch how the filter behavior changes. Happy coding!

Sensor Fusion with MPU6050 and Kalman Filter – Complete Guide

Note: Make sure to install the required Python libraries (pyqtgraph, pyserial) before running the Python script.

Leave a Comment

Your email address will not be published. Required fields are marked *