1

センサーから取得したクォータニオンを変換し、シータを計算してピッチを取得しようとしています。興味のある方は、この記事(第 2 章) をご覧ください。

私の問題は次のコードにあります。

 private void quaternionToEuler(float[] q, float[] euler)
    {
        euler[0] = (float)Math.Atan2((2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0] * q[0]) + ((2 * q[1] * q[1]) - 1));
        euler[1] = -(float)Math.Asin(((2 * q[1] * q[3]) + (2 * q[0] * q[2]))); // theta
        euler[2] = (float)Math.Atan2((2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + ((2 * q[3] * q[3]) - 1)); // phi

        Console.WriteLine(euler[0] + ","+euler[1]+"," + euler[2]);
    }

ピッチを取得する euler 1 は常に Nan (数値ではない) を返します。アルゴリズムを正しく実装したかどうかはわかりません。何らかの理由で、Asin(d)、ここで d の出力は >1 かつ <-1 です。

4

1 に答える 1