舒尔补在Slam中的应用

本文参考深蓝学院的VINS课程，加入自己的理解，有疑问欢迎交流。

另发现了白巧克力也有相关的博客，附大神链接：https://blog.csdn.net/heyijia0327/article/details/52822104

在看文章之前可以先看一下舒尔补的定义  以及  条件概率和联合概率相关知识

还是以多元变量x服从高斯分布，且由两部分组成 x=[a b], 那么变量之间构成的协方差矩阵为

其中A = COV(a,a), D = cov(b,b), C= cov(a,b).  由此变量x的概率分布为

中间的协方差矩阵就可以用舒尔补进行分解，具体的分解过程是：

这样就可以从多元高斯分布中直接分解出边际概率和条件概率；

以example1为例：

设x2为室外的温度， x1， x3 分别为房间1和房间3的室内温度，三者之间的关系如下：

   

 

其中v1v2,v3 为服从高斯分布的均值为0， 方差为σ12(σ1的平方)， σ22(σ2的平方)， σ32 (σ3的平方)， 根据多元高斯分布规律，三者的联合概率分布公式为

其中协方差中每一项为对应两项变量的期望；

求得

为协方差矩阵；

 那么信息矩阵的求解方式可以通过求联合概率分布的方式获得：

从上述推导公式中可以获得信息矩阵为：

 

其中元素为0 的位置表示这两个变量关于其他变量条件独立；也就是x1,x3之间的关系是关于x2 独立，也就是在x2确定时，x1,x3之间独立；

确定了协方差矩阵和信息矩阵之后，接下来在例子中，如果要将变量x3去除，那么信息矩阵与协方差矩阵要怎么变化呢？针对这个例子我们有两种方法求解，

第一种是按照上面求解步骤再求解一次，就可以得到新的协方差和信息矩阵如下：

    

但是这种方法在变量比较少时可以直接求解，其实就是从上面的公式中去掉对应颜色的部分（x3对应为蓝色的部分），但是在slam中观测量很多的情况下，我们需要的是在已知矩阵(16)和(13) 的情况下直接推到出21， 23，也就是slam中边缘化时，如何从N个观测决定的状态中推到出去除某一个关键帧对应的状态，也就是N-1个观测所对应的状态；

这里就用到前面舒尔补的公式，列出如下：

对应到该例子中，

原协方差矩阵为：     信息矩阵为：　

则根据中式(32)的计算，从联合概率的协方差矩阵推导边际概率的协防差矩阵即为原协方差矩阵中的A部分；条件概率的协方差矩阵即为A的舒尔补 ， DeltaA；

那么其对应的信息矩阵也就是A.inv() 和 DeltaA.inv() ,

但是我们需要的不是从协方差中计算信息矩阵，而是要从原来的信息矩阵中推算出新的信息矩阵，所以接下来就需要知道A.inv 和 deltaA.inv与原信息矩阵各个分块

之间的关系，； 可以从公式36中进行推算求解，结果为

这样就分别求得了边际概率的信息矩阵 和 条件概率的信息矩阵 与联合概率的信息矩阵之间的关系；

所以在回到例子1， 求新的边际概率的协方差矩阵和信息矩阵 就可以用联合概率协方差矩阵和信息矩阵求出：