卡尔曼滤波算法概要

卡尔曼滤波器是一个有状态滤波器，或者说数据筛——因为滤波器实在是太宽泛了。

核心思想

卡尔曼滤波器的核心思想事实上是十分平凡的：

• 系统存在若干的状态
• 不可能给出状态的真值
• 可以测量系统的状态得到测量值
• 测量值遵循一个关于真值的高斯分布（真值并不一定是 $\mu$）
• 状态之间是相关的
• 测量值之间是相关的
• 系统可以受到已知作用的影响而改变状态
• 系统可以受到未知作用的影响而改变状态
• 可以根据当前状态估算未来的状态
• 可以通过测量值修正估算的状态

我们接下来看一下具体如何通过这些核心思想来构建一个数据筛。

步步推导

设一个系统包含一个 $n$ 维状态向量 $\vec{x}$.

但是我们知道系统给定的状态并不是完美的，其遵循一个高斯分布 $G\sim(\mu, \sigma)$. $\mu$ 为分布的期望，$\sigma$ 为分布的标准差。即状态向量是一个随机变量的列向量。

状态之间可能会存在相关，这个相关在随机变量间的体现即为协方差。而对于一个 $n$ 维的随机变量向量，可以定义其协方差矩阵 $\mathbf{P}$ 如下：

状态可能是时变的。我们将当前时间记为 $k$, 当前时间的测量向量记作 $\vec{x}_k$, 当前时间内的协方差矩阵记作 $\mathbf{P}_k$.

我们使用极大似然估计，取当前最佳估计 $\hat{\vec{x}}$ 为各个随机变量的期望 $\mu$.

现在，我们希望能够估计在下一时刻 $k + 1$ 系统的状态。对于每个状态，我们都可以有一个预测向量 $\vec{f}_{k+1}$, 这些预测向量可以组成一个预测矩阵 $\mathbf{F}_{k+1}$.

那么，我们就可以得到一个对未来的估算状态 $\hat{\vec{x}}_{k + 1} = \mathbf{F}_{k + 1} \hat{\vec{x}}_{k}$.

现在我们得到了关于最佳估计向量的预测值，但是我们仍需要知道整体分布的变化，以及协方差矩阵的变化。我们直接暴力计算得到协方差矩阵的变化如下：

不过注意到预测矩阵的物理意义是系统自发的变化，即如果系统不受任何外力影响的状态下各个状态的变化。如果我们要包含外部变化，我们则应引入一些新的矩阵。

对于确定的外部作用，可以定义一个控制向量 $\vec{u}$ 及描述其对系统的各状态的影响的控制矩阵 $\mathbf{B}$. 由于线性代数的绝佳性质（线性性），我们可以直接把这些值加起来：

这里的 $k + 1$ 可能让人有些摸不着头脑，其实在其他教材中这个下标是写作 $k$ 而所谓的「旧值」是写作 $k - 1$ 的。这个下标似乎还暗示着这些矩阵有可能是时变的，诚然在一些情况下这些矩阵的内容的确会随着时间而发生变化，不过在大多数的应用中，这些矩阵的值实际上是不变的。

我们的系统并不是真空农场中的球形鸡，所以外部影响也并不是完全是可知的。对于不可知的外部影响，虽然我们没法「知道」其具体如何影响系统，但是我们仍旧可以将其纳入到我们的考虑范围内。这些不可知的影响会为我们带来新的不确定性，那么我们可以将其建模成为一个关于系统各个状态的协方差矩阵，或者叫噪声矩阵 $\mathbf{Q}$，并将其加入我们原有的协方差矩阵：

现在，我们就得到了关于系统的未来的一个预测：式 ($\circ$) 和式 ($\diamond$).

现在，我们可以通过测量来修正关于未来的预测。

传感器与系统的状态之间可能存在单位不一致的情况，或者传感器本身并不是线性传感器。这时我们可以引入一个转换矩阵 $\mathbf{H}$ 来表示从系统到传感器读数的变换。$\vec{x}$ 即可变换为 $\mathbf{H}\vec{x}$, 其协方差也变换为 $\mathbf{H}\mathbf{P}\mathbf{H}^{T}$.

但是传感器本身也可能会受到干扰。我们将这一不确定性的协方差矩阵记为 $R$, 传感器本身的读数为 $\vec{z}$, 那么对于 $\vec{z}$, 也存在一个最优估计 $\hat{\vec{z}}$ 是 $\vec{z}$ 的期望 $\mu$.

现在我们有两个符合正态分布的随机变量：$\mathbf{H}\vec{x}$ 和 $\vec{z}$. 现在一个符合直觉的做法是将这两个随机变量相乘——因为二者都是符合高斯分布的随机变量，相乘则可以获得二者重合的部分并从中取出最优估计。

我们先观察对于 $N\sim (x, \mu_0, \sigma_0) \cdot N\sim (x, \mu_1, \sigma_1) = N(x, \mu_?, \sigma_?)$ 中 $\mu$ 与 $\sigma$ 之间的关系。一顿暴力计算可以得出：

可以提取公因式 $b := \frac{\sigma^2_0}{\sigma^2_0 + \sigma^2_1}$. 上式可改写为：

我们再将其应用到矩阵上，令 $\Sigma$ 为随机变量的协方差矩阵，则：

这里的 $\mathbf{K}$ 称作卡尔曼增益（尽管这里其本质是个矩阵）。

现在，我们将之前推导得出的结论代入我们的系统状态-传感器读数的相乘中，得到：

其中，卡尔曼增益为：

现在我们进行化简，将式 (1) 和式 (2) 均左乘 $\mathbf{H}^{-1}_{k + 1}$, 再将式 (2) 右乘 $\mathbf{H}^{-1T}_{k + 1}$, 得到下式（注意 $\mathbf{K}$ 中含有的矩阵。）

其卡尔曼增益为：

式 $(\oplus), (\otimes), (\oslash)$ 即为所求。

在这之后，我们可以将 $k$ 的值「减一」并复用我们得到的当前最佳估计，不断循环下去。

这篇文章是 bzarg 的这篇文章的摘要。如果你希望获得更好的阅读体验，不妨去看一下。毕竟对面有图。