I am trying to build a self balancing inverted pendulum robot. The problems I am encountering are related to the vibrations that are transferred to the MPU6050 which measures the robot’s angle. I have made sure that the sensor is mounted properly and have compensated for the mounting error and I also calibrated it on each axis beforehand. My problem is that the robot almost balances itself but isn’t quite there and I am stumped. No matter how high I set the Kp it just won’t sit upright on its own.
To sum it up:

1. I believe that my choice of filter is not exactly appropriate. I do not know if I should use a Kalman filter (I am worried about the implementation here as I am not skilled enough to make a fast one), Low pass filter or keep my Exponential filter.
2. Have I made any blunders in the code? I need it to be as fast as possible. Is my approach of using a library to control the motors what is killing the whole deal? It is my first time writing something this complex.

Thank you for taking your time to read this.

#include <Adafruit_MPU6050.h>
#include <Wire.h>
#include <AccelStepper.h>

// Create an instance of the MPU6050 sensor
Adafruit_MPU6050 mpu;

// Define the I2C address of the MPU6050
int MPU_addr = 0x68;

// Pins for controlling stepper motors
#define dirPin_X 2
#define stepPin_X 5
#define dirPin_Y 3
#define stepPin_Y 6
#define enablePin 8

// Variable needed for timing the calibration procedure in the setup
uint32_t LoopTimer;

// PID control variables
double Kp = 20;
double Ki = 0;
double Kd = 0;
double P = 0;
double I = 0;
double D = 0;
double prevError = 0;
double Setpoint = 0;
double err_in_PID;
double PID_output = 0;


//Needed as a global variable due to being in the setup function
const int maxSpeed = 2000;

// Variables for storing sensor data and sensor calibration data
// Needed as global variables due to being used in the setup function for the dynamic calibration
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
float RateRoll, RateYaw, RatePitch;
int RateCalibrationNumber;

// Create instances of the AccelStepper library for controlling motors
AccelStepper stepper1(1, stepPin_X, dirPin_X);
AccelStepper stepper2(1, stepPin_Y, dirPin_Y);

// Filter parameters for smoothing angle
// Needed as a global variable due to being able to be updated from the Serial Monitor
const float alpha_angle_filter = 0.5;

// Filtered angle values
// Needed as a global variable due to being used in determining the motor control logic
float filteredAnglePitch = 0.0;

// Flag to indicate if PID parameters have changed
bool PID_parameters_changed = false;

// Function to read gyro and accelerometer signals
void gyro_signals() {

// Sensitivity for ACC and GYRO
  const float GYRO_SENSITIVITY = 65.5;
  const float ACC_SENSITIVITY = 4096.0;

// Calibration values for accelerometer
// Calibration was not needed for X and Y axis
const float const_calib_Z = 0.2;

// Angle Correction needed due to the offset originating from the mounting procedure
const float angle_correction = 0.5;

  // Read accelerometer data
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(MPU_addr, 6);

  int16_t AccXLSB = (Wire.read() << 8) | Wire.read();
  int16_t AccYLSB = (Wire.read() << 8) | Wire.read();
  int16_t AccZLSB = (Wire.read() << 8) | Wire.read();

  // Read gyroscope data
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(MPU_addr, 6);

  int16_t GyroX = (Wire.read() << 8) | Wire.read();
  int16_t GyroY = (Wire.read() << 8) | Wire.read();
  int16_t GyroZ = (Wire.read() << 8) | Wire.read();

  // Convert raw data to useful values
  RateRoll = GyroX / GYRO_SENSITIVITY;
  RatePitch = GyroY / GYRO_SENSITIVITY;
  RateYaw = GyroZ / GYRO_SENSITIVITY;

  float AccX = AccXLSB / ACC_SENSITIVITY;
  float AccY = AccYLSB / ACC_SENSITIVITY;
  float AccZ = (AccZLSB / ACC_SENSITIVITY) - const_calib_Z; 

  // Calculate Pitch angle (degrees)
  float AccMagnitude = sqrt(AccX * AccX + AccY * AccY);
  float AnglePitch = (atan(AccZ / AccMagnitude) * (180.0 / PI)) - angle_correction;

  // Apply exponential filter
  filteredAnglePitch = (alpha_angle_filter * AnglePitch + (1 - alpha_angle_filter) * filteredAnglePitch);

  //    Angle Acquisition DIAGNOSTICS   //

//Serial.print(Setpoint);
//Serial.print(",");
//Serial.println(F("err_in_PID"));
//Serial.println(err_in_PID);
//Serial.println(F("AnglePitch"));
//Serial.println(AnglePitch);
//Serial.println(F("filteredAnglePitch"));
//Serial.println(filteredAnglePitch);
 
}

// Function to move the robot based on PID output
void moveRobot() {

  const int minSpeed = 50;
  int motorSpeed1, motorSpeed2;

  // Adjust motor speeds based on pitch angle and PID output
  if (filteredAnglePitch > 0) {

    motorSpeed1 = (minSpeed + PID_output);
    motorSpeed2 = -(minSpeed + PID_output);
  } else {
    motorSpeed1 = -(minSpeed + PID_output);
    motorSpeed2 = (minSpeed + PID_output);
  }

  // Set motor speeds
  stepper1.setSpeed(motorSpeed1);
  stepper2.setSpeed(motorSpeed2);

  // Move motors
  stepper1.runSpeed();
  stepper2.runSpeed();

  //DEBUG AREA

//Serial.println(F("motorSpeed1"));
//Serial.println(motorSpeed1);
//Serial.println(F("motorSpeed2"));
//Serial.println(motorSpeed2);
}

// Function to compute PID output
int PID(double err_in_PID) {

  const int max_I = 255;
  // Proportional term
  P = Kp * err_in_PID;

  // Integral term
  I += Ki * err_in_PID;
  if (I > max_I) {
    I = max_I;
  } else if (I < -max_I) {
    I = -max_I;
  }

  // Derivative term
  D = Kd * (err_in_PID - prevError);
  prevError = err_in_PID;

  int speed = P + I + D;

  if (speed > maxSpeed) {
    speed = maxSpeed;
  } else if (speed < -maxSpeed) {
    speed = -maxSpeed;
  }
  return speed;

//          PID  DIAGNOSTICS       //
//Serial.println(F("P"));
//Serial.println(P);
//Serial.println(F("I"));
//Serial.println(I);
//Serial.println(F("D"));
//Serial.println(D);
//Serial.println(F("PID_output"));
//Serial.println(PID_output);


}

// Function to update PID values from Serial input
void updatePIDValues() {
  if (Serial.available() > 0) {
    String inputString = Serial.readStringUntil('n');
    inputString.trim();

    if (inputString.startsWith("Kp=")) {
      Kp = inputString.substring(3).toFloat();
    } else if (inputString.startsWith("Ki=")) {
      Ki = inputString.substring(3).toFloat();
    } else if (inputString.startsWith("Kd=")) {
      Kd = inputString.substring(3).toFloat();
    } 

    // Print updated PID values
    Serial.println("Updated PID Values:");
    Serial.print("Kp: ");
    Serial.println(Kp);
    Serial.print("Ki: ");
    Serial.println(Ki);
    Serial.print("Kd: ");
    Serial.println(Kd);
  }
}

// Setup function
void setup() {
  // Initialize Serial communication
  Serial.begin(115200);

  // Initialize MPU6050 sensor
  if (!mpu.begin()) {
    digitalWrite(LED_RED, LOW); // Turns on red led to show that there is an issue
    Serial.println("Sensor init failed");
    while (1)
      yield();
  }
  Serial.println("Found a MPU6050 sensor");

  // Initialize I2C communication
  Wire.begin();

  // Setup MPU6050 configuration
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();

  // Calibrate gyro
  for (RateCalibrationNumber = 0; RateCalibrationNumber < 1000; RateCalibrationNumber++) {
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(1);
  }
  RateCalibrationRoll /= 1000;
  RateCalibrationPitch /= 1000;
  RateCalibrationYaw /= 1000;
  LoopTimer = micros();

  // Set CNC shield enable pin
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin, LOW);

  // Set motor pins
  pinMode(stepPin_X, OUTPUT);
  pinMode(dirPin_X, OUTPUT);
  pinMode(stepPin_Y, OUTPUT);
  pinMode(dirPin_Y, OUTPUT);

  // Set maximum motor speed
  stepper1.setMaxSpeed(maxSpeed);
  stepper2.setMaxSpeed(maxSpeed);

  pinMode(LED_BUILTIN, OUTPUT);

}

// Main loop
void loop() {

  //digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  //digitalWrite(LED_BLUE, LOW);

  unsigned int loopTimer = millis();
  static int loopInterval = 3000;
  
  // Update PID parameters from Serial input
  if (Serial.available() > 0) {
    updatePIDValues();
    PID_parameters_changed = true;
  }

  // If PID parameters have changed, update the PID controller
  if (PID_parameters_changed) {
    PID_parameters_changed = false;
  }


  if (loopTimer- loopInterval > 1)
  {
    // Read gyro and accelerometer signals
    gyro_signals();

  RateRoll-=  RateCalibrationRoll;
  RatePitch-= RateCalibrationPitch;
  RateYaw-=   RateCalibrationYaw;

    // Calculate error in PID
    err_in_PID = abs(filteredAnglePitch - Setpoint);

    // Compute PID output
    PID_output = PID(err_in_PID);

    // Move the robot based on PID output
    moveRobot();

    loopTimer = 0;
  }

}

I tried tuning the PID but to no avail. No matter how high the Kp I set it simply did not want to balance on itself. The chassis is light enough for the motors to balance it. I believe that the software is lacking. I have also experimented with various filters but the results were rather unsatisfying. The board I am using is the ESP32 Nano, with 240 MHz.