纯惯性导航

主要分为姿态解算、速度解算、位置解算；

1. 由初始位置得位置矩阵  ；初始对准之后的初始姿态角得 ，继而得到初始四元数；初始速度得到 ；
2. 加速度计和陀螺仪分别给出载体测出来的比力和角速度，然后通过转换矩阵转将比力和角速度转到地固坐标系下；
3. 通过力学编排公式计算出 ，通过一次积分；得到速度
4. 通过3计算出的速度更新
5. 通过位置矩阵微分方程积分更新位置矩阵微分方程
6. 通过 ，反算出经纬度
7. 陀螺仪得到 ，由 ， ，通过利用 和四元数微分方程进行四元数更新，通过四元数得到更新后的 ，通过 ，反算出姿态角