声明： 本文的图文均来自https://www.kalmanfilter.net如有侵权，请联系删除。

本文由微信公众号组成【DeepDriving】由于全文内容较多，分为三部分发出。关注微信官方账号【DeepDriving】，后台回复关键词卡尔曼滤波器可获得全文PDF。

上面介绍了一维卡尔曼滤波器。我相信你对卡尔曼滤波器有一定的了解，但在实际应用中，我们通常需要处理具有多维状态数据的系统。例如，对于三维空间中的飞机，我们需要9维向量来描述其位置、速度和加速度：

[ x y z x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ ] T \begin{bmatrix} x & y & z & \dot{x} & \dot{y} & \dot{z} & \ddot{x} & \ddot{y} & \ddot{z} \end{bmatrix}^{T} [xyzx˙y˙​z˙​x¨​y¨​​z¨​]T

假设我们采用恒加速度（constant acceleration）动态模型，那么在$n$时刻飞机的状态可以写为

$$\{\begin{array}{l}{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{array}$$

状态外推方程的作用是在当前时刻$n$基于现有的知识去预测$n+1$时刻系统的状态，所以也叫状态预测方程或者状态转移方程，其矩阵形式的公式如下：

$${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\mathbf{u}}_{\mathbf{n}}+{\mathbf{w}}_{\mathbf{n}}$$

其中，${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}$是预测的$n+1$时刻的系统状态向量；${\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}$是估计的$n$时刻的系统状态向量；${\mathbf{u}}_{\mathbf{n}}$