A Sliding Window Filter for SLAM

这篇专题的idea主要来源于2006年Gabe Sibley的论文A Sliding Window Filter for SLAM，此论文详细说明了SLAM后端优化的稀疏性的缘由，并提出了应用于slam中Sliding window filter算法，在进入主题之前，小编先向大家安利一波。

接下来，小编就给大家分享一下读了论文之后的心得。老生常谈地，先简单说明一下slam这个问题的数学模型，机器人在运动过程中，通常会记录大量pose，以及landmark,我们用x_p表达pose，用x_m表达landmark，那么如果在运动过程中，有m个pose，n个landmark，整个向量可以写成这样的形式：

 

对于空间中的三维运动，一般pose是一个6维的向量(位置和姿态)，landmark是一个三维的向量(位置)，在这样一次运动过程中，由于我们能获得大量的pose和landmark，而且他们总是充满了一些联系，因此，接下来的任务，就是对这样的联系进行数学建模

 

Process Model

首先我们要建立pose之间的联系，一个简单的方法自然就是使用运动学公式，我们可以通过惯性传感器(IMU)测量到加速度(acceleration)和角速度(angularvelocity),这样通过积分以及给定的上一时刻的pose和速度(velocity)，直接就可以算到当前的pose了，当然，其实也可以建立其他很复杂的关系，但是我们总是希望这个关系是马尔科夫性的，也就是如果知道了上一时刻的pose，那么当前的pose和上一时刻之前的pose信息是相互独立的，或者说可以认为上一时刻的pose已经包含了之前pose的所有信息，这样的模型也许并不那么完美，比如说传感器随着时间精度有所变化等，可能得不到那么完整的信息，导致模型不满足马尔科夫性，这是完全有可能的，但是我们也这样去近似了，就认为当前状态仅仅之和前一时刻的状态有关，和更前面的状态没有关系，由此，我们有这样一个抽象的模型：

其中u代表输入，对于速度加速度模型可以理解为速度，如果建立了动力学模型，完全可以理解为力和力矩等，是一个给定的具体值，w代表噪声(均值为0，方差为Q)，如果我们把每个时刻的pose方程拼起来，那么我们就愉快地得到了一个估计方程组(表达式)：

由此，我们可以通过这个方程组，写出整个x_p的分布，由于噪声是满足高斯分布的，所以整个x_p的分布也满足高斯分布，如下：

Sensor Model
接下来让我们建立landmark和pose之间的联系，可以想象的是，一个pose会对多个landmark进行观测，同时同一个landmark可能被不同的pose观测到，而且由于传感器的不同，观测方程可能千变万化，近年火起来的视觉slam，就是把激光传感器换成了相机，用像素代表观测值，从而想方设法得到landmark以及pose的估计，一个更加general的模型是这样的一个方程：

这个代表的是在j位置对i观测，z是观测值，h是观测函数，v是满足均值为0方差为R的噪声，你可以想象，每个pose都有大量观测，如此多的观测凑在一起，拼成了一个无比庞大的函数：

　　
同样的，由于噪声满足高斯分布，我们也可以写出这个概率密度函数：

　　Model

有了这些玩意儿，我们是时候建一波把他们统一在一起的模型了。

首先，我们有一些先验信息，比如说你大概猜了一下，机器人都有第一个pose在哪个地方，沿途看到了哪些路标，同时有一个不确信度，整个信息满足高斯分布，如下：

其中Pi表示信息矩阵，那么概率密度如下：

有了这些个概率，我们可以写一个联合概率密度：

根据贝叶斯公式，可以表示后验概率为：

注意到分母和x无关，因此，可以写出这样一个公式：

那么这个时候，我们定义一个函数g:

　　

那么我们用最大似然估计，可以得到一个能量方程(g2o上面那个):

于是，这个时候应该愉快地高斯牛顿了。具体推导我就不做了，最后可以得到一个类似这样的方程：

 

然而，这个不是重点，重要的是对hessian的认识，请看下图：

 

看起来是不是很酷？左边第一个是雅可比矩阵，有颜色部分是有数据的，其他都是0，左边第二块是整个hessian矩阵的长相，它正好等于传感器给你的(观测模型)+运动过程中建立的(运动模型)+先验信息。

为啥会长这个样子呢？其实很简单，注意观测模型，其实正好是稀疏BA的矩阵，它反映了pose和point之间的连接，第二个反映了pose和pose之间的连接(可以看到这个块块比第一个左上角的块块大，其实是把连接放进去了)，第三个反映了对地图的未知性(看右下角，认为point之间可能是有关联的)，以及对第一帧的预测，还有地图的预测，具体公式推导，请自行进行，并不难。

 

The Sliding Window Filter

我想，最后是时候上算法了，讲了这么多，我想大家对slam问题的数学模型也有一定的了解了，但是其实有一个比较大的问题，即大规模运动和观测总会跪，什么意思呢，虽然slam有很好的稀疏性，有强大的g2o作为后端优化的神器，但是数据总不能没有限制地增大增大再增大，所以，这个时候，如果我们有一个将计算复杂程度限制甚至固定的方法，这样是不是很酷呢？

Marginalization

这个算法的核心就是边缘化了，什么是边缘化呢，简单说，边缘化就是利用shur补减少优化参数，从而提升计算性能。shur补是啥呢？是时候上一波维基百科：

以上，均为维基百科对舒尔补的解释，实际上，使用shur补，就是计算了余留参数在准备remove的参数下的条件概率。所以说，每marg一次，就会代入一些交叉项信息，请看下图，这是论文上的原图

我不想去翻译着些英文，就大致说明一下是怎么回事

marg了最老的一帧，那么你需要在首先在这一帧上看到的landmark之间建立起联系，并且需要建立和被marg的那一帧上看到的landmark与那一帧联系的所有帧之间的联系(由于有马尔可夫性，所以一般就是下一帧和上一帧之间的联系)，最后可以直接marg掉点

下面那个网格是从动态贝叶斯网的观点看这个原理，我不想多做说明，至于marg的具体操作，其实就是做一次舒尔补罢了。这就是小编这次想给大家分享的所有内容，希望大家喜欢！！

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能否进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

【注】商业转载请联系刘富强（liufuqiang_robot@hotmail.com）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。