12、MATLAB 中卡尔曼滤波器的多变量示例

MATLAB 中卡尔曼滤波器的多变量示例

1. 场景概述与卡尔曼滤波器设置

在这个场景中，我们要研究一张从初始高度 $y_0$ 落下的纸团的高度变化。可以用离散时间模型来描述其高度的演变，相关方程如下：
- 状态转移方程 ：
- $x(t + 1) = F(t)x(t) + G(t)u(t)$ （式 10.1）
- 其中，$x = \begin{bmatrix} y_k \ y_k’ \end{bmatrix}$ （式 10.2），$y$ 是纸团的瞬时高度，$y’$ 是其速度。
- $F = \begin{bmatrix} 1 & \Delta T \ 0 & 1 \end{bmatrix}$ （式 10.3）
- $G = \begin{bmatrix} -\frac{1}{2}(\Delta T)^2 \ -\Delta T \end{bmatrix}$ （式 10.4）
- $\Delta T$ 是采样间隔，即两次连续信号采样之间的时间，$g = 9.81 m/s^2$ 被赋值给 $u(t)$。由此可得：
$\begin{bmatrix} y_{k + 1} \ y_{k + 1}’ \end{bmatrix} = \begin{bmatrix} 1 & \Delta T \ 0 & 1 \end{bmatrix} \begin{bmatrix} y_k \ y_k’ \end{bmatrix} + \begin{bmatrix} -\frac{1}{2}(\Delta T)^2 \ -\Delta T \end{bmatrix} g$ （式 10.5）
-