Slow Serial printing of data when using complementary filter.

This is the code that i wrote to get the yaw,pitch and roll values of my quadcopter.The values are okay but the printing of values in the serial monitor is slow .Like if an angle 20 degrees is made,the serial printing gradually increases from 1,9,15 and then finally 20 and becomes stable.Current i am using baud rate of 250000 and it seems okay but when using lower baud rate , the problem reappears.Help.


double accelX,accelY,accelZ,temperature,gyroX,gyroY,gyroZ,gyro_x_cal,gyro_y_cal,gyro_z_cal,accel_x_cal,accel_y_cal,accel_z_cal; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int.  We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double roll, pitch ,yaw; //These are the angles in the complementary filter
float rollangle,pitchangle;
int cal_int;
void setup() {
  // Set up MPU 6050:
  TWBR = 12;   
  Serial.println("Calibrating Accelerometer & Gyroscope......");
   gyro_x_cal += gyroX;
   gyro_y_cal  += gyroY ;
   gyro_z_cal += gyroZ;

   accel_x_cal +=accelX;
   accel_y_cal +=accelY;
   accel_z_cal +=accelZ;
  Serial.println("Calibration Done..!!!");
  gyro_x_cal /= 2000;
  gyro_y_cal /= 2000;
  gyro_z_cal /= 2000;

  accel_x_cal /= 2000;
  accel_y_cal /= 2000;
  accel_z_cal /= 2000;
  //start a timer
  timer = micros();

void loop() {
//Now begins the main loop. 
  //Collect raw data from the sensor.
  gyroX = gyroX / 65.5;
  gyroY = gyroY / 65.5;
  gyroZ = gyroZ / 65.5;

  accelX = accelX / 4096.0;
  accelY = accelY / 4096.0; 
  accelZ= accelZ / 4096.0;
  double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
  timer = micros(); //start the timer again so that we can calculate the next dt.

  //the next two lines calculate the orientation of the accelerometer relative to the earth and convert the output of atan2 from radians to degrees
  //We will use this data to correct any cumulative errors in the orientation that the gyroscope develops.
  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET


  //This filter calculates the angle based MOSTLY on integrating the angular velocity to an angular displacement.
  //dt, recall, is the time between gathering data from the MPU6050.  We'll pretend that the 
  //angular velocity has remained constant over the time dt, and multiply angular velocity by 
  //time to get displacement.
  //The filter then adds a small correcting factor from the accelerometer ("roll" or "pitch"), so the gyroscope knows which way is down. 
  roll = 0.85 * (roll+ gyroX * dt) + 0.15 * rollangle; // Calculate the angle using a Complimentary filter
  pitch = 0.85 * (pitch + gyroY * dt) + 0.15 * pitchangle; 
  Serial.print("roll  ");
  Serial.print("   pitch  ");
  Serial.print("   yaw    ");


void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x08); //Setting the gyro to full scale +/- 500deg./s 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0x10); //Setting the accel to +/- 8g
  Wire.beginTransmission(0b1101000);                                      //Start communication with the address found during search
  Wire.write(0x1A);                                                          //We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  Wire.endTransmission();                                                    //End the transmission with the gyro    
void recordRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.requestFrom(0b1101000,14); //Request Accel Registers (3B - 40)
  while(Wire.available() < 14);
  accelX =<<8|; //Store first two bytes into accelX
  accelY =<<8|; //Store middle two bytes into accelY
  accelZ =<<8|; //Store last two bytes into accelZ<<8|;
  gyroX =<<8|; //Store first two bytes into accelX
  gyroY =<<8|; //Store middle two bytes into accelY
  gyroZ =<<8|; //Store last two bytes into accelZ

  if(cal_int == 2000)
    gyroX -= gyro_x_cal;
    gyroY -= gyro_y_cal;
    gyroZ -= gyro_z_cal;

    accelX -=accel_x_cal;
    accelY -=accel_y_cal;
    accelZ -=accel_z_cal;

Not sure if it's the issue (it would be at low baudrates) but there is a software buffer in the HardwareSerial code that you possibly fill faster than it can be emptied (transmitted); once the buffer is full, the code will block till there is space before it will place the next byte in the software buffer and continue and hence slow down your code.

Measure how long your recordRegisters() function take

void setup()
  // existing code

  unsigned long startTime = millis();
  Serial.println(millis() - startTime);

void loop()

That should give you an idea. If recordRegisters is slower than the time it takes to send the data, it's not the problem; else it's the problem. You can calculate the time how long it takes to send the data (10 bits per byte at 250000 kBits/s).