NotesUESTC/算法开发笔记/组合导航/组合导航数学基础.md

组合导航数学基础

一、欧拉角

​ 欧拉角表示绕X、Y、Z轴的旋转。欧拉角的旋转顺序具有不可交换性。XYZ和ZYX所得到的旋转矩阵的结果是不同的。对于右手直角坐标系而言，我们只需要关注旋转的顺序即可。在不同的坐标系下，我们有时候可以定义绕X轴是Pitch,有时候也可以定义绕Y轴是Pitch。所以本质上我们可以不去看Yaw、Roll、Pitch，我们只需要管是绕X、Y、Z就行了。

五、GnssIns观测误差

​ 标准的组合导航观测误差是速度误差和位置误差：

Z=[\delta V \space \delta P]

​ 在东北天(ENU坐标系下)，我们可以写为：

Z=[\delta v_e \space \delta v_n \space \delta v_u \space \delta p_e \space \delta p_n \space \delta p_u]

六、卡尔曼滤波

6.1 增益矩阵更新

K=\frac{PH^T}{HPH+R}

6.2 状态向量更新

X=X+K(Z-HX)

​ 有时候，部分情况下状态向量每次更新被清空，上述方程可以化简为：

X=K*Z