多变量卡尔曼滤波

$$\left[ \begin{matrix} x\\ y\\ z\\ \end{matrix} \right]$$

描述飞机位置和速度的状态向量是六维的：

$$\left[ \begin{matrix} x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\\ \end{matrix} \right]$$

描述飞机位置、速度和加速度的状态向量则是九维的：

$$\left[ \begin{matrix} x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\\ \ddot{x}\\ \ddot{y}\\ \ddot{z}\\ \end{matrix} \right]$$

假设匀加速运动模型，我们可以用九个运动方程在 $n-1$ 时刻来外插飞机在 $n$ 时刻的状态：

$$\begin{cases} x_{n} = x_{n-1} + \dot{x}_{n-1} \Delta t+ \frac{1}{2}\ddot{x}_{n-1} \Delta t^{2}\\ y_{n} = y_{n-1} + \dot{y}_{n-1} \Delta t+ \frac{1}{2}\ddot{y}_{n-1} \Delta t^{2}\\ z_{n} = z_{n-1} + \dot{z}_{n-1} \Delta t+ \frac{1}{2}\ddot{z}_{n-1} \Delta t^{2}\\ \dot{x}_{n} = \dot{x}_{n-1} + \ddot{x}_{n-1} \Delta t\\ \dot{y}_{n} = \dot{y}_{n-1} + \ddot{y}_{n-1} \Delta t\\ \dot{z}_{n} = \dot{z}_{n-1} + \ddot{z}_{n-1} \Delta t\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1}\\ \end{cases}$$

通常我们把九个线性方程用矩阵形式合写成一个方程。

首先，逐一列举这些方程很费劲，用矩阵形式来记述更简洁明了。

其次，计算机进行矩阵计算效率很高，实现矩阵版的卡尔曼滤波能得到更短的计算时间。

后续章节会用矩阵形式描述卡尔曼滤波。并且，理论部分后会紧随具有完整求解流程的数值示例。

最后一章包括两个数值示例。第一个示例里，我们设计一个无输入的六维卡尔曼滤波器；第二个示例里，我们设计一个有输入的二维卡尔曼滤波器。