搓一个组合导航需要的程序

1.cbn函数

可以用旋转矩阵，也可以用四元数阵。
按不同的旋转顺序和坐标系不一样
//todo:完善一下

2. 获取欧拉角的函数

与第三行有关

3.四元数姿态更新，或者旋转矩阵姿态更新

4.初始姿态角、位置、速度设定

（静对准）

5.地球自转相关矩阵和地球重力更新

6.惯导速度位置姿态更新主函数

7. PQR阵的初始化和赋值

可以有很多组

8.组合导航函数，需要调用ekf

ekf（eskf）函数，几个公式相乘

9. Fai阵赋值函数

10.Fai阵更新和Q阵更新函数