滤波估计理论（二）——卡尔曼滤波器（Kalman Filter）

线性高斯模型

我们知道，贝叶斯滤波的具体形式取决于先验概率分布、系统动态模型等多个方面，二者不同的搭配会带来不同的结果，因此这里我们首先来看一个最为简单的搭配——线性模型+高斯分布。

对于一个线性系统来说，其状态空间模型可以写成如下的矩阵形式：
$$\{\begin{array}{rl}& {\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{q}}_{k-1}\\ & {\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{r}}_k\end{array}$$

式中矩阵${\mathbf{A}}_{k-1}$和${\mathbf{H}}_k$分别称为状态转移矩阵和量测矩阵，${\mathbf{q}}_{k-1}\sim N(0,{\mathbf{Q}}_{k-1})$和${\mathbf{r}}_k\sim N(0,{\mathbf{R}}_k)$分别为服从零均值高斯分布的过程噪声和量测噪声，系统的初值同样服从高斯分布${\mathbf{x}}_0\sim N({\mathbf{\mu }}_0,{\mathbf{P}}_0)$。

根据高斯分布的线性性，我们可以很容易将该状态空间模型转化为概率状态空间模型：
$$\begin{array}{rl}& p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})\sim N({\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1},{\mathbf{Q}}_{k-1})\\ & p({\mathbf{z}}_k|{\mathbf{x}}_k)\sim N({\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)\end{array}$$

在进一步推导Kalman滤波的具体形式之前，我们需要先介绍关于Gaussian变量的两个重要性质。

高斯变量的联合分布与条件分布

高斯变量的联合分布（条件->联合）

对于两个高斯分布随机变量$\mathbf{x}\sim {\mathbb{R}}^n$和$\mathbf{y}\sim {\mathbb{R}}^m$，已知边缘分布$p(\mathbf{x})$和条件分布$p(\mathbf{y}|\mathbf{x})$如下：
$$\begin{array}{rl}p(\mathbf{x})& \sim N(\mathbf{\mu },\mathbf{P})\\ p(\mathbf{y}|\mathbf{x})& \sim N(\mathbf{H}\mathbf{x}+\mathbf{u},\mathbf{R})\end{array}$$

则$\mathbf{x}$和$\mathbf{y}$的联合分布以及$\mathbf{y}$的边缘分布如下：
$$\begin{array}{rl}\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]& \sim N\left(\left[\begin{array}{c}\mathbf{\mu }\\ \mathbf{H}\mathbf{\mu }+\mathbf{u}\end{array}\right],\left[\begin{array}{cc}\mathbf{P}& \mathbf{P}{\mathbf{H}}^T\\ \mathbf{H}\mathbf{P}& \mathbf{H}\mathbf{P}{\mathbf{H}}^T+\mathbf{R}\end{array}\right]\right)\\ \mathbf{y}& \sim N(\mathbf{H}\mathbf{\mu }+\mathbf{u}，\mathbf{H}\mathbf{P}{\mathbf{H}}^T+\mathbf{R})\end{array}\text{\hspace{1em}(1)}$$

高斯变量的条件分布（联合->条件）

反之，如果我们已知高斯随机变量$\mathbf{x}$和$\mathbf{y}$的联合分布为：
$$\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]\sim N\left(\left[\begin{array}{c}\mathbf{a}\\ \mathbf{b}\end{array}\right],\left[\begin{array}{cc}\mathbf{A}& \mathbf{C}\\ {\mathbf{C}}^T& \mathbf{B}\end{array}\right]\right)$$

则$\mathbf{x}$和$\mathbf{y}$各自的边缘分布以及条件分布如下：
$$\begin{array}{rl}p(\mathbf{x})& \sim N(\mathbf{a},\mathbf{A})\\ p(\mathbf{y})& \sim M(\mathbf{b},\mathbf{B})\\ p(\mathbf{x}|\mathbf{y})& \sim N(\mathbf{a}+\mathbf{C}{\mathbf{B}}^{-1}(\mathbf{y}-\mathbf{b}),\mathbf{A}-\mathbf{C}{\mathbf{B}}^{-1}{\mathbf{C}}^T)\\ p(\mathbf{y}|\mathbf{x})& \sim N(\mathbf{b}+{\mathbf{C}}^T{\mathbf{A}}^{-1}(\mathbf{x}-\mathbf{a}),\mathbf{B}-{\mathbf{C}}^T{\mathbf{A}}^{-1}\mathbf{C})\end{array}\text{\hspace{1em}(2)}$$

式中$\mathbf{A}-\mathbf{C}{\mathbf{B}}^{-1}{\mathbf{C}}^T$和$\mathbf{B}-{\mathbf{C}}^T{\mathbf{A}}^{-1}\mathbf{C}$分别称为矩阵$\mathbf{B}$和矩阵$\mathbf{A}$相对于协方差矩阵的舒尔补（Schur Complement）。上述所有公式都可以通过写出对应的概率密度函数并进行整理简化即可，并没有什么复杂的变换过程，因此具体过程这里略过，只要记忆对应的结论即可。

线性高斯Kalman滤波

状态预测

现在我们接着回到线性高斯模型下的贝叶斯滤波中，同样在未获得$k$时刻的量测值${\mathbf{z}}_k$之前，我们可以根据$k-1$时刻的系统状态对$k$时刻的系统状态做出预测，即求解在给定${\mathbf{z}}_{1:k-1}$下${\mathbf{x}}_{k-1}$和${\mathbf{x}}_k$的联合分布$p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})$，根据贝叶斯公式有：
$$p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})=p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})$$

由于${\mathbf{x}}_k$和${\mathbf{x}}_{k-1}$均与${\mathbf{z}}_{1:k-1}$无关，因此$p({\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})$就相当于边缘概率$p({\mathbf{x}}_{k-1})$，根据式（1）有：
$$\begin{array}{rl}\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{x}}_{k-1}\end{array}\right]& \sim p({\mathbf{x}}_k,{\mathbf{x}}_{k-1}|{\mathbf{z}}_{1:k-1})\\ & =p({\mathbf{x}}_k|{\mathbf{x}}_{k-1})p({\mathbf{x}}_{k-1})\\ & =N({\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1},{\mathbf{Q}}_{k-1})N({\mathbf{\mu }}_{k-1},{\mathbf{P}}_{k-1})\\ & =N\left(\left[\begin{array}{c}{\mathbf{\mu }}_{k-1}\\ {\mathbf{A}}_{k-1}{\mathbf{\mu }}_{k-1}\end{array}\right],\left[\begin{array}{cc}{\mathbf{P}}_{k-1}& {\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T\\ {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}& {\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_k\end{array}\right]\right)\end{array}$$

进而可由式（1）得到边缘分布$p({\mathbf{x}}_k|{\mathbf{z}}_{1:k-1})$为：
$$p({\mathbf{x}}_{\mathbf{k}}|{\mathbf{z}}_{1:\mathbf{k}-1})=p({\mathbf{x}}_k)\sim N({\mathbf{A}}_{k-1}{\mathbf{\mu }}_{k-1},{\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_k)\text{\hspace{1em}(3)}$$

式（3）即为Kalman滤波的一步预测过程，为了方便后续书写，这里我们使用上标$\widehat{(\cdot )}$表示预测值，简记为$p({\mathbf{x}}_{\mathbf{k}}|{\mathbf{z}}_{1:\mathbf{k}-1})\sim N({\hat{\mathbf{\mu }}}_k,{\hat{\mathbf{P}}}_k)$。

量测更新

在获得了$k$时刻量测信息${\mathbf{z}}_k$的条件分布$p({\mathbf{z}}_k|{\mathbf{x}}_k)$之后，同样根据式（1）有：
$$\begin{array}{rl}p({\mathbf{z}}_k,{\mathbf{x}}_k|{\mathbf{z}}_{1:k-1})& =p({\mathbf{z}}_k|{\mathbf{x}}_k)p({\mathbf{x}}_k|{\mathbf{z}}_{k-1})\\ & =N({\mathbf{H}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)N(\widehat{{\mathbf{\mu }}_k},{\hat{\mathbf{P}}}_k)\\ \Rightarrow \left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{z}}_k\end{array}\right]& \sim N(\left[\begin{array}{c}{\hat{\mathbf{\mu }}}_k\\ {\mathbf{H}}_k{\hat{\mathbf{\mu }}}_k\end{array}\right],\left[\begin{array}{cc}{\hat{\mathbf{P}}}_k& {\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T\\ {\mathbf{H}}_k{\hat{\mathbf{P}}}_k& {\mathbf{H}}_k{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+{\mathbf{R}}_k\end{array}\right])\end{array}$$

虽然根据公式我们最后求得的应为${\left[\begin{array}{cc}{\mathbf{x}}_k|{\mathbf{z}}_{1:k-1}& {\mathbf{z}}_k|{\mathbf{z}}_{1:k-1}\end{array}\right]}^T$的联合分布，但由贝叶斯估计的Markov性和条件独立性可知，${\mathbf{x}}_k$和${\mathbf{z}}_k$均与${\mathbf{z}}_{1:k-1}$无关，因此实际上$p({\left[\begin{array}{cc}{\mathbf{x}}_k|{\mathbf{z}}_{1:k-1}& {\mathbf{z}}_k|{\mathbf{z}}_{1:k-1}\end{array}\right]}^T)=p({\left[\begin{array}{cc}{\mathbf{x}}_k& {\mathbf{z}}_k\end{array}\right]}^T)$。进一步根据式（2）有：
$$p({\mathbf{x}}_k|{\mathbf{z}}_k,{\mathbf{z}}_{1:k-1})=p({\mathbf{x}}_k|{\mathbf{z}}_k)\sim N({\tilde{\mathbf{\mu }}}_k,{\tilde{\mathbf{P}}}_k)$$

我们使用上标$\widetilde{\left(\cdot \right)}$来表示估计值，则${\tilde{\mathbf{\mu }}}_k$和${\tilde{\mathbf{P}}}_k$分别为：
$${\tilde{\mathbf{\mu }}}_k=\hat{\mathbf{\mu }}+{\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+{\mathbf{R}}_k\right)}^{-1}\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{\mu }}}_k\right)=\hat{\mathbf{\mu }}+\mathbf{K}{\hat{\mathbf{z}}}_k\text{\hspace{1em}(4)}$$

$${\tilde{\mathbf{P}}}_k={\hat{\mathbf{P}}}_k-{\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+{\mathbf{R}}_k\right)}^{-1}{\mathbf{H}}_k{\hat{\mathbf{P}}}_k={\hat{\mathbf{P}}}_k-\mathbf{K}{\mathbf{H}}_k{\hat{\mathbf{P}}}_k\text{\hspace{1em}(5)}$$

式（4）和式（5）即为Kalman滤波的量测更新过程，其中矩阵$\mathbf{K}$称为Kalman增益矩阵，其表征的是将对预测的状态值和量测的状态值进行加权估计。${\hat{z}}_k$称为Kalman新息或残差，其表征的是我们实际的测量值和我们的预测值之间的偏差。

总结

可以看出，虽然经典的Kalman滤波算法最初是从最优控制的中的最小二乘估计问题得来的，但Kalman就是贝叶斯滤波在线性高斯模型下的闭合解。式（3）~式（5）即为经典Kalman的五步估计方程，这里整理如下：
$$\begin{array}{rl}一步预测& \{\begin{array}{rl}{\hat{\mathbf{\mu }}}_k& ={\mathbf{A}}_{k-1}{\mathbf{\mu }}_{k-1}\\ {\hat{\mathbf{P}}}_k& ={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_k\end{array}\\ 量测更新& \{\begin{array}{rl}\mathbf{K}& ={\hat{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\hat{\mathbf{P}}}_k{\mathbf{H}}^T+{\mathbf{R}}_k\right)}^{-1}\\ {\tilde{\mathbf{\mu }}}_k& =\hat{\mathbf{\mu }}+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{\mu }}}_k\right)\\ {\tilde{\mathbf{P}}}_k& ={\hat{\mathbf{P}}}_k-\mathbf{K}{\mathbf{H}}_k{\hat{\mathbf{P}}}_k\end{array}\end{array}$$

在推导经典Kalman滤波的过程中，我们用到了线性系统模型和Gaussian分布两个假设中。其中Gaussian假设在大多数情况下都可以默认是合理的，但线性模型假设则并不符合实际生活中的大多数情况。因此后续我们将舍弃线性模型假设，从非线性的角度与进一步看贝叶斯滤波的特殊形式（例如EKF、UKF等）。