关于四元数的值是如何获得的：
1.这里没有做初始对准，假设一开始载体坐标系和导航坐标系是重合的，也就是尾翼I指东，II指北，火箭头指向天顶。
这时候Q = [1 0 0 0];（实部1 虚步0） T = [1 0 0;0 1 0;0 0 1];（就是单位矩阵）。于是我们有了Q的初始值。
下一步打算用GPS结合加速度计、磁阻传感器数据来确定Q的初始值，MEMS陀螺要做初始对准还是比较难的。
2.有了初始值之后，根据更新公式，不断用角速度值来更新Q的数值。
找到一篇支持93观点的文章：
捷联惯性导航制导系统中方向余弦矩阵的递推算法.pdf
1.11MB
PDF
343次下载
预览
其实更新DCM比四元数法推导还要简单，但DCM的正交化较为麻烦。
附上姿态矩阵法代码，此处只有DCM更新部分，把楼主位代码的四元数运算部分换掉即可：
