写在前面

前面大概说了卡尔曼滤波的原理以及一个简单的demo。

在阅读阿波罗源码，以及其他卡尔曼滤波的教程时发现，往往使用矩阵形式。这里接上篇一起介绍下。

数学模型

• 状态转移方程 $$\boldsymbol{x}_k = \boldsymbol{Ax}_{k-1} + \boldsymbol{Bu}_{k-1} + \boldsymbol{w}_k$$ 式中：
  • $\boldsymbol{A}$为状态转移矩阵
  • $\boldsymbol{B}$为控制矩阵
  • $\boldsymbol{u}_{k-1}$为上一时刻的作用于系统的外界控制向量，本文暂时不考虑。
  • $\boldsymbol{w}_k$为过程噪声。
• 观测方程 $$\boldsymbol{z}_k = \boldsymbol{Hx}_{k} + \boldsymbol{v}_{k}$$ 式中：
  • $\boldsymbol{z}_k$为直接观测值
  • $\boldsymbol{H}$为观测矩阵，用来模拟直接观测量和状态量之间的数学关系。有时候需要估计的状态是无法直接被观测到的，所以需要这个公式，这个矩阵
  • $\boldsymbol{v}_{k}$为观测噪声

预测过程

跟之前一样，先将上一时刻得到的最终状态估计值$\boldsymbol{x}_{k-1}$代入状态转移方程，得到当前时刻状态的理论预测值$\hat{\boldsymbol{x}}_k$。即有，

$$\hat{\boldsymbol{x}}_k = \boldsymbol{Ax}_{k-1}$$

类似地，可以得到$\hat{\boldsymbol{x}}_k$的方差$\hat{\boldsymbol{P}}_k$，

$$\hat{\boldsymbol{P}}_k = \boldsymbol{A}\boldsymbol{P}_{k-1}\boldsymbol{A}^T+\boldsymbol{Q}_k$$

其中，$\boldsymbol{Q}_k$为当前时刻过程噪声的协方差矩阵。这两个公式便是矩阵形式的卡尔曼滤波预测过程。

更新过程

然后，还是要用当前时刻的观测值$\boldsymbol{z}_k$来校正理论预测值$\hat{\boldsymbol{x}_k}$。

但是，对于大多数情况，$\boldsymbol{z}_k$和$\hat{\boldsymbol{x}}_k$都不表示相同的状态量（说人话就是，直接观测的量不是需要估计的量，比如引擎内部的温度，由于过高而无法测量，只能测外壁的温度然后估算）。

这就使得更新过程跟原来不太一样，需要引入观测矩阵$\boldsymbol{H}$进行计算。

首先，更新（融合）公式如下：

$$\boldsymbol{x}_k = \hat{\boldsymbol{x}}_{k} + \boldsymbol{K}(\boldsymbol{z}_k - \boldsymbol{H}\hat{\boldsymbol{x}}_{k}) = (\boldsymbol{I} - \boldsymbol{KH})\hat{\boldsymbol{x}}_{k} + \boldsymbol{K}{\boldsymbol{z}}_{k}$$ $$\boldsymbol{P}_k = (\boldsymbol{I} - \boldsymbol{KH})\hat{\boldsymbol{P}}_{k}$$

式中，只有$\boldsymbol{K}$是未知的（矩阵形式的卡尔曼增益），并且可以看到观测结果的协方差以及噪声都没有直接体现在融合公式中。这是因为这些信息都包含在卡尔曼增益矩阵中，具体有：

$$\boldsymbol{K} = \hat{\boldsymbol{P}}_{k}\boldsymbol{H}^T\boldsymbol{S}^{-1}$$

其中，$\boldsymbol{S}$为$\boldsymbol{z}_k - \boldsymbol{H}\hat{\boldsymbol{x}}_{k}$的协方差矩阵，具体有：

$$\boldsymbol{S} = \boldsymbol{H}\hat{\boldsymbol{P}}_{k}\boldsymbol{H}^T+\boldsymbol{R}_k$$

综合来看，预测过程需要先计算$\boldsymbol{S}$，再计算$\boldsymbol{K}$，然后用观测值$\boldsymbol{z}_k$和$\boldsymbol{K}$参与理论预测值（状态及方差）的校正。

具体实例

以阿波罗源码中目标跟踪所采用的“匀速卡尔曼滤波器”为例，应用以上理论。首先确定状态转移方程和观测方程，需要估计的状态向量为二维的坐标及车速，由于是匀速运动模型，所以有：

$$\left[ \begin{array}{c} x \\ y \\ \dot x \\ \dot y \\ \end{array} \right]_k = \left[ \begin{array}{cccc} 1 & 0 &dt &0 \\ 0 & 1 &0 &dt \\ 0 & 0 &1 &0 \\ 0 & 0 &0 &1 \\ \end{array} \right] \left[ \begin{array}{c} x \\ y \\ \dot x \\ \dot y \\ \end{array} \right]_{k-1}$$

直接观测状态为二维坐标，因此可以得到观测方程为：

$$\left[ \begin{array}{c} z_x \\ z_y \\ \end{array} \right]_k = \left[ \begin{array}{cccc} 1 & 0 &0 &0 \\ 0 & 1 &0 &0 \\ \end{array} \right] \left[ \begin{array}{c} x \\ y \\ \dot x \\ \dot y \\ \end{array} \right]_{k}$$

至此，便得到了状态转移矩阵$\boldsymbol{A}$和观测矩阵$\boldsymbol{H}$。然后在初始化时给定$\boldsymbol{x}_0$和$\boldsymbol{P}_0$，便可完成预测、更新、预测…的迭代过程。

其实还要给定$\boldsymbol{Q}$和$\boldsymbol{R}$，过程噪声视情况而定要不要跟着迭代，阿波罗的做法是对于目标在图像中的中心点不跟着迭代，但是对于目标中心点的3D坐标跟着迭代。

测量噪声一般来自于传感器厂家，对于整个卡尔曼滤波过程而言是一个常数，初始化时给定即可，一般不跟着迭代。