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

news2025/11/1 6:53:14

0. 内容

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

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

在这里插入图片描述
SLAM问题的建模，MAP->MLE->LSP，我们只关心最大后验分布的那个点

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

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

所以零均值的分布， $Cov(x_ix_j)=E(x_ix_j)-E(x_i)E(x_j)=E(x_ix_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)*p(x_2)=p(x_1|x_2)*p(x_3|x_2)*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

在这里插入图片描述

同样地， $联合 = 条件 * 边缘$ ， $x_1, x_3$ 独立，(18)推导:
$p(x_1,x_2,x_3)=p(x_2|x_1,x_3)*p(x_1,x_3)=p(x_2|x_1,x_3)*p(x_1)*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协方差矩阵和信息矩阵小结

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

2. 舒尔补应用

在这里插入图片描述

将矩阵M变为上三角或者下三角矩阵，得到的 $\Delta A$ 叫做舒尔补
在这里插入图片描述

在这里插入图片描述

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

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

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

在这里插入图片描述
(31)严格来说应该减去 $a, b]^T$ 的期望，但是这里不怎么关注期望

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

(33)是全概公式

在这里插入图片描述

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

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

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

在这里插入图片描述

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