Вычисление отклонения от курса по магнитометру и акселерометру

Я использую Arduino и датчик с 9 степенями свободы (гироскоп, акселерометр и магнитометр) и пытаюсь использовать угол наклона, крена и рыскания, которые датчик дает мне, чтобы вращать объект в единстве.

Мне удалось правильно вычислить тангаж и крен (ось z и x в единице) по акселерометру, но, похоже, я не могу правильно рассчитать рысканье. Под этим я подразумеваю, что когда я поворачиваю датчик по тангажу или крену, он странным образом вращает и мое отклонение.

Код в Arduino для получения заголовка

void getHeading(void)
{
  heading=180*atan2(Mxyz[0],Mxyz[1])/PI;
  if(heading <0) heading +=360;
}

void getTiltHeading(void)
{
  //float шаг = asin(-Axyz[0]);
  //floatroll = asin(Axyz[1]/cos(pitch));
  float pitch = atan( Axyz[0] / sqrt( Axyz[1] * Axyz[1] + Axyz[2] * Axyz[2] ) );
  float roll = atan( Axyz[1] / sqrt(Axyz[0] * Axyz[0] + Axyz[2] * Axyz[2]));

  float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
  float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
  float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
  tiltheading = 180 * atan2(yh, xh)/PI;
  if(yh<0)    tiltheading +=360;
}

Подача и перекат

    float _Pitch  = (float)(180 / Math.PI * Math.Atan( m_ResultX / Math.Sqrt( m_ResultY * m_ResultY + m_ResultZ * m_ResultZ ) ) );
    float _Roll = (float)(180 / Math.PI * Math.Atan(m_ResultY / Math.Sqrt(m_ResultX * m_ResultX + m_ResultZ * m_ResultZ)));
    float _Yaw = (float)(m_TiltHeadingResult);

Не стесняйтесь спрашивать подробности.