Need help with complementary filter for MPU 6050

actually i have a small project using MPU 6050, but i need the data from that sensor to be filtered using complementary filter. i have no idea how to to implement that filter for my code. can someone fix that. this is my scheme and code for arduino

 * Connections:
 * GY521:
 * - INT  -- D02
 * - SDA  -- A04
 * - SCL  -- A05
 * Powered at 5 V.
 * SD Card:
 * - MOSI -- D11
 * - MISO -- D12
 * - SCLK -- D13
 * - CS   -- D10
 * The SD works fine even if the data connections are also powered at 5 V.
 * LED:
 * - D09
 * DIP:
 * - #01 -- +5V battery
 * - #02 -- D05
 * - #03 -- D04
 * - #04 -- D03
 * 
 * History:
 * R01 150425
 * Initial trials with the MPU6050. 
 * Cleaning of MPU6050.
 * LED added.
 * Serial bit-rate from 9600 to 38400.
 * Time tag introduced.
 * Time interval for data storage introduced.
 * SERIAL vs SD option introduced.
 *
 * R02 150430
 * Moved from protoboard to PCB.
 * DIPs introduced:
 * - #01: battery power on/off
 * - #02: continuous reading on/off
 * - #03: reading interruption
 * - #04: calibration on/off
 * Reading&Logging frequency introduced.
 */

// Includes:
// For I2C:
#include<Wire.h>

// For the SD Card:
#include <SPI.h>
#include <SD.h>

// Defines:
const int MPU = 0x68;   // I2C address of the MPU-6050
const int SD_CS = 10; // SD CS pin
const int LEDpin = 9; // LED
const int DIP02 = 5; // DIP02 = continuous reading on/off
const int DIP03 = 4; // DIP03 = interrupt reading
const int DIP04 = 3; // DIP04 = calibration procedure at setup

// Declaration of global functions:
void Read_Write();
void blinkL(unsigned long T, int N);
void stampC();
void errorFW();

// Global variables:
unsigned long Time0 = 0; // A starting time reference, in millis
unsigned long TimeD = 0; // Delay required to match the desired R&W frequency. 
bool DIP02status = true; // DIP for continuous reading
bool DIP03status = true; // DIP for reading interruption
bool DIP04status = true; // DIP for calibration
bool LEDstatus = false;
bool Serial_plus_SD = true; // Output to SD and Serial, or only to SD.

// Required input:
unsigned long TimeR = 10000UL; // Time of data reading/saving in millis.
unsigned long TimeC = 20000UL; // Time for calibration, in millis.
unsigned long TimeDRW = 25UL; // Average time for data reading and writing to file, ms.
unsigned int frequency = 30; // Data R&W frequency, Hz. Limited by TimeDRW: fmax=1000/TimeDRW.

// Setup:
void setup()
{    
  // Configure the LED:
  pinMode(LEDpin, OUTPUT); 
  
  // Configure DIP for pull-up resistors:
  pinMode(DIP02, INPUT); digitalWrite(DIP02, HIGH);
  pinMode(DIP03, INPUT); digitalWrite(DIP03, HIGH);
  pinMode(DIP04, INPUT); digitalWrite(DIP04, HIGH);
  
  // Join I2C bus via Wire library:
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  // Initialize serial communication:
  if (Serial_plus_SD)
    Serial.begin(38400);
  
  // Initialize SD card:
  if (Serial_plus_SD)
    Serial.println("\nInitializing SD card.");

  if (!SD.begin(SD_CS)) 
  {
    if (Serial_plus_SD)
      Serial.println("Card failed, or not present.");
    // Blink LED quickly to indicate trouble:
    blinkL(200UL, 10);
    digitalWrite(LEDpin, LOW);
    // And "do nothing else":
    while(1)
      ;
  }
  else
  {
    if (Serial_plus_SD)
      Serial.println("Card initialized.");  
  }
  
  // Calibration:
  // Read for the corresponding DIP:
  DIP04status = digitalRead(DIP04);
  if (DIP04status) // DIP04status = HIGH <-> TRUE <-> off <-> calibrate
  {
    Time0 = millis();
    TimeC += Time0;   
    while (Time0 < TimeC) // Read data and save to file:
    {
      // Read and write:
      Read_Write();
      Time0 = millis();
    } // endof while
  } // endof calibration
  
  // Blink LED to indicate readiness:
  blinkL(300UL, 10);
  
  // Calculate the required time delay in order to match the data logging frequency:
  if (1000UL <= TimeDRW * frequency)
    TimeD = 0UL; // if the specified frequency is too large, a cap is introduced.
  else
    TimeD = (1000UL-TimeDRW*frequency)/frequency;
  
  // Initialize time references:
  Time0 = millis();
  DIP02status = digitalRead(DIP02);
  if (DIP02status) // DIP02status = HIGH <-> TRUE <-> OFF <-> time-defined reading
    TimeR += Time0; 
  else
    TimeR = Time0 + 7200000UL; // two hours, in ms, which can be changed as required.

} // end of setup

// Main:
void loop()
{
  // Body of main/loop
  // Check status:
  Time0 = millis();
  DIP03status = digitalRead(DIP03);

  if (DIP03status)
  { // // DIP03status = HIGH <-> TRUE <-> OFF <-> go on reading
    if (Time0 < TimeR) // Then read data and save to file:
    {
      // Read and write:
      Read_Write();
  
      // LED on to indicate correct activity:
      digitalWrite(LEDpin, HIGH);
  
      // Spend some miliseconds in order to match the reading frequency requested:
      delay(TimeD);
    } // endof (if Time0>TimeR)
    else // Time has been exceeded: signal it and "stop":
    {
      // Blink LED slowly to indicate completion:
      blinkL(500UL, 10);
      
      // LED off and "nothing else":
      digitalWrite(LEDpin, LOW);
      while(1)
          ;
    } // endof if-else (Time0>TimeR)
  } // endof if DIP03 = true = off
  else // DIP03 = false = on, do not read:
  {
      // LED off and continue:
      digitalWrite(LEDpin, LOW);
  }

}

void Read_Write()
// function that reads the MPU and writes the data to the SD.
{
 // Local variables:
  int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; // Variables read from MPU6050

  // Read data:
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);  // request a total of 14 registers
  AcX = Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY = Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
  // Data preparation for file saving:
  String dataString = ""; // string for assembling the data to log:

  // Add time tag:
  dataString += String(Time0); dataString += ",";

  // Append the MPU6050 data to the string:
  dataString += String(AcX); dataString += ",";
  dataString += String(AcY); dataString += ",";
  //dataString += String(AcZ); dataString += ",";
  //dataString += String(GyX); dataString += ",";
  //dataString += String(GyY); dataString += ",";
  //dataString += String(GyZ);
  
  // Open the file in append mode:
  File dataFile = SD.open("datalog.txt", FILE_WRITE);

  // If the file is available, write to it:
  if (dataFile) 
  {
    dataFile.println(dataString);
    dataFile.close();
    if (Serial_plus_SD)
      Serial.println(dataString);
  }
  // if the file does not open, pop up an error:
  else 
    errorFW();
  
  return;
}

void stampC()
// function that writes a mark of calibration
{
  // Local variables:
  String dataString = " CALIBRATION "; 
 
  // Open the file in append mode:
  File dataFile = SD.open("datalog.txt", FILE_WRITE);

  // If the file is available, write to it:
  if (dataFile) 
  {
    dataFile.println(dataString);
    dataFile.close();
    if (Serial_plus_SD)
      Serial.println(dataString);
  }
  // if the file does not open, pop up an error:
  else 
    errorFW();
  
  return;
}

void blinkL(unsigned long T, int N)
// function that blinks the LED N times with a delay of T millis.
{
  bool statusL = false;
  for (int i=0; i<N; i++)
  {
    delay(T);
    statusL = !statusL;
    digitalWrite(LEDpin, statusL);
  }
  return;
}

void errorFW()
// function that informs about an error while writing in the file, and "holds on" the sketch
{
  if (Serial_plus_SD)
    Serial.println("Error opening datalog.txt.");

  // In any case, blink LED quickly to indicate trouble:
  blinkL(200UL, 10);
  // LED off and "nothing else":
  digitalWrite(LEDpin, LOW);
  while(1)
    ;
 
  return;
}

MPU6050_02REV.ino (7.89 KB)

Is this thread useful? http://forum.arduino.cc/index.php?topic=58048.0

The Madgwick filter looks like a good one.

Or this one.

http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/