Направление по компасу не изменяется линейно

Я использую магнитометрический компас HMC-5983 на своем Arduino Uno, чтобы определить направление строящегося автомобиля. Но показания моего компаса не меняются линейно. Например, когда я поворачиваю компас на 90 градусов, изменение показаний не соответствует 90 градусам. Я попытался сопоставить необработанные выходные данные компаса с фактическим направлением, но когда я это делаю, направления между точками, которые я нанес на карту, недостаточно точны, что приводит к тому, что моя машина ведет себя ненормально. Я также искал в Google, но не нашел ничего полезного, хотя было упоминание об использовании алгоритма для решения этой проблемы. Пожалуйста, помогите мне решить эту проблему.

    #include <Wire.h> //Библиотека I2C Arduino
#define Magnetometer_mX0 0x03  
#define Magnetometer_mX1 0x04  
#define Magnetometer_mZ0 0x05  
#define Magnetometer_mZ1 0x06  
#define Magnetometer_mY0 0x07  
#define Magnetometer_mY1 0x08  
int mX0, mX1, mX_out;
int mY0, mY1, mY_out;
int mZ0, mZ1, mZ_out;
float heading, headingDegrees, headingFiltered, declination;
float Xm,Ym,Zm;
#define Magnetometer 0x1E // 7-битный адрес I2C HMC5883
void setup(){
  // Инициализация последовательной связи и связи I2C
  Serial.begin(9600);
  Wire.begin();
  delay(100);

  Wire.beginTransmission(Magnetometer); 
  Wire.write(0x02); // Регистр выбора режима
  Wire.write(0x00); // Непрерывный режим измерения
  Wire.endTransmission();
}
void loop(){

//---- ось X
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mX1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
    if(Wire.available()<=1)   
  {
    mX0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mX0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
  if(Wire.available()<=1)   
  {
    mX1 = Wire.read();
  }
  //---- ось Y
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mY1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
  if(Wire.available()<=1)   
  {
    mY0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mY0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
  if(Wire.available()<=1)   
  {
    mY1 = Wire.read();
  }

  //---- ось Z
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mZ1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
  if(Wire.available()<=1)   
  {
    mZ0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передать на устройство
  Wire.write(Magnetometer_mZ0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); 
  if(Wire.available()<=1)   
  {
    mZ1 = Wire.read();
  }

  //---- ось X
  mX1=mX1<<8;
  mX_out =mX0+mX1; // Необработанные данные
  // Из таблицы данных: 0,92 мГ/разр.
  Xm = mX_out*0.00092; // Единица Гаусса
  //* Магнитное поле Земли колеблется от 0,25 до 0,65 Гаусса, так что это значения, которые нам нужно получить приблизительно.
  //---- ось Y
  mY1=mY1<<8;
  mY_out =mY0+mY1;
  Ym = mY_out*0.00092;
  //---- ось Z
  mZ1=mZ1<<8;
  mZ_out =mZ0+mZ1;
  Zm = mZ_out*0.00092;
  // ===============================
  //Вычисление заголовка
  heading = filtered_angle(Zm, Xm);// арктангенс z/x

  // Коррекция курса с углом склонения в зависимости от вашего местоположения
  // Вы можете найти свой угол склонения по адресу: http://www.ngdc.noaa.gov/geomag-web/
  // В моем местоположении 4,2 градуса => 0,073 рад
  declination = 0.03717551307 ;
  heading += declination;

  // Исправление при перепутывании знаков
  if(heading <0) heading += 2*PI;
  // Исправление за счет добавления угла склонения
  if(heading > 2*PI)heading -= 2*PI;
  headingDegrees = heading * 180/PI; // Заголовок в градусах
  // Сглаживание выходного угла / Фильтр нижних частот
  headingFiltered = headingDegrees;
  //Отправка значения заголовка через последовательный порт в Processing IDE

  if(headingFiltered >= 0 && headingFiltered <= 89){
                c_head=map(headingFiltered,0,90,270,359);
        }   
    if(headingFiltered > 89 && headingFiltered <= 204){
                c_head=map(headingFiltered,90,204,0,90);
        }     
    if(headingFiltered > 204 && headingFiltered <= 290){
                c_head=map(headingFiltered,205,290,91,180);
        }         
      if(headingFiltered > 290){
               c_head=map(headingFiltered,291,359,181,269);
        }   




// Serial.print("Градусы - ");
 Serial.println(headingDegrees);
delay(500);
}

float filtered_angle(float p, float q) {
         const float filter_constant = 0.15;
         static float q_f, p_f; // отфильтрованные компоненты
         q_f += filter_constant * (q - q_f);
         p_f += filter_constant * (p - p_f);
         return atan2(p, q); 
        }