Madgwick: roll, pitch and yaw from an IMU
An accelerometer knows which way is down but is shaken by every bump, and a gyroscope is smooth but drifts. Sebastian Madgwick鈥檚 orientation filter blends the two into one steady estimate of roll, pitch and yaw, and the Arduino Madgwick library (MadgwickAHRS.h) packages it in five calls. It reads no sensor itself: you feed it numbers from an MPU6050 or any other IMU.
Built in on Mokxi: #include <MadgwickAHRS.h> and it builds, with nothing to install. Free, and no account needed.
What Madgwick does
The filter needs to know how often you will call it, given in hertz to begin(), and you must then call it at that rate. Call it faster or slower than you said and the angles lag or overshoot, because the gyro readings are integrated over the time step you promised.
Units matter too: updateIMU() takes rotation in degrees per second and acceleration in any consistent unit, usually g. Yaw from accelerometer and gyro alone drifts slowly, since gravity says nothing about heading; update() with a magnetometer鈥檚 three axes fixes that.
| Library Manager name | Madgwick |
|---|---|
| Author | Arduino |
| Include | #include <MadgwickAHRS.h> |
| Boards | every board Mokxi compiles for but ATtiny85 |
| In the simulator | Simulated: it runs against the part the way it does on the bench. |
| Parts it drives | Accelerometer and gyroscope, MPU6050 |
Install it
It builds on every board Mokxi compiles for but ATtiny85.
In Mokxi it is built in: #include <MadgwickAHRS.h> and the sketch compiles, with no download, no zip and no Library Manager.
On a real board, install "Madgwick" by Arduino from the Arduino IDE鈥檚 Library Manager (Sketch, Include Library, Manage Libraries), or add Madgwick to lib_deps in PlatformIO. Mokxi follows the calls of release 1.2.0, so a sketch written here builds there too.
ESP32-C3 路 Arduino Uno 路 Arduino Nano 路 Arduino Mega 2560 路 Arduino Leonardo 路 SparkFun Pro Micro 路 Arduino Micro 路 ESP32-C6 路 Raspberry Pi Pico 路 Raspberry Pi Pico W 路 Seeed XIAO SAMD21 路 STM32 路 STM32 Nucleo-F411RE 路 STM32 Blue Pill 路 micro:bit V2 路 Arduino Uno R4 路 ESP32 DevKit 路 ESP32-S3-DevKitC-1 路 ESP8266 NodeMCU 路 Arduino Pro Mini 3.3 V 路 Seeed XIAO ESP32-C3 路 ESP32-C3 SuperMini 路 Seeed XIAO ESP32-C6 路 ESP32-DevKitC V4 路 LOLIN32 Lite 路 Wemos D1 R32 路 Cheap Yellow Display 路 TTGO T-Display 路 Seeed XIAO ESP32-S3 路 Arduino Nano ESP32 路 Arduino Nano RP2040 Connect 路 Seeed XIAO RP2040 路 Waveshare RP2040-Zero 路 Arduino Uno R4 WiFi
The calls you will use
In the order a sketch meets them.
Madgwick filter
Creates the filter.
filter.begin(25)
Tells the filter it will be updated 25 times a second.
filter.updateIMU(gx, gy, gz, ax, ay, az)
One step with gyro readings in degrees per second and accelerometer readings in g.
if (micros() - last >= 1000000 / 25) {
last += 1000000 / 25;
filter.updateIMU(gx, gy, gz, ax, ay, az);
}filter.update(gx, gy, gz, ax, ay, az, mx, my, mz)
The same step with a magnetometer鈥檚 readings, which holds yaw steady.
filter.getRoll()
Roll in degrees. getRollRadians() gives radians.
filter.getPitch()
Pitch in degrees.
filter.getYaw()
Yaw (heading) in degrees, 0 to 360. Without a magnetometer it drifts over minutes.
Example: Roll from an MPU6050 through Madgwick鈥檚 filter
On the Arduino Uno R3. It runs in the browser exactly as printed.
#include <Adafruit_MPU6050.h>
#include <MadgwickAHRS.h>
Adafruit_MPU6050 mpu;
Madgwick filter;
const float RATE = 50; // updates a second
unsigned long next = 0;
int count = 0;
void setup() {
Serial.begin(9600);
if (!mpu.begin()) {
Serial.println("no MPU6050");
while (1);
}
filter.begin(RATE);
}
void loop() {
if (millis() < next) return;
next += 1000 / RATE;
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
// The filter takes g and degrees a second; the driver gives m/s^2 and rad/s.
filter.updateIMU(g.gyro.x * 57.2958, g.gyro.y * 57.2958, g.gyro.z * 57.2958,
a.acceleration.x / 9.80665, a.acceleration.y / 9.80665, a.acceleration.z / 9.80665);
if (++count % 50 == 0) {
Serial.print("roll ");
Serial.println(filter.getRoll(), 0);
}
}Wiring
- Arduino Uno R3 A4 to Accelerometer and gyroscope, MPU6050 SDA
- Arduino Uno R3 A5 to Accelerometer and gyroscope, MPU6050 SCL
- Arduino Uno R3 5V to Accelerometer and gyroscope, MPU6050 VCC
- Arduino Uno R3 GND to Accelerometer and gyroscope, MPU6050 GND
Common errors and how to fix them
The angles move far too slowly or swing past the real tilt
The rate passed to begin() does not match how often updateIMU() is called. Time the calls with micros() as the example does, and give begin() the same number.
Roll and pitch jump to large values when the board is still
The gyro values are raw counts or radians. Convert raw MPU6050 counts to degrees per second (divide by 131 at the default range) before passing them in.
Yaw creeps round by itself
That is expected with only an accelerometer and gyro, which cannot see heading. Calibrate the gyro offsets to slow it, or add a magnetometer and call update() with its readings.
Try Madgwick without installing anything
Open the example, change the sketch, and keep your version in a free account.