6DOF & 9DOF Sensor Fusion with Madgwick's Filter, MPU6050, HMC5883L (GY-86 Module)

In this video I show how to use Madgwick's Filter to fuse sensors readings from an InvenSense MPU6050 gyroscope / accelerometer and a Honeywell HMC5883L magnetometer. This filter is very easy to use, with only two settings that require attention: the gain, and the sample frequency. I use Madgwick's Filter to determine the pitch angle of my balancing robot, and take the first steps toward getting the robot to balance. To help visualize the sensor fusion, I also wrote a very basic Java program using the Java3D and jSerialComm libraries. It shows a 3D cube rotating based on the quaternion output of the filter.

Firmware source code is here:

The Java3D test program source code is here:

The sensor module I used is available from ICStation here:

This video is part of a series showing how to build a balancing robot:

Part 1: Modify RC Servos for Continuous Rotation and External H-Bridge Control

Part 2: Building a Robot Chassis with Brass Square Tube, a Dremel, and Solder

Part 3: First Steps with a GY-86 10DOF Sensor: MPU6050, HMC5883L and MS5611

Part 4: [THIS VIDEO] 6DOF & 9DOF Sensor Fusion with Madgwick's Filter, MPU6050, HMC5883L (GY-86 Module)

Part 5: How to Use CC2500 PA LNA 2.4GHz Wireless RF Modules

Part 6: GPU-Accelerated Data Logging and Telemetry

Part 7: How to Tune PID Control Loops Visually

[steubens]

i think this is the first time i've actually seen full rotation modified servos actually do a thing, i thought they would be way worse

[EdwinFairchild]

Quick question when you take multiple readings to get the offset, why do you ultimately subtract it from a raw reading ? Without checking if the offset is negative to begin with, or positive. Because if the offset is negative then subtracting it from a negative reading only increases that negative reading ...or am I not understanding something here?

[arjen]

awesome tutorial about the code man i'm gonna get started : )

[princetn]

cool work. I have been trying to get a similar unit I bought off of ebay to read all sensors. so far I've been only successful at reading MPU & BMP barometer but the magnetometer won't work with any library. I'm using arduino. I tried to turn off the MPU master i2c like you suggest still not readings from magnetometer mine is the HW-290. It has the same pin layout as your unit. but sensors are not exactly the same possibly. I found out that the barometer on the unit is the BMP085. Any suggestion which code i can try to test/read out the magnetometer reading the mag chip it reads DB 5883 7006?

[martinlyon]

I thought to try your Madgwick filter approach to measure pitch on my self balancing robot. I implemented your code just to the point of measuring the pitch. My quandary: starting with the robot on its back, it takes about 12 seconds before I measure near -90 degrees. After those 12 seconds, it measures correctly, and seemingly very accurately. That behavior does not jive with your video. You turn the robot on, and instantly it seems to be ready to go, nearly balancing. When I go further and drive my motors, it looks very much like what you show in this video - only I have to wait 12 seconds. Am I doing something wrong? Thanks. I think your tutorials are fantastic.

[No_Fancy_Edit]

Hi! I wanna use a MPU6050 that has 6DOF in project, and my project is not a balancing robot. Which filter should I use, Kalman or Madgwick? Which one is better and more accurate and works with 6DOF? Thanks in advance.

[jamesthornton]

Hello I was wondering if if you wrote a program so when you first talk with switch song I break with hole for a specified amount of time which would be the balancing on the other wheel and then unlocked and travel forward for a certain amount of stops and then repeat the process

[aron]

Hi. Thank you for these amazing videos. I recently started to design a balancing robot myself, so I wanted to follow along these tutorials. I've managed to read and convert the raw IMU values, and validated them. The problem comes when I want to calculate the pitch from the quaternions. Even in a steady state the value jumpes from -pi/2 to 0.95... to 4.35... did you have similar problem?

[deancollins]

Upgrdman, Awesome presentation. Thanks for sharing the video. Could you kindly help with the connections between the sensors and Microcontroller ?

[Shadowex]

What form do you have to give the readings to the function in? Raw data or processed?

[bekirisgor]

Hi. Firstly, i should thank to you for sharing the code and video. I have a few question.

-Should I get unit vector from accelerometer.
- Sometimes there is jumped betweet 0-65536 because of noise so, should I use "if(gyroY>32768) gyroY=gyroY-65536;".
-After Madgwick algorithm i have quaternion values. How can i get angle axis from quaternion ( I am using euler angles so i don't know is it true) but in your code, you used arccosin formula to get pitch angle.

By the way, I am trying to make a gimball.

[tajammalnawaz]

if gyroscope saturate does it affect euler angles or quaternions which coming from sensor fusion?

[abdulwaiz]

nice video, i recently designed my balancing robot but still it is not so balance as urs, i used complementary, now i want to use Madgwick's Filter, i saw ur code but i didnt understand how u get this value float gyro_x = gyro_x_raw / 939.650784f;
can u plz explain it .

[jamesthornton]

How would it know where balance is because it would always be falling for your rear and trying to stay upright so for it to stop and stand in place it would have to have a reference .42 know it it needs to stay in that one spot to balance in place but I haven't got a clue what I'm saying but this this fascinates me

[abdulwaiz]

i have gyro_x = gyro_x_raw / 65.5 // sensitivity with 500 deg/s so why u took 939.650784

[ICStation]

Thanks for your awesome work !
Share a good news with you : ICStation will have 18% off discount during the Father's Day.