当我试图将旋转矩阵应用于重力矢量（从静态加速度计获得并存储）时，当设备以一定的旋转（偏航，俯仰，滚动）移动时，我遇到了问题。旋转矩阵不应该改变向量的大小，但在我的情况下，向量的大小在每次迭代时都会改变。下面的代码显示了我使用的旋转矩阵：
averageA包含恒定的重力矢量，angleypr包含使用陀螺仪，磁力计，加速度计计算的纱线，俯仰和滚转角度。
temp[0]=averageA[0];
temp[1]=averageA[1];
temp[2]=averageA[2];
//Yaw
temp[0]=cos(angleypr[0]*pi/180)*temp[0]+sin(angleypr[0]*pi/180)*temp[1];
temp[1]=-sin(angleypr[0]*pi/180)*temp[0]+cos(angleypr[0]*pi/180)*temp[1];
temp[2]=temp[2];
//pitch
temp[0]=temp[0];
temp[1]=(cos(angleypr[1]*pi/180))*temp[1]+(sin(angleypr[1]*pi/180))*temp[2];
temp[2]=(-sin(angleypr[1]*pi/180))*temp[1]+cos(angleypr[1]*pi/180))*temp[2];
//roll
temp[0]=(cos(angleypr[2]*pi/180))*temp[0]-(sin(angleypr[2]*pi/180))*temp[2];
temp[1]=temp[1];
temp[2]=(sin(angleypr[2]*pi/180))*temp[0]+(cos(angleypr[2]*pi/180))*temp[2];
gravity[0]=temp[0];
gravity[1]=temp[1];
gravity[2]=temp[2];
我现在面临的一个简单问题：
temp[0]=cos(angleypr[0]*pi/180)*20+sin(angleypr[0]*pi/180)*30;
temp[1]=-sin(angleypr[0]*pi/180)*20+cos(angleypr[0]*pi/180)*30;
temp[2]=50;
temp[0]=cos(angleypr[0]*pi/180)*temp[0]+sin(angleypr[0]*pi/180)*temp[1];
temp[1]=-sin(angleypr[0]*pi/180)*temp[0]+cos(angleypr[0]*pi/180)*temp[1];
temp[2]=temp[2];
我在矢量[20,30,50]上应用相同的偏航旋转矩阵两次，第一次旋转给我一个恒定的幅度，但第二次旋转最终每次迭代都有不同的值。
请提前告知我错误的地方！