卡尔曼滤波算法概要

/ 0评 / 0

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

核心思想

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

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

步步推导

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

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

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

$$ \begin{bmatrix}Cov(x_1, x_1) & Cov(x_1, x_2) & \dots & Cov(x_1, x_n) \\ Cov(x_2, x_1) & Cov(x_2, x_2) & \dots & Cov(x_2, x_n) \\ \vdots & \vdots & \ddots & \vdots \\ Cov(x_n, x_1) & Cov(x_n, x_2) & \dots & Cov(x_n, x_n) \end{bmatrix} $$

状态可能是时变的。我们将当前时间记为 \(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} \).

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

$$\mathbf{P}_{k + 1} = \mathbf{F}_{k+1}\mathbf{P}_{k}\mathbf{F}_{k+1}^{T}$$

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

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

$$\hat{\vec{x}}_{k + 1} = \mathbf{F}_{k + 1}\hat{\vec{x}} + \mathbf{B}_{k + 1}\vec{u}_{k + 1} ~~~~~~~~(\circ) $$

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

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

$$\mathbf{P}_{k + 1} = \mathbf{F}_{k + 1}\mathbf{P}_{k}\mathbf{F}_{k + 1}^{T} + \mathbf{Q}_{k + 1} ~~~~~~~~(\diamond)$$

现在,我们就得到了关于系统的未来的一个预测:式 (\(\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 \) 之间的关系。一顿暴力计算可以得出:

$$\mu_? = \mu_0 + \frac{\sigma^2_0(\mu_1 - \mu_0)}{\sigma^2_0 + \sigma^2_1} $$

$$\sigma^2_? = \sigma^2_0 - \frac{\sigma^4_0}{\sigma^2_0 + \sigma^2_1}$$

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

$$\mu_? = \mu_0 + b (\mu_1 - \mu_0)$$

$$ \sigma^2_? = \sigma^2_0 - b\sigma^2_0 $$

我们再将其应用到矩阵上,令 \(\Sigma\) 为随机变量的协方差矩阵,则:

$$ \mathbf{K} := \Sigma_0 ( \Sigma_0 + \Sigma_1)^{-1} $$

$$ \vec{\mu_?} = \vec{\mu_0} + \mathbf{K} (\vec{\mu_1} - \vec{\mu_0}) $$

$$ \Sigma_? = \Sigma_0 - \mathbf{K} \Sigma_0 $$

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

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

$$ \mathbf{H}_{k + 1}\hat{\vec{x\prime}}_{k + 1} = \mathbf{H}_{k + 1}\hat{\vec{x}}_{k + 1} + \mathbf{K}(\hat{\vec{z}}_{k + 1} - \mathbf{H}_{k + 1}\hat{\vec{x}}_{k + 1} ~~~~~~ (1) $$

$$ \mathbf{H}_{k + 1}\mathbf{P\prime}_{k+1}\mathbf{H}^{T}_{k + 1} = \mathbf{H}_{k + 1}\mathbf{P}_{k + 1}\mathbf{H}^{T}_{k + 1} - \mathbf{K}\mathbf{H}_{k + 1}\mathbf{P}_{k+1}\mathbf{H}^{T}_{k+1} ~~~~~~ (2) $$

其中,卡尔曼增益为:

$$\mathbf{K} = \mathbf{H}_{k + 1}\mathbf{P}_{k+1}\mathbf{H}^{T}_{k+1}(\mathbf{H}_{k + 1}\mathbf{P}_{k+1}\mathbf{H}^{T}_{k+1} + \mathbf{R}_{k+1})^{-1} $$

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

$$\hat{\vec{x\prime}}_{k + 1} = \hat{\vec{x}}_{k + 1} + \mathbf{K}(\hat{\vec{z}}_{k + 1} - \mathbf{H}_{k + 1}\hat{\vec{x}}_{k + 1}) ~~~~~~ (\oplus)$$

$$\mathbf{P\prime}_{k+1} = \mathbf{P}_{k + 1} - \mathbf{K}\mathbf{H}_{k + 1}\mathbf{P}_{k+1} ~~~~~~ (\otimes) $$

其卡尔曼增益为:

$$\mathbf{K} = \mathbf{P}_{k+1}\mathbf{H}^{T}_{k+1}(\mathbf{H}_{k + 1}\mathbf{P}_{k+1}\mathbf{H}^{T}_{k+1} + \mathbf{R}_{k+1})^{-1} ~~~~~~ (\oslash) $$

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

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


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

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注

Your comments will be submitted to a human moderator and will only be shown publicly after approval. The moderator reserves the full right to not approve any comment without reason. Please be civil.