I don't know what I've done wrong but the PID stabilization doesn't work

So after a while I finally got my MPU6050 working and I've been getting accelerometer and gyroscope values, but I've faced more problems. I've been working on software that uses the Madgwick filter library and a PID controller to stabilize error. Originally I was using the Madgwick filter in the Drehm Flight VTOL code however that wasn't working so I re-wrote my code using the Madgwick library. Finally I'm getting IMU, pitch, yaw, roll and PID values and it's writing to the servo but it isn't working correctly. The orientation data is significantly inaccurate, stops working suddenly and will stop writing to the servo, as well as stopping even measuring these values, there is also significant lag. I don't know how to fix the many problems I'm encountering (partly because I don't know what's causing most of them) and would love some help. I am only 13 so I am not particularly experienced and probably overlooked/ignored something somewhere.

#include <MadgwickAHRS.h>
#include <basicMPU6050.h> 
#include <Wire.h>
#include <Servo.h> 


// Create instance
basicMPU6050<> imu;
Madgwick filter;
//sensor rate fixed at 1 kHz.
float sensorRate = 1000.0; 
float im;
float ACCEL_SCALE_FACTOR = 16384.0;
float GYRO_SCALE_FACTOR = 131.0;

float B_madgwick = 0.4; //Madgwick filter parameter
float B_accel = 0.14; //Accelerometer filter parameter
float B_gyro = 0.1; //Gyroscope filter parameter



float Kp_roll_rate = 1; //Roll P-gain
float Ki_roll_rate = 1; //Roll I-gain
float Kd_roll_rate = 1; //Roll D-gain

float Kp_pitch = 1; //Pitch P-gain
float Ki_pitch = 1; //Pitch I-gain
float Kd_pitch = 1; //Pitch D-gain 

float Kp_yaw = 1; //Yaw P-gain 
float Ki_yaw = 1; //Yaw i-gain 
float Kd_yaw = 1; //Yaw D-gain



Servo servoX; //X axis servomotr
Servo servoY; //Y axis servomotor
Servo ESC; //the esc for the engine

float dt; 
unsigned long current_time, prev_time; 
unsigned long print_counter, serial_counter;

//IMU 
float AccX, AccY, AccZ; 
float AccX_prev, AccY_prev, AccZ_prev; 
float GyroX, GyroY, GyroZ;
float GyroX_prev, GyroY_prev, GyroZ_prev; 
float roll_IMU, pitch_IMU, yaw_IMU; 
float roll_IMU_prev; 
float pitch_IMU_prev, yaw_IMU_prev;
float AccErrorX, AccErrorY, AccErrorZ, GyroErrorX, GyroErrorY, GyroErrorZ;
float pitch, yaw, roll;

float AcX, AcY, AcZ;
float GyX, GyY, GyZ;

float thro, roll_des, pitch_des, yaw_des; 

float error_roll, error_roll_prev, integral_roll, integral_roll_prev, integral_roll_il, integral_roll_ol, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, derivative_roll_prev, PID_roll = 0; 
float error_pitch, error_pitch_prev, integral_pitch, integral_pitch_prev, integral_pitch_il, integral_pitch_ol, integral_pitch_pev_il, integral_pitch_prev_ol, derivative_pitch, derivative_pitch_prev, PID_pitch = 0;
float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, integral_yaw_il, integral_yaw_ol, integral_yaw_prev_il, integral_yaw_prev_ol, derivative_yaw, derivative_yaw_prev, PID_yaw = 0;
float prop_pitch, prop_yaw, prop_roll;




void setup() {
  // Set registers - Always required
  imu.setup();

  // Initial calibration of gyro
  imu.setBias();

  // Start console
  Serial.begin(38400);

  servoX.attach(12);
  servoY.attach(11);



  delay(10);

  servoX.write(0);
  servoY.write(0);

  delay(100);

  

  

}

void loop() { 
  // Update gyro calibration 
  calibrateAttitude();
  
  imu.updateBias();
 
  //-- Scaled and calibrated output:
  // Accel
  /*Serial.print( imu.ax() );
  Serial.print( " " );
  Serial.print( imu.ay() );
  Serial.print( " " );
  Serial.print( imu.az() );
  Serial.print( "    " );
  
  // Gyro
  Serial.print( imu.gx() );
  Serial.print( " " );
  Serial.print( imu.gy() );
  Serial.print( " " );
  Serial.print( imu.gz() );
  Serial.print( "    " );  
  
  // Temp
  Serial.print( imu.temp() );
  Serial.println();
  */
  
  
  
  
 
  
 

  

  
  
  
}
void getIMUdata() {
  //retrieves IMU data

  imu.updateBias();
  
  float accelX = imu.ax();
  float accelY = imu.ay();
  float accelZ = imu.az();

  //Serial.println(accelX);

  

  //corrects outputs by subracting from error value
  //acceX = accelX - AccErrorX; 
  //AccY = AccY - AccErrorY; 
  //AccZ = AccZ - AccErrorZ;
  //Low-pass filter applied to accelerometer data
  /*accelX = (1.0 - B_accel) * AccX_prev + B_accel * accelX;
  accelY = (1.0 - B_accel) * AccY_prev + B_accel * accelY;
  accelZ = (1.0 - B_accel) * AccZ_prev + B_accel * accelZ;
  AccX_prev = accelX;
  AccY_prev = accelY;
  AccZ_prev = accelZ;
  */
  
 
  
  //Gyro values
  float GyroX = imu.gx();
  float GyroY = imu.gy();
  float GyroZ = imu.gz(); 

  

  //Serial.print(imu.gx());
  //Serial.print(GyroX);

  /*GyroX = GyroX / GYRO_SCALE_FACTOR; // converts to degrees per second
  GyroY =  GyroY / GYRO_SCALE_FACTOR;
  GyroZ = GyroZ / GYRO_SCALE_FACTOR;
  */

  //Gyro error correct values
  //GyroX = GyroX - GyroErrorX;
  //GyroX = GyroX - GyroErrorX;
 // GyroX = GyroX - GyroErrorX;
  //Low-Pass filter for Gyro data
  /*GyroX = (1.0 - B_gyro) * GyroX_prev + B_gyro * GyroX;
  GyroY = (1.0 - B_gyro) * GyroY_prev + B_gyro * GyroY;
  GyroZ = (1.0 - B_gyro) * GyroZ_prev + B_gyro * GyroZ;
  GyroX_prev = GyroX;
  GyroY_prev = GyroY;
  GyroZ_prev = GyroZ; */

  //normalizes acceleration
  float ax = convertRawAcceleration(accelX);
  float ay = convertRawAcceleration(accelY);
  float az = convertRawAcceleration(accelZ);
  float gx = convertRawGyro(GyroX);
  float gy = convertRawGyro(GyroY);
  float gz = convertRawGyro(GyroZ);


  
  filter.updateIMU(ax, ay, az, gx, gy, gz);

  //prints values.
  roll = filter.getRoll();
  pitch = filter.getPitch();
  yaw = filter.getYaw();

  Serial.println(pitch);
  //Serial.println(pitch);
  PID();

 





}

void calibrateAttitude() {
    prev_time = current_time;
    current_time = micros();
    dt = (current_time - prev_time) / 1000000.0; //change in microseconds
    getIMUdata();
    //loopRate(2000);
    //motion();

    
  
}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
  
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}







void getDesiredStates() {
  thro = 0;
  roll_des = 0;
  pitch_des = 0;
  yaw_des = 0;

}

void PID() { //My PID logic controller.
  //Pitch PID calculation
  error_pitch_prev = error_pitch;
  error_pitch = pitch_des - pitch;
  



  prop_pitch = Kp_pitch * error_pitch; //proportional term fine.

  float totalerror_pitch;
  totalerror_pitch += error_pitch;

  integral_pitch = Ki_pitch * ((totalerror_pitch) * dt); //Integral term needs revision

  derivative_pitch = Kd_pitch * ((error_pitch - error_pitch_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?

  PID_pitch = prop_pitch + integral_pitch + derivative_pitch;


  //Roll PID calculation
  error_roll_prev = error_roll;
  error_roll = roll_des - roll;

  float totalerror_roll;
  totalerror_roll += error_roll;


  prop_roll = Kp_roll_rate * error_roll; //proportional term fine.

  integral_roll = Ki_roll_rate * ((totalerror_roll) * dt); //Integral term needs revision

  derivative_roll = Kd_roll_rate * ((error_roll - error_roll_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?

  PID_roll = prop_roll + integral_roll + derivative_roll;

  //Yaw PID calculation
  error_yaw_prev = error_yaw;
  error_yaw = yaw_des - yaw;

  float totalerror_yaw;
  totalerror_yaw += error_yaw;


  prop_yaw = Kp_yaw * error_yaw; //proportional term fine.

  integral_yaw = Ki_yaw * ((totalerror_yaw) * dt); //Integral term needs revision

  derivative_yaw = Kd_yaw * ((error_yaw - error_yaw_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?

  PID_yaw = prop_yaw + integral_yaw + derivative_yaw;

  //servoX.write(PID_pitch);
  //Serial.println(PID_pitch);

  
 
  

}




float invSqrt(float x) {
  //Fast inverse sqrt for madgwick filter
  /*
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  y = y * (1.5f - (halfx * y * y));
  return y;
  */
  //alternate form:
  unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
  float tmp = *(float*)&i;
  float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
  return y;
}

Your post was MOVED to its current location as it is more suitable.

Variables declared locally without a value are undefined and vanish when the function returns.
I think you meant:

  static float totalerror_pitch = 0.0;
  totalerror_pitch += error_pitch;

ditto for the other similar variables in PID()