Преобразование кватерниона IMU Madgwick в положение x, y, z в Unity3D ⇐ C#

[Anonymous]

Я пытаюсь добиться этого, а также этого, но в Unity3D и с использованием датчика IMU ICM-20948 с 9 степенями свободы:
До сих пор мне удавалось использовать алгоритм Мэджвика и вращайте куб, наклоняя датчик в воздухе. Теперь я не уверен, как заставить его двигаться как указатель мыши. Я испытываю следующее: когда я перемещаю датчик вверх и вниз (как бы наклоняя его вверх, как будто это пульт дистанционного управления), кажется, что он работает. Но когда я наклоняю его влево и вправо, он не работает. Также значения быстро перескакивают с плюса на минус, в результате чего объект оказывается на противоположной стороне экрана — это для x и y. Единственный способ, которым это работает, — это очень легкое перемещение его, но тогда оно остается в очень небольшом диапазоне. Я хочу иметь возможность перемещать его в любое место экрана.
Вот мой код:

Код: Выделить всё

MadgwickAHRS madgwickAHRS = new MadgwickAHRS(1f / 256f, 0.1f);
madgwickAHRS.Update(gx, gy, gz, ax, ay, az, mx, my, mz); // called in a separate thread loop

float[] angles = ToEuler(madgwickAHRS.Quaternion);

float horzValue = angles[0] - horzZero;
float vertValue = angles[1] - vertZero;

horzZero = angles[0]; // yaw
vertZero = angles[1]; // roll

void FixedUpdate() {
transform.position = new Vector3(horzValue, vertValue, 0);
}
...

float[] ToEuler(float[] q)
{
float yaw = (float)Math.Atan2(2.0 * (q[1] * q[2] + q[3] * q[0]), q[3] * q[3] - q[0] * q[0] - q[1] * q[1] + q[2] * q[2]);
float pitch = (float)Math.Asin(-2.0 * (q[0] * q[2] - q[3] * q[1]));
float roll = (float)Math.Atan2(2.0 * (q[0] * q[1] + q[3] * q[2]), q[3] * q[3] + q[0] * q[0] - q[1] * q[1] - q[2] * q[2]);

return new float[2] { yaw, roll };
}

Пытался использовать эту функцию и для каждого значения углов, но это только усугубляет ситуацию.

Код: Выделить всё

private float CalcDist(float angle, float initAngle)
{
angle = (float)((angle - initAngle) * (180 / Math.PI));
angle = angle < 0 ? angle + 360 : angle;
angle = angle > 180 ? angle - 360 : angle;
float dist = (float)(Math.Round(-offsetDistance * Math.Tan(angle * (Math.PI / 180))));

return dist;
}

Также попробовал использовать исправленный кватернион (кстати, это хорошо работает для вращения):

Код: Выделить всё

Quaternion objectOrientation = new Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
var qEualr = objectOrientation.eulerAngles;
Quaternion correctQuaternion = Quaternion.Euler(qEualr.z, -qEualr.y, qEualr.x);

Может ли кто-нибудь мне помочь?

Подробнее здесь: https://stackoverflow.com/questions/670 ... in-unity3d