zkk9527

SLAM14讲学习笔记（六）后端（最难一章：卡尔曼滤波器推导、理解以及扩展）

这一章的内容并不多，不像视觉里程计那章的内容那么厚。但是我认为它是整个SLAM十四讲中最难的一章，难点主要在于卡尔曼滤波器的推导涉及到一屁股的数学公式，非常的讨厌，如果不能静下心来，就很容易被吓到。但是真正看懂以后实质上它是比较简单的，并没有用到什么太过复杂的变换（不像傅里叶变换就十分的抽象，这也是我从电信科专业转到计算机专业的主要原因之一，因为我实在厌烦了与傅里叶变换打交道。），本质上这节内容掌握一些基本的数理统计和矩阵知识就可以明白含义了。如果实在不明白推导过程，我最后也总结了怎么从宏观上明白卡尔曼滤波器的作用。希望这节的内容可以帮助到大家！另外很乐意能和相关领域的研究者有交流，所以欢迎留言交流。

另外，配一个跟推导无关的图画解释，从现实意义上讲解什么是卡尔曼滤波器：

视觉SLAM14讲学习笔记（十五）卡尔曼滤波器的直观理解

状态估计的概率解释：位姿x和路标y服从某种概率分布，目的是通过某些运动数据u（比如惯性测量传感器IMU输入）和观测数据z（比如拍摄到的照片像素点的值）来确定状态量x和y的分布。

再次看一下状态方程和观测方程，第一个状态方程表明了根据上一时刻k-1的状态、传感器的读数u和噪声误差wk计算得到这一时刻k的状态，第二个观测方程实际上指的就是拍摄的图像，是根据k时刻的实际状态xk和地标y（把两者写到一起）、像素误差vk得到这一时刻的观测数据zk。

先提前介绍两个概念的实际意义（这块一定要深刻明白，再往下读）：

先验：根据0~k-1时刻的各种数据来推测k时刻的状态

后验：结合了k时刻观测数据的k时刻状态。

（比如这章的卡尔曼滤波在做的事情，目的就是从k-1时刻的后验数据来推测k时刻的先验数据，再用k时刻的先验数据推k时刻的后验数据。）

关于这部分内容到现在为止先有个大致了解，下面我将进行展开叙述：

我们希望的目的，就是根据0~k时刻所有的数据，来观测当前的状态。那么这就可以写成：

下标没必要叙述大家应该都能明白。这个东西怎么来算呢？就是根据贝叶斯法则拆开。先回忆一下贝叶斯法则：

贝叶斯等式左边的正比于贝叶斯等式右边的分子，因为分母是在求和，不会有选择和改变的余地。

因此式子写成：

（其实对于这个式子的推导，我个人觉得不应该直接这么写，为了方便理解应该先写一步中间过程，即左边先正比于P（X0，U1：k，Z1：k | Xk）* P(Xk)，这样才符合贝叶斯公式的直接推导。然后对于第一项，显然k时刻的先验并不能决定除了Zk以外的在k时刻之前的数据，因此第一项就等于P（Zk | Xk)，第二项单独一个Xk拿出来是什么？是没有Zk情况下的k时刻状态，它也就等于在前面的记录的x0，z1：k-1……发生情况下产生的数据。）

这个式子不应该写到这里就直接继续推导了，而是应该先停一下，看看其中每部分的含义：

1. 等式左边是我们想要知道的，根据0~k时刻所有的数据，来得到当前的状态，

2. 等式右边第二项，是根据0~k-1时刻的观测数据，推测得知k时刻的先验状态（对于这个，里面的u作为一个传感器数据，其实可以忽略掉它，因为0~k-1时刻的数据不应该包含uk，但是本书里面着重要讲的是视觉SLAM，u应该指的是惯性逻辑单元IMU之类的数据，如果只依靠观测的图像来判断状态的话，忽略掉u就行了）

3.等式右边第一项，是“给定了一个先验Xk的情况下，什么观测数据Zk最能符合这个先验“的概率。因此这就是所谓的似然。

4.把整个式子联合起来看，"根据0~k时刻所有的数据得到的k时刻的状态"所服从的概率分布，是正比于"根据0~k-1时刻的观测数据，推测得知k时刻的先验状态"的概率分布与 "由k时刻产生的先验状态的基础上最符合的观测数据zk"的概率分布的乘积！

即：后验=似然*先验。

注意这里的先验，被表示为，它只是表示了“在有0时刻的状态和后续的观测数据”的情况下，推测k时刻的状态。我们如果想把它写的紧凑一些，弄成k-1时刻的状态和k时刻的状态的关系，那么就用如下表示：

这个式子是怎么得出来的呢，这其实本身是一个全概率公式：

B就是先验Xk，非要多出一项Xk-1的话，那么就把Xk-1当成是A。（注意上面的全概率公式和上面的10.6公式等式右边的两项是相反顺序的，懒得手打公式了，直接找了个公式截图，顺序刚好左右相反，但是乘积是一样的）

那么对离散的A求和，和对连续的状态求积分是一样的。

如果把先验的展开代回到后验=似然*先验公式里去的话，也就是很多论文里经常出现的马尔科夫链：

这个就很清晰了，后验=似然*先验的展开，似然意味着给定状态解释当前测量的概率，先验的展开意味着系统的动态变化。个人觉得高博最开始就应该从这个公式入手，然后再逐步加入什么X0，u0：k……

到这里为止，如果认为k时刻的状态只与k-1时刻有关，那就是用扩展卡尔曼滤波器（EKF）来做。如果觉得和所有状态都有关，那就是用非线性优化来做。目前主流是非线性优化。

再放一下10.6式：

如果是假设了马尔科夫性，即k时刻状态只与k-1时刻的状态有关，那么没用的都删掉，等式右边第一项就变成了：

第二项变成：

（k-1时刻的状态与之后的传感器读数uk无关）

这第二项就变成了等式左边的形式，这说明这个先验的积分可以逐渐不断的向更早时间推移，这说明只要维护一个状态量就可以了。

扩展卡尔曼滤波器的定义在讲述之前，先应该知道卡尔曼滤波器，这也是这章公式最多的一块，前所未有的多，但是细读并不是很难。我们这里先树立一个主线，那就是：后验=似然*先验，这之前已经推导过了。之所以要把这个主线树立起来，是因为这节推导比较繁琐，防止中途忘记了自己是想干什么，以至于之后进行不下去了。在推导的过程中要牢牢抓住这个主线！

先假设状态量都服从高斯分布，这是一个线性高斯系统：

这时候就引入了附录A.3

由于状态方程是，我们应该再补充一个Xk-1时刻的后验分布：

然后按照附录A.3里的推导，用尖帽子xk表示后验，横线帽子xk表示先验，那么k时刻的先验应该是：

这样正态分布里的第一项均值和第二项的协方差，就是k时刻的先验：

这样我们主线里的 “后验=似然*先验” 的先验就表达出来了，即先验=

根据主线里面，似然还不知道。这个根据观测方程：

得到似然的高斯分布：P（Zk|Xk)=。

关于这块可能会有一个疑问：这里的协方差是一个Q，为什么不套用附录，得到协方差是CkP^k-1CkT+Q了（这里只是一个Q）？

我个人的理解是：因为这里的Xk已经由状态方程产生了，作为一个实实在在的确定的Xk值，对应的Zk的协方差自然是0。（虽然Xk的先验均值服从于N（Xk横线帽子，Pk横线帽子），但是对于一个确定的Xk，它就是它自身，因此协方差是0。而状态方程的推导过程中，Xk的分布受Xk-1分布的影响。

用尖帽子表示后验概率，那么主线的“后验=似然*先验”就是：

为什么一定要抓住这个主线呢，是因为我们最终是想要比较靠谱的后验状态，而后验状态是根据似然和先验算出来的。

高斯分布的表示形式主要是指数部分不同，把上面的式子两边的指数展开，先看看附录2.1：

根据上面的属性，展开以后就是：

看到这里还是先停一下，以防在公式推导里迷失掉。

上面的xk，指的是状态xk所满足的分布，它可能是任何值，这些值暗中满足一个高斯分布；

xk带尖帽子，指的是后验的xk满足的分布均值状态；xk带横线帽子，指的是先验的xk满足的分布的均值状态；Pk尖帽子是K时刻后验状态满足的分布的协方差，Pk横线帽子是K时刻先验状态满足的分布的协方差，Q是观测方程的噪声满足的协方差。（状态方程的噪声满足的协方差R去了哪里？被写进了Pk横线帽子里面）Ck是状态方程中zk=Ck*xk+vk

在对这个公式里的各个参数足够了解以后，再继续往下看：

在上面的方程里，xk是未知数，总的思路是展开上式，通过两边对于xk的一次与二次系数相等，从而确定等式左边的后验Xk（尖帽子）和等式右边的先验Xk（横线帽子）之间的关系！这就是公式进一步推导的目的。

书上没有展开，我把它亲手推了一遍：

（值得注意的是，这个等式中的每一项都是一个常数，对于常数来说，求它的转置与它本身是相同的，举例来说：

这两个一次项怎么合并成的？答：就是前者求转置等于其本身，然后合并。另外还有一个问题，P，Q这种转置等于自身吗？答：是的，因为它们是协方差矩阵，根据定义是对称非负定矩阵）

到现在为止，得到两个等式：

这个是二次项系数的等式；

这个是一次项系数的等式；

先说二次项系数的：

文中的处理方式是把左边化成单位矩阵I：

然后定义一个中间变量K（这个K其实就是所谓的卡尔曼增益）：

从而得到：

我们主线的目的是，建立起先验和后验的关系，因此要把这个式子移项：

因此我们可以看出，卡尔曼增益K描述了“后验状态满足的分布的协方差”与“先验状态满足的分布的协方差”之间的关系。

不过这里有个很蛋疼的事情：在之前K的定义中，是由后验协方差Pk尖帽子和观测方程系数Ck和观测方程噪声协方差Q表述的，也就是说K是由后验协方差Pk尖帽子定义的，上面这个式子后验协方差Pk尖帽子又是由K定义的，到底是先有鸡还是先有蛋？循环定义。关于这点，高博在注释里说，K可以不依靠后验的协方差Pk尖帽子算出（这是必定的，不然既然我知道了后验协方差还算什么卡尔曼增益？算卡尔曼增益的目的是，根据先验的协方差Pk横线帽子来计算后验协方差Pk尖帽子）

关于这点，现在留心一下，一会下文还要再说。

再说一次项系数的：

这个直接看高博书里的推导吧：

也就是说，后验的状态均值Xk尖帽子，等于先验的状态均值Xk横线帽子加上一个修正量。

修正量表述为

记得最开始的观测方程：

zk-CkXk应该是vk，也就是像素误差，因此10.22和10.23里的实际含义就是：

后验的位姿状态与先验的位姿状态的差异，事实上就是（“观测值”与“估计的观测值”之间的误差）乘以一个（卡尔曼增益）！

（这是书里没有详细写明的地方，如果上面的公式没太看明白怎么推导的，也没有关系，因为如果现在把资料都收起来，就给我一张纸和笔叫我裸推，我大概率也推不出来。我觉得等我准备找工作的时候，再准备裸推。）

所以到这里卡尔曼滤波器就快结束了，一起再总结下：

卡尔曼增益表达了两层性质：

第一：后验协方差与先验协方差之间的关系，即：

第二，后验状态均值与先验状态均值之间的关系：

不过，之前我们还有一个历史遗留问题，那就是卡尔曼增益的定义，本身包括了后验协方差，也就是说我们实际利用卡尔曼增益K，根据先验计算后验的时候，是没法计算的，因为你想求结果需要用到K，但是K里面包含了结果。（也就是刚刚说的循环定义问题）高博给了一个不同的步骤：

这个步骤也就是：

1.先根据k-1时刻的后验分布的均值和协方差，算k时刻的先验状态的均值与协方差；（所谓的预测，A是状态方程系数，R是状态方程的噪声协方差，之后的C是观测方程的系数，Q是观测方程的噪声协方差）

2.计算卡尔曼增益K（注意，这里的K定义就不再是循环定义了，它用到的是K时刻的先验协方差而不是后验协方差，因为是想计算后验协方差。问题：这里的K定义与之前的相同吗？答：相同，据高博所言，用到了SWB恒等式，其证明过程比较繁琐）

3.利用卡尔曼增益，根据k时刻状态的先验均值和协方差，计算k时刻状态的后验均值与协方差。

值得注意一点：卡尔曼滤波器有若干种推导方式，这里是从概率角度出发的最大后验概率估计的形式。卡尔曼滤波器构成了线性系统的最优无偏估计。

到上面为止，卡尔曼滤波器的内容就结束了。不得不感慨数学真是博大精深，一切工科专业归根到底就是数学。

卡尔曼滤波器针对的是线性系统，高斯分布。而这是有问题的，一个高斯分布经过非线性的变换，结果往往不再是高斯分布。

接下来讲解扩展卡尔曼滤波器（Extended Kalman Filter，EKF），这部分内容也要抓住一个主线：

EKF是KF的修正，原理是相同的，但是是在某个点附近，对运动方程与观测方程一阶泰勒展开，保留一阶项（线性部分），然后按照KF方式来推导。

把偏导数记为F和H。

还是把握KF的主线：后验=先验*似然

这里的非线性的近似先验就满足：

顺便放上线性的卡尔曼滤波器的比较一下：

非线性的近似先验是怎么得出的？其实是：

等式左边满足正态分布，右边也是，根据附录A.3：

此处，相当于是y，相当于是x，相当于是Ax，相当于是b。然而的均值是0，因此也就是的均值就是，方差是

也就是：中，均值和方差的由来。

对于观测方程，也就是似然，分布满足于：

注意这里，在状态方程的推导中，的均值是0，而在这里，的均值不是0，这个我们很早就解释过了：以Xk-1为例：

这里的协方差为什么还是只是一个Q而不是根据附录得到的HPk-1HT+Q，我的理解还是和卡尔曼那里一样，因为是先产生先验再产生似然，先验已经产生了那就是确定的值了，不用满足某个分布。（这里可能不对，欢迎指正）

最后得到的卡尔曼增益：

是扩展卡尔曼滤波器那里原先的系数C这里换成了偏导数H。

后验概率和先验概率分布的关系为：

这个给出的是单次线性近似下的最大后验估计（MAP）

讨论：EKF 的局限：

1.是基于马尔科夫性的，但如果当前帧与很久前有关，滤波器难以处理。（不如非线性优化可以考虑很久前的状态）

2.EKF是用泰勒展开一次导数的系数作为线性近似，但如果实际情况有着强烈的非线性，那就会有非线性误差。

3.EKF slam由于视觉slam中路标数量很大，因此存储量呈平方增长。

关于这部分我的理解是，书里仅仅是介绍概念和理论，其实看完，哪怕实际推导完一遍以后，也不是很明白该怎么用，比如怎么利用代码来实现（扩展）卡尔曼滤波器。书上的代码给的是BA的过程，也强调了slam中一般是使用非线性优化的方式。