Filtering mpu9250 data using madgwick filter

Hello there,

I bought mpu9250 IMU sensor to control servo motor
and i want to know how to filter its data using madgwick filter?

I'm using arduino nano, arduino IDE v1.13.8

I found a library for the filter but it seems to work only for arduino 101.

I need a library or code that I can use to filter accelerometer, gyroscope, and magnetometer data.

where can I find one? i searched on google like crazy but didn't find anything useful.

here's the code that I'm using...

#include "MPU9250.h"
#include <Servo.h>

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
Servo myServo;
int status;
double roll , pitch, yaw;
float Xh;
float Yh;

int servo_pin = 5;
void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }
  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  
  myServo.attach(servo_pin);
}

void loop() {
  
  // read the sensor
  IMU.readSensor();
/*
  // display the data
  Serial.print(IMU.getAccelX_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelY_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelZ_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroX_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroY_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroZ_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagX_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagY_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagZ_uT(),6);
  Serial.print("\t");
  Serial.println(IMU.getTemperature_C(),6);
*/
pitch = atan2 (IMU.getAccelY_mss() ,( sqrt ((IMU.getAccelX_mss() * IMU.getAccelX_mss()) + (IMU.getAccelZ_mss() * IMU.getAccelZ_mss()))));
roll  = atan2(-IMU.getAccelX_mss() ,( sqrt((IMU.getAccelY_mss() * IMU.getAccelY_mss()) + (IMU.getAccelZ_mss() * IMU.getAccelZ_mss()))));

Yh = (IMU.getMagY_uT() * cos(roll)) - (IMU.getMagZ_uT() * sin(roll));
Xh = (IMU.getMagX_uT() * cos(pitch))+(IMU.getMagY_uT() * sin(roll)*sin(pitch)) + (IMU.getMagZ_uT() * cos(roll) * sin(pitch));

yaw   =  atan2(Yh, Xh);


roll  = roll*57.3;
pitch = pitch*57.3;
yaw   = yaw*57.3;




Serial.print("pitch: ");
Serial.print(pitch);
Serial.print("  ");
Serial.print("roll: ");  
Serial.println(roll);
Serial.print("  ");

Serial.print("yaw: ");  
Serial.println(yaw);
Serial.print("  ");


delay(100);
myServo.write(yaw);
}

This one works, if you make the effort to calibrate the sensor correctly.

The MPU-9250 Madgwick/Mahony filters posted by Sparkfun and Kris Winer don't work.

Keep in mind that the MPU-6050 and MPU-9250 are obsolete and have not been manufactured for some time, so any cheap modules that you buy from Amazon, Alibaba, etc. probably use reject or counterfeit chips.

thank you so much
i will give it a try right now

i cant add the library
it gives this error "Invalid library found in C:\Users\HPP\Documents\Arduino\libraries\MPU-9250-AHRS: no headers files (.h) found in C:\Users\HPP\Documents\Arduino\libraries\MPU-9250-AHRS"

i downloaded the library manually since I cant even add it as a zip file

Those libraries are not intended to be installed normally. They should be put in a /lib folder in the folder for the MPU-9250 AHRS sketch.

1 Like