Mpu 9250 stabilization code arduino

.ino

#include <Wire.h>
#include <Servo.h>
#include<Filters.h>
//#include <SoftwareSerial.h>
#define SerialPort Serial
#include <SparkFunMPU9250-DMP.h>
MPU9250_DMP imu;
double roll , pitch, yaw;
float phi = 0;
float theta = 0;
float psi = 0;
long int pre_ts = 0;

//Servo intialization

Servo Servo1;
Servo Servo2;
Servo Servo3;
int roll_servo;
int pitch_servo;
int yaw_servo;

//RC intialization

const int chA = 8;
const int chB = 9;
const int chC = 10;
const int chD = 11;

int ch1;
int ch2;
int ch3;
int ch4;

// filter initialization
float cutoff_frequency = 100;
float alpha_filter;
float RC_filter;
long int dt;
float previous_roll;
float previous_pitch;
float previous_yaw;

void setup()
{
  Servo1.attach(5);
  Servo2.attach(6);
  Servo3.attach(7);

  {
    SerialPort.begin(115200);

    if (imu.begin() != INV_SUCCESS)
    {
      while (1)
      {
        SerialPort.println("Unable to communicate with MPU-9250");
        SerialPort.println("Check connections, and try again.");
        SerialPort.println();
        delay(3000);
      }
    }


    imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);


    imu.setGyroFSR(250); // Set gyro to 2000 dps
    // Accel options are +/- 2, 4, 8, or 16 g
    imu.setAccelFSR(2); // Set accel to +/-2g
    imu.setLPF(5); // Set LPF corner frequency to 5Hz
    imu.setSampleRate(10); // Set sample rate to 10Hz
    imu.setCompassSampleRate(10); // Set mag rate to 10Hz
  }
   pre_ts = millis();

  // rc pinmodes
  pinMode (8, INPUT);
  pinMode(9, INPUT);
  pinMode(10, INPUT);
  pinMode(11, INPUT);
 
}

void loop()
{
  if ( imu.dataReady() )
  {
    imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
    printIMUData(millis() - pre_ts);
    pre_ts = millis();
  }
}

void printIMUData( long int dt)
{

  float accelX = imu.calcAccel(imu.ax);
  float accelY = imu.calcAccel(imu.ay);
  float accelZ = imu.calcAccel(imu.az);
  float gyroX = imu.calcGyro(imu.gx);
  float gyroY = imu.calcGyro(imu.gy);
  float gyroZ = imu.calcGyro(imu.gz);
  float magX = imu.calcMag(imu.mx);
  float magY = imu.calcMag(imu.my);
  float magZ = imu.calcMag(imu.mz);

  pitch = atan2 (accelY , ( sqrt ((accelX * accelX) + (accelZ * accelZ))));
  roll = atan2(-accelX , ( sqrt((accelY * accelY) + (accelZ * accelZ))));

  // yaw from mag

  float Yh = (magY * cos(roll)) - (magZ * sin(roll));
  float Xh = (magX * cos(pitch)) + (magY * sin(roll) * sin(pitch)) + (magZ * cos(roll) * sin(pitch));

  yaw =  atan2(Yh, Xh);

// filter angles
 RC_filter = 1/(2*3.14 * cutoff_frequency);
 alpha_filter = (dt / (dt + 1000* RC_filter));

 float filtered_roll = ((alpha_filter * roll)+((1-alpha_filter)*previous_roll));
 float previous_roll = filtered_roll;

 //filter pitch

  float filtered_pitch = ((alpha_filter * pitch)+((1-alpha_filter)*previous_pitch));
 float previous_pitch = filtered_pitch;

 //filter yaw

 float filtered_yaw = ((alpha_filter * yaw)+((1-alpha_filter)*previous_yaw));
 float previous_yaw = filtered_yaw;


  filtered_roll = filtered_roll * 57.3;
  filtered_pitch = filtered_pitch * 57.3;
  filtered_yaw = filtered_yaw * 57.3;

  Serial.println(String(dt/1000.0)+','+String(filtered_roll)+','+String( filtered_pitch)+','+String( filtered_yaw)+','+String(filtered_roll)+','+String(filtered_pitch)+','+String(filtered_yaw));

 {
    //switch the stabilize mode and control mode

  ch4 = pulseIn(11, HIGH, 25000);
    
     if
    (ch4 <= 1500)
     
    { 
        // stabilize mode
           
      filtered_roll = map(filtered_roll, -90, 90, 0, 180);
      filtered_roll = constrain(filtered_roll, 0,180 );
       Servo1.write(filtered_roll );
    
      filtered_pitch = map(filtered_pitch, -90, 90, 0, 180); 
      filtered_pitch = constrain(filtered_pitch, 0,180 );
      Servo2.write(filtered_pitch  );
         
       filtered_yaw = map(filtered_yaw, -90, 90, 0, 160);
       filtered_yaw = constrain(filtered_yaw, 60, 110);    //constrain yaw as per you requirement usually 0 to 180 degree)
       Servo3.write(filtered_yaw  );
    
    }
               
                // control mode
else

{
  
  {
      filtered_roll = map(filtered_roll, -90, 90, 0, 180);
     
      filtered_roll = constrain(filtered_roll, 0,180 );
       Servo1.write(filtered_roll );
   

      filtered_pitch = map(filtered_pitch, -90, 90, 0, 180); 
    
      filtered_pitch = constrain(filtered_pitch, 0,180 );
      Servo2.write(filtered_pitch  );

         
     filtered_yaw = map(filtered_yaw, -90, 90, 0, 160);
      filtered_yaw = constrain(filtered_yaw, 60, 110);
      
      Servo3.write(filtered_yaw  );
    
    }
   
      
   ch1 = pulseIn (8, HIGH,25000);
   ch1 = map(ch1, 1000,2000,-90,90);
   Servo1.write(ch1+ filtered_roll);

   ch2 = pulseIn (9, HIGH,25000);
   ch2 = map(ch2, 1000,2000,-90,90);
   Servo2.write(ch2 + filtered_pitch);

   ch3 = pulseIn (10, HIGH,25000);
   ch3 = map(ch3, 1000,2000,0,180);
  Servo3.write(ch3 + filtered_yaw);

     }
    }
  
  
}