定义说完了，接下来直接开始算法的流程。这里为了编号的方便用大写英文字母表示顺序。

该算法基于以下理想条件：传感器无误差，速度，位移不太大（不用考虑惯性离心力与科里奥利力，也不用考虑地球的形状）

一切之前要读取加速度（比力）与角速度数据，而且实际情况要考虑数据融合。不过部分我还在学……

这里使用旋转矢量法更新姿态，兼顾精度与计算速度，这也是工程上最常见的结算方法。

A求解从上次更新到现在的旋转矢量

B更新四元数

C更新姿态矩阵与欧拉角

至此得出姿态，但是由于低成本MEMS精度不够，直接用会漂，要kalman滤波。这里我还在学，希望下一代飞控搞出来后可以写完整版。

D更新速度

E更新位置

至此得出捷联惯导的结果，但是还需要组合导航，这里只说下我的程序里是怎么做的，还是那个原因（

F数据融合