Unity 3D 中的 Madgwick AHRS 算法

Question

我正在尝试计算Android设备的绝对标题，并相应地旋转游戏对象。因此，游戏对象将充当 3D 指南针，在 3D 空间中指向地球的北方，无论设备方向如何。

为了实现这一点，我使用Madgwick AHRS算法作为由x-io Technologies维护的单个文件C#库。

这是我附加到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()方法。但是由于我正在开发指南针，我认为这种方法不会有用。

谁能指出我做错了什么？我可以根据要求提供更多详细信息。

Answer 1

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

会给你一个具有正确轴的四元数。与算法相比，似乎团结使它们互换了。