Question

我正在尝试转换从传感器获得的四元数，并通过计算θ来获得音高。如果有人有兴趣，请查看in this article - 第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。

Answer 1

我认为你的四元数没有标准化。只有标准化的四元数表示3D中的旋转，您必须具有

q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] == 1

如果是这种情况，那么我们总是有

d = q[1]*q[3] + q[0]*q[2] <= 0.5

因为我们有

q[1]*q[3] <= 0.25 *(q[1] + q[3])^2

其中^表示权力，AM-GM表示q[0]*q[2]

现在我们有了

d <= 0.25 * ( (q[1] + q[3])^2 + (q[0] + q[2])^2 ) 
  <= 0.25 * ( q[1]^2 + q[2]^2 + q[3]^2 + q[4]^2 )
  <= 0.25

通过常态假设。

四元数到欧拉算法。 getPitch（）返回Nan？

1 个答案: