title: Madgwick AHRS algorithm in Unity 3D
body: <p>I am trying to calculate the absolute heading of an Android device, and rotate a GameObject accordingly. So the GameObject will act as a 3D compass which points earth's north in 3D space, regardless of the device orientation.</p>
  
  <p>To achieve this, I am using <a href="http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/" rel="nofollow noreferrer">Madgwick AHRS algorithm</a> as a <a href="https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MadgwickAHRS.cs" rel="nofollow noreferrer">single file C# library</a> maintained by x-io Technologies.</p>
  
  <p>Here is the script I attached to the <code>CompassArrow</code> game object.</p>
  
  <pre class="lang-cs prettyprint-override"><code>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
              );
          }
      }
  }
  </code></pre>
  
  <p>This is the closest I could come to the desired output. But still the axises are swapped, and I have to rotate the device about 4 times for the compass arrow to complete a single full rotation.</p>
  
  <p>My guess is, sensor data fed to the algorithm is in wrong units. I assure you the device I used has all 3 sensors required for this algorithm to run.</p>
  
  <p>Here are the parameter descriptions for <code>Update()</code> method from the library.</p>
  
  <pre class="lang-cs prettyprint-override"><code>/*
  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.
  */
  </code></pre>
  
  <p>The library also has a <code>Update()</code> method which doesn't require magnetometer readings. But since I am developing a compass, I don't think that method will be useful.</p>
  
  <p>Can anyone point out what I am doing wrong? I can provide more details upon request.</p>