四元數到歐拉算法。 getPitch（）返回Nan？

我想轉換我從傳感器獲得的四元數並通過計算theta得到音高。如果有人有興趣，看看in this article - 第2章四元數到歐拉算法。 getPitch（）返回Nan？

我的問題是在下面的代碼：

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

歐拉1它得到在球場總是返回南（非數字） 我不知道我是否正確實施了算法。出於某種原因，Asin（d），其中d的輸出是> 1和< -1。

來源

2013-08-05 COLD ICE

我認爲你的四元數不規範。只有標準化的四元數表示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

由正常性假設。

來源

2013-08-05 15:54:14 wendazhou

四元數到歐拉算法。 getPitch（）返回Nan？

回答

相關問題