Problem with PID can't tune it right ( quadcopter )

Hi guys i’m building a quadcopter for an university exam.
i’m useing the PID_v1 library but can’t get it perfectly stable. Could you suggest me something :stuck_out_tongue_closed_eyes:
That’s the video: - YouTube
Thats the graph plot : http://i58.tinypic.com/e7meyh.png
And the code:

/*
13/06/2014 P 2.0 , I 0.0 , D 0.4 stabile a 700 abbastanza stabile a 800
14/06/2014 P 1.9 , I 0.5 , D 0.3 eliminato con la I la non orizzontalità
15/06/2014 P 1.9 , I 0.6 , D 0.3 piùo meno come sopra
*/
#include <PID_v1.h>
#include <Servo.h>
#include <math.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

Servo M1, M2, M3, M4;
// Accelerometro gyro
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float ax1,ay1,az1,gx1,gy1,gz1;
// END - Accelerometro Gyro
// Valori PID
float Setpoint=0.0, Input, Output;
int VM1 = 0, VM2 = 0, VM3 = 0, VM4 = 0;
PID yPID(&Input, &Output, &Setpoint,1.9,0.6,0.3, DIRECT);
// END - Valori PID
//  Kalman filter 
float pitch=0;
int delta_t = 0,timer;
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03
float x_bias = 0;
float x_angle = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float  y, S;
float K_0, K_1;
// END - kalman
void setup()
{
  M1.attach(7);    // CCW4
  M1.write(0);
  delay(500);
  M1.writeMicroseconds(590);
  delay(500);
  M1.writeMicroseconds(0);
  M2.attach(5);    // CCW4
  M2.write(0);
  delay(500);
  M2.writeMicroseconds(590);
  delay(500);
  M2.writeMicroseconds(0);
  M3.attach(3);    // CCW4
  M3.write(0);
  delay(500);
  M3.writeMicroseconds(590);
  delay(500);
  M3.writeMicroseconds(0);
  M4.attach(2);    // CCW4
  M4.write(0);
  delay(500);
  M4.writeMicroseconds(590);
  delay(500);
  M4.writeMicroseconds(0);
  delay(3000);
  Serial.begin(57600);
  Wire.begin();
  accelgyro.initialize();
  yPID.SetMode(AUTOMATIC);
  yPID.SetOutputLimits(-100, 100);
  timer = micros();
}

void loop()
{
  delta_t = micros() - timer;
  timer=micros();
  ax1=0,ay1=0,az1=0,gx1=0,gy1=0,gz1=0;
  for(int i=0;i<10;i++)
  {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     
    ax1 += ax / 16.384;                                
    ay1 += ay / 16.384;                                
    az1 += az / 16.384;
    gx1 += gx / 131.00;
    gy1 += gy / 131.00;
    gz1 += gz / 131.00;   
  }
  ax1 /= 100;
  ay1 /= 100;
  az1 /= 100;
  gx1 /= 100;
  gy1 /= 100;
  gz1 /= 100;
    
  pitch = (atan2(ay1,sqrt(ax1*ax1+az1*az1)))*RAD_TO_DEG;  
  Input = kalmanCalculate(pitch,gx1,delta_t);
  yPID.Compute();  // calcolo PID Y
  calc_motori();
  update_motori();
  //Serial.print("Angolo: ");Serial.print(Input);
  //Serial.print("PID: ");Serial.println(Output);
  
}

float kalmanCalculate(float newAngle, float newRate,int looptime)
{
  float dt = float(looptime)/1000;
  x_angle += dt * (newRate - x_bias);
  P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
  P_01 +=  - dt * P_11;
  P_10 +=  - dt * P_11;
  P_11 +=  + Q_gyro * dt;

  y = newAngle - x_angle;  
  S = P_00 + R_angle;
  K_0 = P_00 / S;
  K_1 = P_10 / S;

  x_angle +=  K_0 * y;
  x_bias  +=  K_1 * y;
  P_00 -= K_0 * P_00;
  P_01 -= K_0 * P_01;
  P_10 -= K_1 * P_00;
  P_11 -= K_1 * P_01;

  return x_angle;
}

void calc_motori()
{
  VM1 = ((Output/2)+801);
  VM2 = ((-Output/2)+800);
      //VM1 = ((100+Output)/100)*800;
      //VM3 = ((100+Output)/100)*800;  
      //VM2 = (abs((-100+Output)/100))*800;
      //VM4 = (abs((-100+Output)/100))*800;
}
void update_motori()
{
  M1.writeMicroseconds(VM1);
  M2.writeMicroseconds(VM2);
  Serial.println(Input);
  Serial.println(Output);
  //Serial.print("Motore1: ");Serial.print(VM1);
  //Serial.print(" Motore2: ");Serial.print(VM2);
  //Serial.print(" dt: ");Serial.print(delta_t/1000);Serial.print(" ms: ");
  //Serial.print(" Angolo: ");Serial.println(Input);
  M3.writeMicroseconds(0);
  M4.writeMicroseconds(0);
}

That's not bad actually - residual noise will always limit the performance, and
you have fairly cluttered air-flow in your test set-up - try it further from the floor?

Have you done anything special with the ESCs? - some can be reprogrammed to
give tighter response I believe.

Actualy i haven't, because i can't figure out how to program Afro ESC, i've the usb programmer but it's so messy :confused:
If you look at the code i get 10 times the MPU6050 values and calculate the average but sometimes it output a wrong value anyway. is there any way to avoid random wrong reads from the IMU ?

hi guys, i tried to test the PID at 1 meter from the floor but can’t get the quad responsive … i mean , it’s stable but when i touch it it start to oscillate
check out the video http://youtu.be/D8QQK5q5dv0

Oscillation means P or I gain too high, typically. Do you have a D term?

yes i do have a D value, P = 1.0 , I = 0.34 , D = 0.30

that's the values result . i also switched from UDOO to arduino uno due to the UDOO weight .
But the PID response is slow , what should i change ?

If the actual componemts in the loop are slow to respond, the PID loop has
also to be slow or its unstable. You said your ESC wasn't re-programmed?

no, i haven't reprogrammed the ESCs . they are Afro ESC 30A with Simonk firmware. and i've this http://www.hobbyking.com/hobbyking/store/__39437__Afro_ESC_USB_Programming_Tool.html
but don't know how to program them.