我们的目标是将物体的三维旋转的三个角度表达出来，因为四元数在空间旋转上有优势，可以使用四元数计算旋转的角度。那么就需要先了解四元数与三维旋转分别是什么，他们之间的转换关系。以及当一个转动发生时，如何计算角度？是通过角速度来更新四元数，而四元数中存储着角度的信息。这个更新角度或四元数的方法就是一阶龙格库塔法。通过这些步骤来实现姿态解算。
"姿"态解算
一，三维空间的旋转
1.1所有三维空间的旋转都可以看成是以某一向量为轴旋转θ，如图1。
1.2 从正面看，将v分解成v平行和v垂直分量，如图2：
计算v平行和v垂直：
1.2 从上往下看，w向量是v垂直与u向量的叉积，如图3，求出旋转后的v`表达式：
图1，2，3：
二，四元数简介
最基本的表示：q=a+bi+cj+dk;i²=j²=k²=-1;
2.1 四元数乘法
四元数乘法不遵守交换律
q1=a1+b1i+c1j+d1k;
q2=a2+b2i+c2j+d2k;
q1×q2=(a1+b1i+c1j+d1k)×(a2+b2i+c2j+d2k) 用乘法分配律解决它
2.2 四元数的逆和共轭
q是一个四元数，q×q的逆=q的逆×q=1
q×q*=q²
若q是单元四元数，则q²=1，q的逆=q*
2.3 四元数的三角形式
四元数q=[cosΘ,sinθ×U]，q表示将某个向量V绕向量U旋转θ角度。
则q×q=[cos2×Θ,sin2×θ×U]，表示连续进行两次相同的旋转，转过的角度是2θ
三，将四元素与三维空间的旋转结合
3.1 v垂直的旋转的四元素表达式:如图所示
3.2 v旋转的四元数表达式
化简v得’
3.3 两个定理
定理一：若v平行是纯四元数，而q=[a,bu]，u是单位向量，若v平行平行于u，则q×v平行=v平行×q（满足交换律）
定理二：若v平行是纯四元数，而q=[a,bu]，u是单位向量，若v垂直垂直于u，则q×v垂直=v垂直×q*
所以最后v可化简成：
3.4 矩阵的左右乘，利用该性质可以解p×v×p*
得到四元数的旋转矩阵形式
四，欧拉角与四元数转换
4.1 三轴旋转的矩阵形式：如图Aθ等
4.2 欧拉角的四元数表达式，将三角变换矩阵和上面的四元数旋转矩阵一一对应可解出欧拉角。
五，一阶龙格库塔法解四元数微分方程
5.1 简介
一阶龙格库塔法可以用来解微分方程，他的形式为：y(n+1)=y(n)+h*y’,其中y’=f(x(n),y(n))
5.2 四元数微分方程
5.3 一阶龙格库塔法解四元数微分方程，获得新的四元数
