Как реализовать компас с компенсацией наклона?

Поэтому я пытался реализовать компас с компенсацией наклона, используя HMC5883L и MPU6050. Проблема, с которой я сталкиваюсь, заключается в том, что существует 2 уравнения компенсации наклона

Компенсация наклона X: X,Y,Z-это выход компаса (необработанный выход).

new_X = X*cos(pitch) + Y*sin(roll)*sin(pitch) – Z*cos(roll)*sin(pitch)

Компенсация наклона Y:

новый Y = Y*cos(рулон) + Z*sin(рулон)

Заключительный заголовок : Азимут = atan2(Y, X) * RAD_TO_DEG

Углы, которые я получаю от mpu,-это углы с поправкой на дрейф гироскопа, которые проходят через фильтр Калмана. Эти углы подаются в вышеприведенные уравнения, и после atan2 я получаю направление с компенсацией наклона. Но проблема/наблюдение здесь заключается в том, что когда я поворачиваю тело(на котором находятся компас и mpu) вдоль оси Z (ось рыскания), заголовок не меняется(вроде как заблокирован), но меняется, когда я наклоняю тело в X/Y. Я искал подробные ссылки, но не смог найти ни одной!
Вот мой фрагмент кода (использует библиотеку compass part form adafruit):

double HEADING(double r, double p){
  sensors_event_t event; 
  mag.getEvent(&event);
  r = r * (PI/180);
  p = p * (PI/180);
  double x = (event.magnetic.x)*cos(p) + (event.magnetic.y)*sin(r)*sin(p) - (event.magnetic.z)*cos(r)*sin(p);
  double y = (event.magnetic.y)*cos(r) + (event.magnetic.z)*sin(r);
 
  float heading = atan2(y,x);
 
  float declinationAngle = 0.366667 * (PI/180);// (0°22')positve склонение
  heading += declinationAngle;
  
  if(heading < 0)
    heading += 2*PI;
    
  if(heading > 2*PI)
    heading -= 2*PI;
   
  heading = heading * 180/PI; 
  
  return heading;
}

Сбор данных HMC5883L я написал :

// мне просто нужна ссылка на направление, а не на фактический курс, просто чтобы сохранить свой беспилотник 
// устойчиво на оси рыскания относительно этого направления, но оно должно быть компенсировано наклоном.
#include<Wire.h>

#define MAG 0x1E

#define R 0
#define P 1
#define Y 2

float mag_raw[3],h;
float CMP_DEC = 0.36667 * (PI/180);



void setup(){
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);
  
  Wire.beginTransmission(MAG);
  Wire.write(0x02);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
}

void loop(){
  Wire.beginTransmission(MAG);
  Wire.write(0x03);
  Wire.endTransmission(false);
  Wire.requestFrom(MAG,6,true);

  mag_raw[R] = Wire.read() << 8 | Wire.read();
  mag_raw[P] = Wire.read() << 8 | Wire.read();
  mag_raw[Y] = Wire.read() << 8 | Wire.read();

  h = atan2(mag_raw[P],mag_raw[R]);

// h += CMP_DEC;

  if(h < 0) h+= (2*PI);
  if(h > (2*PI)) h -= (2*PI);
  h *= (180/PI);
  
// Serial.print("X : ");
// Serial.print(mag_raw[R]);
// Serial.print(" Y : ");
// Serial.print(mag_raw[P]);
// Serial.print(" Z : ");
// Serial.print(mag_raw[Y]);

  Serial.print(" H : ");
  Serial.println(h);
  
}

Некоторые рекомендации будут очень полезны!