【深蓝学院】手写VIO第4章--基于滑动窗口算法的 VIO 系统：可观性和 一致性--笔记

0. 内容

由于我们是要做一个实时的定位系统，而不是SfM那种离线的三维重建的工作，所以需要在滑动窗口中不断地添加新数据以及删除旧数据，而前者可以把所有数据放在一个BA中求解。

1. 从高斯分布到信息矩阵

SLAM问题的建模，MAP->MLE->LSP，我们只关心最大后验分布的那个点


这个${\Sigma }^{-1}$表示的是每次观测所占的权重，可调，也有方法可以动态地估计出。${\Sigma }^{-1}$，可以看论文。
之前的正规矩阵中没有写${\Sigma }^{-1}$是因为当时设为单位阵，实际上中间是有个权重矩阵${\Sigma }^{-1}$的

如图中的绿框

协方差对角线元素为对应下标的元素的协方差，由于是零均值，所以$\mu =0$。
右边表示x和y之间的相关性情况，如果是严格线性的，那点的分布就是一条直线，但是明显不是严格线性的，即协方差矩阵非对角元素不为0。
计算方法参考下图：


所以零均值的分布，$Cov(x_i{x}_j)=E(x_i{x}_j)-E(x_i)E(x_j)=E(x_i{x}_j)$（下面式(11)用到）

本节课主题：如何往窗口中加新的变量以及把旧的变量删掉。

下面的例子有助于我们理解SLAM问题中的信息矩阵$H$的组成。

1.1 例子1

对于(11)，此处是写为了标量形式，回顾第3章3.3节的结论：

(11)的结果写成矩阵形式就是${\omega }_1{\sigma }_2^2{\omega }_1^T+{\sigma }_1^2$

求协方差矩阵的逆，可以直接对(13)求逆，但是麻烦，还可以通过计算联合高斯分布来得到协方差矩阵的逆：

有$条件=\frac{联合}{边缘}，上述有向无环图箭头的起点表示条件，终点表示结果。故$（14）由来：$$p(x_1,x_2,x_3)=p(x_1,x_3|x_2)\ast p(x_2)=p(x_1|x_2)\ast p(x_3|x_2)\ast p(x_2)$$
不清楚为什么(14)可以这样写，本来从协方差矩阵中可以看出$x_1,x_3$不是相互独立的，但是写出来的感觉好像是独立的。（这个问题在下面有解释）
然后就是通分，结合高斯分布的形式：

可以看出协方差矩阵的逆就是中间的那个矩阵。

回答上面的问题，从${\Sigma }^{-1}$可以看出，${\Sigma }_{13}={\Sigma }_{31}=0$，代表在变量$x_2$的条件下，$x_1,x_3$是相互独立的。即当房间2的温度($x_2$)确定时，房间1，3的温度($x_1,x_3)$互不影响，相互独立。所以其实上面推导(14)时已经是使用了“在$x_2$的条件下，$x_1,x_3$相互独立”这个结论。

协方差矩阵和信息矩阵的符号所代表的含义不同。

1.2 例子2

同样地，$联合=条件\ast 边缘$，$x_1,x_3$独立，(18)推导:
$$p(x_1,x_2,x_3)=p(x_2|x_1,x_3)\ast p(x_1,x_3)=p(x_2|x_1,x_3)\ast p(x_1)\ast p(x_3)$$

${\Sigma }_{13}=0$但是$({\Sigma }^{-1}{)}_{13}>0$，表示当$x_2$确定时，$x_1,x_3$呈负相关，对应实际问题就是：房间2中的温度一定时，房间1温度高点的话，房间3温度就要低一点才能保证房间2中温度一定。

上面的节点和边是一个有向无环图，是贝叶斯图论中的东西（之前有博士师姐研究这个的，每次听她汇报都一脸问号，压根听不懂）。

1.3 例子1的拓展

如果例子1中没有$x_3$，那么在$\Sigma ,{\Sigma }^{-1}$中直接把有关$x_3$的变量置为0即可：

但是实际中我们不知道$\Sigma ,{\Sigma }^{-1}$由哪几项构成，也就无法去掉为0的项，这时需要引入其他的方法来帮助我们找出这些项：marginalization(边缘化)和Schur’s complement(舒尔补)。

1.4协方差矩阵和信息矩阵小结

1. 协方差矩阵$\Sigma$非对角元素>0则代表对应元素正相关，信息矩阵${\Sigma }^{-1}$中非对角线元素<0代表两元素正相关，>0则是负相关(毕竟是求逆)。
2. 协方差矩阵表示两个变量之间的关系，而信息矩阵则表示两个变量在某种特定条件下的关系。

2. 舒尔补应用

将矩阵M变为上三角或者下三角矩阵，得到的$\Delta A$叫做舒尔补

舒尔消元便于求解方程$H\Delta x=b$，$H=J^{T}J$是稀疏的，把路标点信息边缘化到相机位姿中，求解出相机位姿后再求解位姿。

这部分内容可以复习14讲后端1中关于稀疏化和边缘化的部分，看完例子之后再来理解这里的内容会比较容易。
如果是VSlam，下面的$x$可以看做是由相机位姿(a，se(3))和路标点(b,$R^3$)组成，K就是$J^{T}J$（可能VIO会有些差别）

2.1 边缘概率P(a), 条件概率P(a|b)的协方差矩阵

(31)严格来说应该减去$[a,b{]}^T$的期望，但是这里不怎么关注期望

将式(29)带入得，可以整理为只与a,b有关的部分，就分解为边缘概率和条件概率：

(33)是全概公式

正规方程也可以写为以下形式(vins-mono里面有这个，暂时还不理解为什么要这样写)

但是实际上信息矩阵的左上角的块不一定好求，因为$A^{-1}$不一定好求，左上角的块不仅仅跟$A^{-1}$有关，见(36)

边缘概率实际中是相机位姿的概率，条件概率是在相机位姿的条件下看到的路标点的概率。

2.2 P(a), P(a|b)的信息矩阵

已知信息矩阵${\Sigma }^{-1}$，可以看出${\Lambda }_{aa}$不只是和$A^{-1}$有关，但是边缘分布的协方差就是$\mathbf{A}$，$P(a)$的信息矩阵就是${\mathbf{A}}^{-1}$，给了信息矩阵${\Sigma }^{-1}$，如何求出${\mathbf{A}}^{-1}$呢？（为什么要求呢？）

高斯消元可得${\mathbf{A}}^{-1}$

对应到1.3的例子，去掉$x_3$时，求$P(x_1,x_2)$的信息矩阵时，直接套用(38)如下式(39)，$K_2^{-1}$即为$P(x_1,x_2)$的信息矩阵

2.1 总结

信息矩阵本身就能表示出条件概率，如1.1的式(16)中的，信息矩阵${\Sigma }_{ij}=0$代表i, j关于其他变量条件独立，描述的本身就是条件概率。

总结：

1. 在SLAM中，舒尔补主要用在滑动窗口算法中，将旧的信息丢掉，我们是希望通过舒尔补的形式将一部分信息给marg掉，且通常是操作边缘概率的信息矩阵。
2. 边界概率协方差矩阵好求，条件概率信息矩阵好求。
3. 边界概率的协方差就是从联合分布中取对应的矩阵块就行了；条件概率$P(b|a)$协方差变为 a 对应的舒尔补，均值也变了。
4. 边际概率 P (a)信息矩阵难求，条件概率信息矩阵直接取对应的块。

3. 滑动窗口算法