forked from logzhan/NotesUESTC
31 lines
1.0 KiB
Markdown
31 lines
1.0 KiB
Markdown
|
组合导航数学基础
|
|||
|
|
|
|||
|
|
一、欧拉角
|
|||
|
|
|
|||
|
|
欧拉角表示绕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
|
|||
|
|
$$
|