Алгоритм Madgwick AHRS в Unity 3D

Question

Я пытаюсь рассчитать абсолютный курс устройства Android и соответственно повернуть GameObject. Таким образом, GameObject будет действовать как 3D-компас, который указывает на север Земли в 3D-пространстве независимо от ориентации устройства.

Для этого я использую алгоритм AHRS Madgwick в качестве библиотеки C # для одного файла , поддерживаемой x-io Technologies.

Вот скрипт, который я прикрепил к игровому объекту CompassArrow.

using System;
using UnityEngine;
using AHRS;

namespace MyCompass
{
    public class CompassArrowController : MonoBehaviour
    {
        static MadgwickAHRS AHRSInst = new MadgwickAHRS(1f / 256f, 0.1f);

        void Start()
        {
            Input.compass.enabled = true;
            Input.gyro.enabled = true;
            Input.location.Start();
        }

        void Update()
        {
            UpdateAHRS();
            //Get algorithm result quaternion
            var q = AHRSInst.Quaternion;
            //Create unity quaternion
            Quaternion arrowRotation = new Quaternion(q[0], q[1], q[2], q[3]);
            transform.rotation = Quaternion.Inverse(arrowRotation);
        }

        static void UpdateAHRS()
        {
            AHRSInst.Update(
                //Gyro
                Input.gyro.rotationRateUnbiased.x,
                Input.gyro.rotationRateUnbiased.y,
                Input.gyro.rotationRateUnbiased.z,
                //Acceleration
                Input.acceleration.x,
                Input.acceleration.y,
                Input.acceleration.z,
                //Magnetometer
                Input.compass.rawVector.x,
                Input.compass.rawVector.y,
                Input.compass.rawVector.z
            );
        }
    }
}

Это самое близкое, что я мог прийти к желаемому результату. Но оси все еще меняются местами, и мне нужно повернуть устройство примерно 4 раза, чтобы стрелка компаса выполнила один полный оборот.

Я предполагаю, что данные датчика, поданные в алгоритм, указаны в неправильных единицах. Уверяю вас, устройство, которое я использовал, имеет все 3 датчика, необходимые для работы этого алгоритма.

Вот описания параметров для метода Update() из библиотеки.

/*
Signature: public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
Summary: Algorithm AHRS update method. Requires only gyroscope and accelerometer data.
gx: Gyroscope x axis measurement in radians/s.
gy: Gyroscope y axis measurement in radians/s.
gz: Gyroscope z axis measurement in radians/s.
ax: Accelerometer x axis measurement in any calibrated units.
ay: Accelerometer y axis measurement in any calibrated units.
az: Accelerometer z axis measurement in any calibrated units.
mx: Magnetometer x axis measurement in any calibrated units.
my: Magnetometer y axis measurement in any calibrated units.
mz: Magnetometer z axis measurement in any calibrated units.
*/

В библиотеке также есть метод Update(), который не требует показаний магнитометра. Но поскольку я разрабатываю компас, я не думаю, что этот метод будет полезен.

Кто-нибудь может указать, что я делаю неправильно? Я могу предоставить более подробную информацию по запросу.