C# 四元数到欧拉算法。getPitch()返回Nan?
我正试图转换我从传感器获得的四元数,并通过计算θ得到螺距。如果有人感兴趣,请看第2章 我的问题出现在以下代码中:C# 四元数到欧拉算法。getPitch()返回Nan?,c#,nan,euler-angles,C#,Nan,Euler Angles,我正试图转换我从传感器获得的四元数,并通过计算θ得到螺距。如果有人感兴趣,请看第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));
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,我认为你的四元数没有标准化。只有标准化的四元数表示3D中的旋转,您必须具有该属性
q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] == 1
q[1]*q[3] <= 0.25 *(q[1] + q[3])^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
如果是这样的话,那么我们总是
d = q[1]*q[3] + q[0]*q[2] <= 0.5
根据正态性假设