13、卡尔曼滤波：从高度估计到IMU姿态估计的应用探索

卡尔曼滤波：从高度估计到IMU姿态估计的应用探索

1. 卡尔曼滤波在高度估计中的应用

1.1 状态向量校正分析

在高度估计的场景中，先验状态向量的两个元素的校正分别由卡尔曼增益（KG）的对应元素调节。由于它们都是2×1的列向量，括号内的量为标量。具体表示为：
[
\begin{bmatrix}
x_{A1} \
x_{A2}
\end{bmatrix}
=
\begin{bmatrix}
x_{B1} \
x_{B2}
\end{bmatrix}
+
\begin{bmatrix}
K_{G1} \
K_{G2}
\end{bmatrix}
\left{
z -
\begin{bmatrix}
1 & 0
\end{bmatrix}
\begin{bmatrix}
x_{B1} \
x_{B2}
\end{bmatrix}
\right}
]
每次迭代的后验高度估计可表示为：
[
x_{A1} = x_{B1} + K_{G1}(z - x_{B1})
]
其中，$x_{B1}$ 是模型得出的先验高度估计。通过绘制向量KG的第一个元素 $K_{G1}$ 的变化情况，可以直观地看到从模型高度结果到后验高度估计的“高度校正强度”的演变。若 $K_{G1}$ 为1，后验高度估计将完全校正为测距仪测量的值；若 $K_{G1}$ 为0，后验高度估计将采用模型得出的值。$K_{G1}$ 初始值较