3

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.

To achieve this, I am using Madgwick AHRS algorithm as a single file C# library maintained by x-io Technologies.

Here is the script I attached to the CompassArrow game object.

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
            );
        }
    }
}

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.

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.

Here are the parameter descriptions for Update() method from the library.

/*
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.
*/

The library also has a Update() method which doesn't require magnetometer readings. But since I am developing a compass, I don't think that method will be useful.

Can anyone point out what I am doing wrong? I can provide more details upon request.

nipunasudha
  • 2,427
  • 2
  • 21
  • 46
  • Is [this](https://stackoverflow.com/a/39477339/3785314) helpful? – Programmer Jul 06 '18 at 09:05
  • @Programmer Thank you for the reply, but sadly no. Unity `Input.compass.trueHeading` gets confused when the device is hold vertically. May be there is no tilt compensation underneath. – nipunasudha Jul 06 '18 at 09:22
  • 1
    If that's the case then you will need to build a plugin to access the sensor for each mobile platform. – Programmer Jul 06 '18 at 09:26
  • yes @Programmer that is my fallback, and I'm working on it. I'm experimenting with the native android call `SensorManager.GetRotationMatrix`, seems like it does the job without any 3rd party algorithm. – nipunasudha Jul 06 '18 at 09:54
  • 1
    Sometimes, native plugin is the proper way to fix an issue. For iOS, CoreMotion should be fine. If you solve the issue, please show us how – Programmer Jul 06 '18 at 09:57

1 Answers1

0
 ahrs.Update(g.x,g.y,g.z,a.x,a.y,a.z);
 var _q = ahrs.Quaternion;
        
 Quaternion q = new Quaternion(_q[0],_q[1],_q[2],_q[3]);
 var qEualr = q.eulerAngles;
Quaternion correctQuaternion =  Quaternion.Euler(qEualr.z, -qEualr.y, qEualr.x);

Will give you a quaternion with correct axes. Seems like unity has them swapped compared to the algorithm.

Usmiech
  • 181
  • 3
  • 12