卡尔曼滤波系列——经典卡尔曼滤波推导

最近在看一本很不错的计算机视觉基础书籍——《计算机视觉：模型、学习和推理》。

个人认为这本书挺适合计算机视觉的初学者，感兴趣的读者可以买一本或者在网上找一本电子版读一读，内容有一定的难度，翻译质量我觉得挺好，不影响理解。

之前，在知乎上看到了一个回答，个人觉得说的很有道理，链接如下：

https://www.zhihu.com/question/293700785/answer/700094530

里面提到的对“计算机视觉”的成体系的认识这一点，一度令我深思，毕竟像我这种半路出家的，对CV缺少系统性地学习，为了能够更好地投身到计算机视觉领域中，而不是蹭热度，个人认为潜心学习CV的基础知识是很有必要的，这能决定一个CVer在这条路上究竟走多远。

话不多说，开始正文

关于卡尔曼滤波这一古老的方法，几乎在很多领域都在发挥着作用，关于它的讲解和推导，也有太多太多的人讲过了，这里只是记录一下我自己的推导方式，因为为了推导这个，我花了一个多小时……感觉不记录下来，都对不起自己~。

在推导之前，先介绍几个必要的知识点：

1、舒尔补恒等式（Schur complement identity）

第一次看到这个公式的时候，我头都要炸了。。。这个公式我们会在后续的从二元联合高斯分布中推导二元条件高斯分布  时候会用到的。

2、二元条件高斯分布

对于二元变量  ，其二元高斯分布表达式如下：

上式即两个变量的联合高斯分布表达式，有的时候，我们需要考察二者之间的条件高斯分布，比如：  。这个问题是《计算机视觉：模型、学习和推理》的一道书后习题5.5。接下来，我们来求解一下从联合推导出条件：

首先，对于一个高斯分布，其关键部分在于指数函数中的二次项：

将其展开：

其中：

第一项是二次项，中间两项是一次项，注意到这两个一次项是都是标量，因此彼此的转置是相等的（协方差矩阵是对称的），所以可以合并成一项  ，不过，我们只看一半  就行，另一半  不用看，最后一项就是常数项了。

请注意，协方差矩阵出现在二次项中，而均值  出现在一次项中，这个性质很有用，对于我们在推导复杂的高斯分布时，利用好这一点，有利于我们求出协方差矩阵 和均值 。

OK，回到上面的联合分布，我们写出其指数中的二次项：

其中  （此时，是不是想到了舒尔补恒等式呢？）

然后，我们展开就会得到一长串的结果，这里，为了展示的方便，我划分成四行：

现在，我们关心的是：在给定  下的  条件高斯分布：

 。

此时  就相当于是常量，我们只需要在上面的四行中关注  即可。前面已经说过了，高斯分布中，协方差矩阵在二次项中，均值在一次项中。因此，接下来，我们把他们找出来即可（由于一次项的对称性，我们只需看一半就行，另一半不需要。）：

二次项： 

一次项： 

好了，均值和方差我们就得到了：

那么，  又等于什么呢？这个时候，开头的舒尔补恒等式就派上用场了，往里面一套，就能得到：

到此，我们就得到了二元条件高斯分布：

这个公式，在后续推导卡尔曼滤波的时候会用到。

3、卡尔曼滤波

卡尔曼滤波是一个典型的马尔科夫链模型，如下图所示：

变量  是系统的状态(  )，比如相机的位姿，而变量  是观测(  )，比如相机所观测到的空间点的坐标。对于这样的系统，我们不妨用线性方程来建模：

状态转移方程：

观测方程：

其中，  是状体转移矩阵，  是观测矩阵，  和  是相应的噪声。然后，对于时刻 ，进行概率建模：

 ， 

 ， 

在马尔可夫假设下，该系统有两个性质：

1. 当前状态  只与前一时刻  有关： 
2. 当前观测  只与当前时刻的状态有关： 

我们所要研究的问题就是：在已知时刻  的状态 和观测 ，如何求得时刻  的状态 概率分布呢？

首先，从状态转移方程，我们可以很容易预测出状态 的均值和方差：

 ，很容易求出来： 

 ，推导略： 

这一步，我们称之为“预测”，因为我们是直接用状态转移方程从时刻推导出来的，并没有借助观测，因此这样的预测结果的方差会变大（想想你闭着眼走楼梯，是不是越走心里越忐忑，忍不住要睁开眼睛瞅瞅呢？）。

如果我们这个时候获得了观测，那均值和方差又会有什么变化呢？即，我们更加在意的是：  。这一步，称之为“校正”，因为我们可以借助观测来修正我们的预测。

为了求出  ，我们首先考察下面的联合概率：

 我们已经知道了，而  也很容易利用观测方程求出：

 利用协方差定义+观测方程也很容易地就能求出来：

于是，便得到了具体表达式：

那么，如何求  就非常容易了，这就是个从联合高斯分布中  求条件高斯分布，公式我们已经在前面推到过了，往里一套就ok了，结果如下：

注意到上面两行有一个相同的因子  ，这就是所谓的“卡尔曼增益”，记为  。

我们把上面的公式总结一下：

1.预测：

2.计算卡尔曼增益：

3.校正：

这就是大名鼎鼎的“卡尔曼滤波”。

以上并不是一个很严谨的数学推导，推导思路比较直观，只需要关注我们每一步要得到什么即可，所以还请大家不要太过于较真里面的一些投机取巧的细节，关于用到的两个数学知识点，舒尔补恒等式和条件高斯分布，在很多问题中都能用上，还希望大家能够掌握。更严谨的卡尔曼滤波推导（也就意味着更复杂）大家可以看《计算机视觉：模型、学习和推理》第19章，或者是《机器人学中的状态估计》。

作者：知乎-Kissrabbit

地址：https://www.zhihu.com/people/yang-jian-hua-63-91

历史文章推荐

你正在看吗？👇