Ayuda con programa PID para un Drone (Quadricoptero)

Buenas tardes,
Bueno después de varias búsquedas e intentos no logra dar con una solución para el programa PID que ando tratando de entender y desarrollar. Mi problema es que parece ser que el PID funciona a la perfección pero en cuanto se realiza una pequeña variación con el sensor IMU que utilizo para la estabilización del Drone hace unos picos muy extraños que impiden que se estabilice por completo haciéndolo permitirme la redundancia muy inestable. No se si esto se deba a alguna linea de código que puse de mas o que no e puesto...
Quisiera ver si me pudieran dar alguna guía o algún consejo respecto a este problema que tengo, a como lo veo, no se si sea así, ya solucionando ese problema volaría el Drone a la perfección.
De ante mano muchas gracias por su atención y tiempo.

P.D: Les adjunto el código que estoy utilizando:

#include <PID_v1.h>
#include <Servo.h>
#include <Wire.h>
#include <Kalman.h> 

#define RESTRICT_PITCH

Kalman kalmanX;
Kalman kalmanY;

double Setpointx, Outputx;
double Setpointy, Outputy;

double Inputx, Inputy;

double Kp = 0.5;
double Ki = 0.4;
double Kd = 0.6;

PID myPIDx(&Inputx, &Outputx, &Setpointx, Kp, Ki, Kd, DIRECT);
PID myPIDy(&Inputy, &Outputy, &Setpointy, Kp, Ki, Kd, DIRECT);

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;

//////////////////// POTENCIOMEROS ///////////////////////////////////////
//const int analogInPin1 = A8; 
//const int analogOutPin1 = 4;

//const int analogInPin2 = A9; 
//const int analogOutPin2 = 3;

//int sensorValue1 = 0;        // valor leido desde el potenciometro
//int outputValue1 = 0;        // valor de la salida PWM (salida analógica)

//int sensorValue2 = 0;        // valor leido desde el potenciometro
//int outputValue2 = 0;        // valor de la salida PWM (salida analógica)

//////////////////////////////////////////////////////////////////////////

int cosix;
int cosiy;

int pos   = 0;
int cont  = 0;
int valmedia  = 40;
 
char rxChar;

int val  = 0;
int valx = 0;
int valy = 0;


/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine
void setup() {
 Serial.begin(9600);
 
 Wire.begin();
#if ARDUINO >= 157
 Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
 TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
 i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
 i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
 i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
 while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
 while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

 while (i2cRead(0x75, i2cData, 1));
 if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
   Serial.print(F("Error reading sensor"));
   while (1);
 }

 delay(100); // Wait for sensor to stabilize

 /* Set kalman and gyro starting angle */
 while (i2cRead(0x3B, i2cData, 6));
 accX = (i2cData[0] << 8) | i2cData[1];
 accY = (i2cData[2] << 8) | i2cData[3];
 accZ = (i2cData[4] << 8) | i2cData[5];

#ifdef RESTRICT_PITCH // Eq. 25 and 26
 double roll  = atan2(accY, accZ) * RAD_TO_DEG;
 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
 double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

 kalmanX.setAngle(roll); // Set starting angle
 kalmanY.setAngle(pitch);
 gyroXangle = roll;
 gyroYangle = pitch;
 compAngleX = roll;
 compAngleY = pitch;

 timer = micros();

 // PID x
 Inputx = kalAngleX; //Se pone comovalor el potenciometro para el PID
 
//  Inputx = kalAngleX;

 Setpointx = 0;

 myPIDx.SetMode(AUTOMATIC);
 myPIDx.SetSampleTime(50);
 myPIDx.SetOutputLimits(-20,20);

 // PID y
 Inputy = kalAngleY; //Se pone comovalor el potenciometro para el PID
 
//  Inputy = kalAngleY;

 Setpointy = 0;

 myPIDy.SetMode(AUTOMATIC);
 myPIDy.SetSampleTime(50);
 myPIDy.SetOutputLimits(-20,20);
 
 myservo3.attach(5);
 myservo4.attach(6);
 myservo1.attach(9);
 myservo2.attach(10);

 
}


void Control_Angulo()
{
 /* Update all the values */
 while (i2cRead(0x3B, i2cData, 14));
 accX = ((i2cData[0] << 8) | i2cData[1]);
 accY = ((i2cData[2] << 8) | i2cData[3]);
 accZ = ((i2cData[4] << 8) | i2cData[5]);
 tempRaw = (i2cData[6] << 8) | i2cData[7];
 gyroX = (i2cData[8] << 8) | i2cData[9];
 gyroY = (i2cData[10] << 8) | i2cData[11];
 gyroZ = (i2cData[12] << 8) | i2cData[13];

 double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
 timer = micros();

#ifdef RESTRICT_PITCH // Eq. 25 and 26
 double roll  = atan2(accY, accZ) * RAD_TO_DEG;
 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
 double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

 double gyroXrate = gyroX / 131.0; // Convert to deg/s
 double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
   kalmanX.setAngle(roll);
   compAngleX = roll;
   kalAngleX = roll;
   gyroXangle = roll;
 } else
   kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

 if (abs(kalAngleX) > 90)
   gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
   kalmanY.setAngle(pitch);
   compAngleY = pitch;
   kalAngleY = pitch;
   gyroYangle = pitch;
 } else
   kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

 if (abs(kalAngleY) > 90)
   gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

 gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
 gyroYangle += gyroYrate * dt;
 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
 //gyroYangle += kalmanY.getRate() * dt;

 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

 // Reset the gyro angle when it has drifted too much
 if (gyroXangle < -180 || gyroXangle > 180)
   gyroXangle = kalAngleX;
 if (gyroYangle < -180 || gyroYangle > 180)
   gyroYangle = kalAngleY;      
 
//  Inputx = kalAngleX;
//  Inputy = kalAngleY;

 Inputx = kalAngleX;
 Inputy = kalAngleY;  
 
//  
//  cosix = Inputx;
//  cosiy = Inputy;
//  
//  Inputx = cosix;
//  Inputy = cosiy;
 
 myPIDx.Compute();
 myPIDy.Compute();
}

void Control_Motor()
{
 double cositas1 = val + pos + (+ Outputx - Outputy)/2;
 double cositas2 = val + pos + (+ Outputx + Outputy)/2;
 double cositas3 = val + pos + (- Outputx + Outputy)/2;
 double cositas4 = val + pos + (- Outputx - Outputy)/2;
 
 if(cositas1 <= 47 && cont == 1)
   cositas1 = 47;
 if(cositas2 <= 47 && cont == 1)
   cositas2 = 47;
 if(cositas3 <= 47 && cont == 1)
   cositas3 = 47;
 if(cositas4 <= 47 && cont == 1)
   cositas4 = 47;  
 
 if(cositas1 >= 179)
   cositas1 = 180;
 if(cositas2 >= 179)
   cositas2 = 180;
 
 if(cositas3 >= 179)
   cositas3 = 180;
 if(cositas4 >= 179)
   cositas4 = 180;
   
 myservo1.write(cositas1);
 myservo2.write(cositas2);
 myservo3.write(cositas3);
 myservo4.write(cositas4);  
}
void loop() {    
   
 Control_Angulo();
 
 ////////////////////////// POTENCIOMETROS ////////////////////////////////////////


// // lectura de la entrada analógica
//  sensorValue1 = analogRead(analogInPin1);            
//  // ubica el valor de la entrada en el rango de la salida analógica
//  outputValue1 = map(sensorValue1, 0, 1023, -20, 20);  
//  // cambia el valor de salida analógica
//  analogWrite(analogOutPin1, outputValue1);     
//
//  // lectura de la entrada analógica
//  sensorValue2 = analogRead(analogInPin2);            
//  // ubica el valor de la entrada en el rango de la salida analógica
//  outputValue2 = map(sensorValue2, 0, 1023, -20, 20);  
//  // cambia el valor de salida analógica
//  analogWrite(analogOutPin2, outputValue2);


//////////////////////////////////////////////////////////////////////////////////
 
 // Si el Dron se voltea se apagaranlos motores
 // if(valx >= 50 || valy >= 50 || valx <= -50 || valy <= -50)
 if(cont == 20)
 {
   val  = 20;
   pos  = 0;
   
   myservo1.write(val);
   myservo2.write(val);
   myservo3.write(val);
   myservo4.write(val);
   cont = 0;
 }
 else
 {
   if(Serial.available())
   {
     rxChar = Serial.read();
     switch(rxChar)
     {
       // ENCENDIDO
       case 'O':
       val  = 0;
       val  = valmedia  + 7;

       myservo1.write(val);
       myservo2.write(val);
       myservo3.write(val);
       myservo4.write(val);
      
       cont = 1;
       break;

       // APAGADO
       case 'F':
       val  = 20;
       pos  = 0;
       cont = 0;
       Kp = 1;
       Ki = 1;
       Kd = 1;
      
       myservo1.write(val);
       myservo2.write(val);
       myservo3.write(val);
       myservo4.write(val);
       break;
     
       // ARRIBA
       case 'U':
       pos = pos + 5;
       if(pos > 100)
         pos = 100;
       Control_Motor();
       break;

       // ABAJO
       case 'D':
       pos = pos - 5;
       if(pos < 0)
         pos =0;
       Control_Motor();
       break;
     
       // GIRO DERECHA
       case 'Y':
       myservo1.write(val + pos + 10 - Outputx + Outputy);
       myservo2.write(val + pos - 10 - Outputx - Outputy);
       myservo3.write(val + pos + 10 + Outputx - Outputy);
       myservo4.write(val + pos - 10 + Outputx + Outputy);
       Control_Motor();
       break;

       // GIRO IZQUIRDA
       case 'W':
       myservo1.write(val + pos - 10 - Outputx + Outputy);
       myservo2.write(val + pos + 10 - Outputx - Outputy);
       myservo3.write(val + pos - 10 + Outputx - Outputy);
       myservo4.write(val + pos + 10 + Outputx + Outputy);
       Control_Motor();
       break;
     
       // ADELANTE
       case 'A':
       Setpointx = -17;
       myPIDx.Compute();
       Control_Motor();
       break;

       // ATRAS
       case 'B':
       Setpointx = 17;     
       myPIDx.Compute();
       Control_Motor();
       break;
     
       // IZQUIERDA
       case 'L':
       Setpointy = -17;
       myPIDy.Compute();
       Control_Motor();
       break;

       // DERECHA
       case 'R':
       Setpointy = 17;
       myPIDy.Compute();
       Control_Motor();
       break;
  
       // BP
       case 'Q':
       Serial.println(" ");
       break;
      
      // BP
       case 'Z':
       Kp = Kp + .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;
      // BP
       case 'X':
       Kp = Kp - .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;
      
      // BP
       case 'C':
       Ki = Ki + .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;
      
      // BP
       case 'V':
       Ki = Ki - .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;
  
         // BP
       case 'N':
       Kd = Kd + .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;
      
      // BP
       case 'M':
       Kd = Kd - .25;
       myPIDx.Compute();
       myPIDy.Compute();
       break;     
     }
   }     
 }
 
 if(cont == 1)
 {
   Setpointx = 0;
   Setpointy = 0;
   Control_Motor();
   myPIDx.Compute();
   myPIDy.Compute();    
 }

 
//  Serial.print("Kp : ");
//  Serial.print(cosa1);
//  Serial.print(" ");
//  
//  Serial.print("Ki : ");
//  Serial.print(cosa2);
//  Serial.print("  ");
//  
//  Serial.print("KD : ");
//  Serial.print(cosa3);
//  Serial.print("\t\t");
 
//  Serial.print("Input x: ");
//  Serial.print(Inputx);
//  Serial.print("\t");
//  
//  Serial.print("Input y: ");
//  Serial.print(Inputy);
//  Serial.print(" ----- \t");
 
 Serial.print("Angulo X = " );    
 Serial.print(kalAngleX);  
 Serial.print("   ");
 Serial.print("Angulo Y = " );    
 Serial.print(kalAngleY);
 Serial.print("   ");
 
 Serial.print(val + pos + (+ Outputx - Outputy)/2);
 Serial.print("   ");
 Serial.print(val + pos + (+ Outputx + Outputy)/2);
 Serial.print("   ");
 Serial.print(val + pos + (- Outputx + Outputy)/2);
 Serial.print("   ");
 Serial.print(val + pos + (- Outputx - Outputy)/2);
 Serial.print("   \t");

 Serial.print("Output x: ");
 Serial.print(Outputx);
 Serial.print("  \t");
 
 Serial.print("Output y: ");
 Serial.print(Outputy);
 Serial.println(" ");
 
 

}

Lo siento por tantos cuadros de respuesta para poder poner el código pero es la primera ves que realizo uno y no había como poner el código entero, de nuevo gracias por la atención y el tiempo.

Hola estoy intentando hacer un drone, mi pregunat es, has conseguido con este codigo que funcione, lo estube mirando el codigo que tienes aqui pero no me aclaro