Any help with my Arduino code for getting filtered gyroscope data? (High Pass Filter)

Other Hardware Sensors
1

Hello, I just made my account here to get some help from you guys.

I'm currently studying about drones, and I was actually trying to make a simple flight code to test my PID controller.

Among sensors, I am using MPU6050 from GY-86 IMU sensor.
I was trying to apply LPF to my accelerometer and HPF to my gyroscope.
My LPF code works fine, but my problem arised from HPF code.

I applied HPF and changed a drone's yaw, the yaw seemed to change, but as soon as I stopped moving the drone, filtered yaw immediately decayed to near zero, like angular velocity does.

I belive that since yaw from gyroscope is the result of angular velocity integration, yaw should not decay but stay at the current angular position.

Below is my related code:

void loop() {
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
  // Calculating Roll and Pitch from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
  LPFR();
  LPFP();
  
  // === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
  // Correct the outputs with the calculated error values
  GyroX = GyroX - GyroErrorX; // GyroErrorX ~(-0.56)
  GyroY = GyroY - GyroErrorY; // GyroErrorY ~(2)
  GyroZ = GyroZ - GyroErrorZ; // GyroErrorZ ~ (-0.8)
  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;

  yaw =  yaw + filtered_GyroZ * elapsedTime;
  HPFY();
 
  
  filtered_roll =  filtered_accAngleX;
  filtered_pitch =  filtered_accAngleY;
  
  // Print the values on the serial monitor
  Serial.print(filtered_roll);
  Serial.print(", ");
  Serial.print(filtered_pitch);
  Serial.print(", ");
  Serial.print(filtered_yaw);
  Serial.print(", ");
  getAltitudeP();
  Serial.println(z_p);

}


void HPFY() {
  if (counterYF == 0) {
    filtered_yaw = yaw/(1+2*3.14*Ts*f_ch);
    counterYF++;
    }
  else {
    filtered_yaw = (yaw - prev_yaw + prev_filtered_yaw)/(2*3.14*Ts*f_ch + 1);
    }
  prev_filtered_yaw = filtered_yaw;
  prev_yaw = yaw;
  
  }

I really appreciate your help!

2

Please post ALL the code, not snippets. It is essential to see how all the variables are declared and defined.

This method of integrating the gyro contribution is mathematically incorrect, and will quickly produce nonsensical results. It is somewhat useful only in certain special circumstances (e.g. nearly level, steady flight).

  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
3

@jremington Oh, I'm sorry, and thanks!
This is my full code, which is just an ensemble of bunch of codes from Googling....
And I actually did not sort things, so it would look really messy

/*
   Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
   by Dejan, https://howtomechatronics.com
*/
#include <Wire.h>
#include "MS5611.h"
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
float prev_accAngleX =0;
float prev_accAngleY = 0 ; 
float alpha;
int counterRF = 0;
int counterPF = 0;
int counterYF = 0;
int counterGXF = 0;
int counterGYF = 0;

float filtered_accAngleX = 0;
float filtered_accAngleY = 0;
float prev_filtered_accAngleX = 0;
float prev_filtered_accAngleY = 0;
float filtered_roll = 0;
float filtered_pitch = 0;
float filtered_yaw  = 0;
float prev_filtered_yaw = 0;
float prev_yaw = 0;
float filtered_GyroZ = 0;
float prev_filtered_GyroZ = 0;
float filtered_GyroX = 0;
float prev_filtered_GyroX = 0;
float filtered_GyroY = 0;
float prev_filtered_GyroY = 0;

float Ts = 0.01;
float f_cl = 5;
float f_ch = 5;
int c = 0;



MS5611 MS5611(0x77);
double T;
double P;
double z_p = 0;

void setup() {
  Serial.begin(115200);
  MS5611.begin();
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  /*
  // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  delay(20);
  */
  // Call this function if you need to get the IMU error values for your module
  calculate_IMU_error();
  delay(20);
}
void loop() {
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
  // Calculating Roll and Pitch from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
  
  LPFR();
  LPFP();
  
  // === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
  // Correct the outputs with the calculated error values
  GyroX = GyroX - GyroErrorX; // GyroErrorX ~(-0.56)
  GyroY = GyroY - GyroErrorY; // GyroErrorY ~(2)
  GyroZ = GyroZ - GyroErrorZ; // GyroErrorZ ~ (-0.8)
  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  LPFY();
  LPFGX();
  LPFGY();
  
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + filtered_GyroZ * elapsedTime;
  
 
  
  filtered_roll =  filtered_accAngleX + 1.3;
  filtered_pitch =  filtered_accAngleY- 0.8;

  roll = 0.98*gyroAngleX + 0.02*filtered_roll;;
  pitch = 0.98*gyroAngleY + 0.02*filtered_pitch ; 
  
  // Print the values on the serial monitor
  
  Serial.print(roll);
  Serial.print(", ");
  Serial.print(pitch);
  Serial.print(", ");
  Serial.println(yaw);
//  Serial.print(", ");
//  getAltitudeP();
//  Serial.println(z_p);

}

void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Read gyro values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
  }
  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  // Print the error values on the Serial Monitor
//  Serial.print("AccErrorX: ");
//  Serial.println(AccErrorX);
//  Serial.print("AccErrorY: ");
//  Serial.println(AccErrorY);
//  Serial.print("GyroErrorX: ");
//  Serial.println(GyroErrorX);
//  Serial.print("GyroErrorY: ");
//  Serial.println(GyroErrorY);
//  Serial.print("GyroErrorZ: ");
//  Serial.println(GyroErrorZ);
}



void LPFR() {
  alpha = (2*3.14*Ts*f_cl)/(2*3.14*Ts*f_cl + 1);
  
  if (counterRF == 0) {
    filtered_accAngleX = alpha*accAngleX;
    counterRF ++;
    }

  else {
    filtered_accAngleX = alpha*accAngleX + (1-alpha)*prev_filtered_accAngleX;
    
    }
    
  prev_filtered_accAngleX = filtered_accAngleX;
  
  }

void LPFP() {
  alpha = (2*3.14*Ts*f_cl)/(2*3.14*Ts*f_cl + 1);

  if (counterPF == 0) {
    filtered_accAngleY = alpha*accAngleY;
    counterPF ++;
    }

  else {
    filtered_accAngleY = alpha*accAngleY + (1-alpha)*prev_filtered_accAngleY;
    
    }
    
  prev_filtered_accAngleY = filtered_accAngleY;
  
  }


void LPFY() {
  alpha = (2*3.14*Ts*f_cl)/(2*3.14*Ts*f_cl + 1);

  if (counterYF == 0) {
    filtered_GyroZ = alpha*GyroZ;
    counterYF ++;
    }

  else {
    filtered_GyroZ = alpha*GyroZ + (1-alpha)*prev_filtered_GyroZ;
    
    }
    
  prev_filtered_GyroZ = filtered_GyroZ;
  
  }


void LPFGX() {
  alpha = (2*3.14*Ts*f_cl)/(2*3.14*Ts*f_cl + 1);

  if (counterGXF == 0) {
    filtered_GyroX = alpha*GyroX;
    counterGXF ++;
    }

  else {
    filtered_GyroX = alpha*GyroX + (1-alpha)*prev_filtered_GyroX;
    
    }
    
  prev_filtered_GyroX = filtered_GyroX;
  
  }


void LPFGY() {
  alpha = (2*3.14*Ts*f_cl)/(2*3.14*Ts*f_cl + 1);

  if (counterGYF == 0) {
    filtered_GyroY = alpha*GyroY;
    counterGYF ++;
    }

  else {
    filtered_GyroY = alpha*GyroY + (1-alpha)*prev_filtered_GyroY;
    
    }
    
  prev_filtered_GyroY = filtered_GyroY;
  
  }
  


void HPFY() {
  if (counterYF == 0) {
    filtered_yaw = yaw/(1+2*3.14*Ts*f_ch);
    counterYF++;
    }
  else {
    filtered_yaw = (yaw - prev_yaw + prev_filtered_yaw)/(2*3.14*Ts*f_ch + 1);
    }
  prev_filtered_yaw = filtered_yaw;
  prev_yaw = yaw;
  
  }




  void getAltitudeP() {
  
    MS5611.read();
    MS5611.setOversampling(OSR_ULTRA_HIGH);
    P = MS5611.getPressure();
    T = MS5611.getTemperature() + 273.15;
    z_p = ((8.31432*T)/(9.80665*0.0289644))*log(1013.25/P);
//    Serial.println(z_p);
  }
4

This is the "complementary filter", a crude and antiquated approximation to the 3D orientation problem, useful only for level flight, and was abandoned long ago. It is probably not worth your time to study.

If you are interested in learning about modern techniques that work well, take the time to learn how filters like the computationally expensive Kalman filter, and the more useful and practical Mahony and Madgwick approaches work.

Even an Arduino Uno can get good results with the latter two approaches, and libraries are available for most 6DOF and 9DOF sensors that will work.

It is essential to calibrate the magnetometer and gyro, though.

5

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.