【VIO】第3讲 基于滑动窗口算法的VIO

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

（1）SLAM 问题概率建模

具体参见：https://blog.csdn.net/ASUNAchan/article/details/124654207?spm=1001.2014.3001.5501

（2）高斯分布和协方差矩阵

​ 1）多元高斯分布

​ 零均值高斯分布：
$$p(x)=\frac{1}{Z}{e}^{-\frac{1}{2}{x}^T{\Sigma }^{-1}x}$$
​ 其中：$\Sigma$ 为协方差矩阵，协方差矩阵的逆记作 $\Lambda ={\Sigma }^{-1}$ ，${\Sigma }_{ij}=E(x_i{x}_j)$ 为对应元素求期望。

​ 2）例1
$$\begin{gathered} x_2=v_2 \\ {x}_1=w_1{x}_2+v_1 \\ {x}_3=w_3{x}_2+v_3 \end{gathered}$$
​ 推导过程略

​ 结论：

​ 1 协方差逆矩阵中如果坐标为 $(i,j)$ 的元素为 0，表示元素 $i$ 和 $j$ 关于其他变量条件独立

​ 2 协方差中非对角元素 ${\Sigma }_{ij}>0$ 表示两变量是正相关

​ 3 信息矩阵中非对角元素为负数，甚至为 0。${\Lambda }_{12}<0$ 表示在变量$x_3$ 发生的条件下，元素 $x_1$ 和 $x_2$ 正相关。

​ 3）例2
$$x_2=w_1{x}_1+w_3{x}_3+v_2$$

2.舒尔补应用：边际概率, 条件概率

（1）舒尔补的概念

​ 给定任意的矩阵块 M：
M = [ A B C D ] M = \left[

\right] M=[AC​BD​]
如果，矩阵块 D 是可逆的，则 $A-B{D}^{-1}C$ 称之为 D 关于 M 的舒尔补。
[ I − B D − 1 0 I ] [ A B C D ] [ I 0 − D − 1 C I ] = [ A − B D − 1 C 0 0 D ] \left[

I−BD−10I" role="presentation" style="position: relative;">I0−BD−1I$$\begin{array}{cc}I& -B{D}^{-1}\\ 0& I\end{array}$$

\right] \left[

ABCD" role="presentation" style="position: relative;">ACBD$$\begin{array}{cc}A& B\\ C& D\end{array}$$

\right] \left[

I0−D−1CI" role="presentation" style="position: relative;">I−D−1C0I$$\begin{array}{cc}I& 0\\ -D^{-1}C& I\end{array}$$

\right] =\left[

A−BD−1C00D" role="presentation" style="position: relative;">A−BD−1C00D$$\begin{array}{cc}A-B{D}^{-1}C& 0\\ 0& D\end{array}$$

\right] [I0​−BD−1I​][AC​BD​][I−D−1C​0I​]=[A−BD−1C0​0D​]
如果，矩阵块 A 是可逆的，则 $D-C{A}^{-1}B$ 称之为 A 关于 M 的舒尔补。
[ I 0 − C A − 1 I ] [ A B C D ] [ I − A − 1 B 0 I ] = [ A 0 0 D − C A − 1 B ] \left[

I0−CA−1I" role="presentation" style="position: relative;">I−CA−10I$$\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}$$

\right] \left[

ABCD" role="presentation" style="position: relative;">ACBD$$\begin{array}{cc}A& B\\ C& D\end{array}$$

\right] \left[

I−A−1B0I" role="presentation" style="position: relative;">I0−A−1BI$$\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}$$

\right] =\left[

A00D−CA−1B" role="presentation" style="position: relative;">A00D−CA−1B$$\begin{array}{cc}A& 0\\ 0& D-C{A}^{-1}B\end{array}$$

\right] [I−CA−1​0I​][AC​BD​][I0​−A−1BI​]=[A0​0D−CA−1B​]
然后，从对角形恢复M矩阵
[ I B D − 1 0 I ] [ A − B D − 1 C 0 0 D ] [ I 0 D − 1 C I ] = [ A B C D ] \left[

IBD−10I" role="presentation" style="position: relative;">I0BD−1I$$\begin{array}{cc}I& B{D}^{-1}\\ 0& I\end{array}$$

\right] \left[

A−BD−1C00D" role="presentation">A−BD−1C00D$$\begin{array}{cc}A-B{D}^{-1}C& 0\\ 0& D\end{array}$$

\right] \left[

I0D−1CI" role="presentation">ID−1C0I$$\begin{array}{cc}I& 0\\ D^{-1}C& I\end{array}$$

\right] =\left[

ABCD" role="presentation">ACBD$$\begin{array}{cc}A& B\\ C& D\end{array}$$

\right] [I0​BD−1I​][A−BD−1C0​0D​][ID−1C​0I​]=[AC​BD​]

[ I 0 C A − 1 I ] [ A 0 0 D − C A − 1 B ] [ I A − 1 B 0 I ] = [ A B C D ] \left[

\right] \left[ \right] \left[ \right] =\left[ \right] [ICA−1​0I​][A0​0D−CA−1B​][I0​A−1BI​]=[AC​BD​]

所以M矩阵的逆矩阵 $M^{-1}$ 为：
[ A B C D ] − 1 = [ I 0 − D − 1 C I ] [ ( A − B D − 1 C ) − 1 0 0 D − 1 ] [ I − B D − 1 0 I ] \left[

\right]^{-1} =\left[ \right] \left[ \right] \left[ \right] [AC​BD​]−1=[I−D−1C​0I​][(A−BD−1C)−10​0D−1​][I0​−BD−1I​]

[ A B C D ] − 1 = [ I − A − 1 B 0 I ] [ A − 1 0 0 ( D − C A − 1 B ) − 1 ] [ I 0 − C A − 1 I ] \left[

\right]^{-1} = \left[ \right] \left[ \right] \left[ \right] [AC​BD​]−1=[I0​−A−1BI​][A−10​0(D−CA−1B)−1​][I−CA−1​0I​]

（2）舒尔补应用于多元高斯分布

假设多元变量x服从高斯分布，且由两部分组成：
$$x=[a,b{]}^T$$
变量之间的协方差矩阵：
K = [ A C T C D ] K = \left[

\right] K=[AC​CTD​]
其中：$A=cov(a,a)$，$D=cov(b,b)$， $C=cov(a,b)$

所以：
p ( a , b ) = p ( a ) p ( b ∣ a ) ∝ e x p ( − 1 2 [ a b ] [ A C T C D ] − 1 [ a b ] ) p(a,b)=p(a)p(b|a) \propto exp( -\frac{1}{2} \left[

\right] \left[ \right]^{-1} \left[ \right] ) p(a,b)=p(a)p(b∣a)∝exp(−21​[a​b​][AC​CTD​]−1[ab​])
使用舒尔补的公式对高斯分布分解：
$$\begin{gathered} p(a,b) \\ \propto exp(\frac{1}{2}[(a^T{A}^{-1}a)+(b-C{A}^{-1}a{)}^T{\Delta }_A^{-1}(b-C{A}^{-1}a)]) \\ \propto exp(\frac{1}{2}{a}^T{A}^{-1}a)exp[\frac{1}{2}(b-C{A}^{-1}a{)}^T{\Delta }_A^{-1}(b-C{A}^{-1}a)] \end{gathered}$$
其中：
$$\begin{gathered} p(b|a)\sim N(C{A}^{-1}a,D-C{A}^{-1}B) \\ p(a)\sim N(0,A) \end{gathered}$$
$p(a),p(b|a)$ 的信息矩阵：

由条件概率 $p(b|a)$ 的协方差为 ${\Delta }_A$ 及上述公式，得信息矩阵 ${\Delta }_A^{-1}={\Lambda }_{bb}$

由边际概率 $p(a)$ 的协方差为 $A$ 及上述公式，得信息矩阵 $A^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba}$

（3）总结

边际概率对于协方差矩阵的操作是很容易的，但不好操作信息矩阵。条件概率恰好相反，对于信息矩阵容易操作，不好操作协方差矩阵。

3.滑动窗口算法

（1）例子

​ 有如下最小二乘系统，对应的图模型如有图所示

$$\xi =argmi{n}_{\xi }\frac{1}{2}\sum _i||r_i|{|}_{{\Sigma }_i}^2$$
其中：$\xi =[{\xi }_1,{\xi }_2,\cdots ,{\xi }_6]$^T，$r=[r_{12},r_{13},r_{14},r_{15},r_{56}{]}^T$

针对上述最小二乘问题，对应高斯牛顿求解：
$$J^T{\Sigma }^{-1}J\Delta \xi =-J^T{\Sigma }^{-1}r$$

所以可以将公式写成：
$$\sum _{i=1}^5{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta \xi =-\sum _{i=1}^5{J}_i^T{\Sigma }_i^{-1}r$$

（2）信息矩阵的稀疏性

​ 由于每个残差只和某几个状态量有关，因此，雅克比矩阵求导时，无关项的雅克比为 0。比如：
$$J_2=\frac{\partial r_{13}}{\partial \xi }=[\frac{\partial r_{13}}{\partial {\xi }_1},0,\frac{\partial r_{13}}{\partial {\xi }_3},0,0,0]$$

$${\Lambda }_2=J_2^T{\Sigma }_2^{-1}{J}_2$$

​ 同理，可以计算 $\Lambda_1 , \Lambda_3 , \Lambda_4 ,\Lambda_5 $，并且也是稀疏的。

​ 可以结合十四讲第9章以及第8章直接法部分来更好理解。

（3）基于边际概率的滑动窗口算法

​ 1 为什么 SLAM 需要滑动窗口算法?

​ 随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。

​ 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。

​ 2 滑动窗口算法大致流程

​ 增加新的变量进入最小二乘系统优化；如果变量数目达到了一定的维度，则移除老的变量。SLAM 系统不断循环前面两步。

​ 3 利用边际概率移除老的变量

​ 直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。

​ 如果是直接丢弃，信息矩阵如何变化？用边际概率来操作又会带来什么问题？

（4）例子

​ 如下图优化系统中，随着 $x_{t+1}$ 的进入，变量 $x_t$ 被移除。

​ marginalization 会使得信息矩阵变稠密！原先条件独立的变量，可能变得相关。

4.滑动窗口中的 FEJ 算法

（1）回顾例子

​ 1 如图所示，在 $t\in [0,k]s$ 时刻, 系统中状态 量为 ${\xi }_i,i\in [1,6]$。第 $k^{\prime }$ 时刻，加入新的观 测和状态量 ${\xi }_7$

​ 2 在第 k 时刻，最小二乘优化完以后，marg 掉变量 ${\xi }_1$。被 marg 的状态量记为 $x_m$, 剩余 的变量 ${\xi }_i,i\in [2,5]$ 记为 $x_r$。

​ 3 marg 发生以后，$x_m$ 所有的变量以及对应的 测量将被丢弃。同时，这部分信息通过 marg 操作传递给了保留变量 $x_r$ ，marg 变量的信息跟 ${\xi }_6$ 不相关。

​ 4 第 $k^{\prime }$ 时刻，加入新的状态量 ${\xi }_7$(记作 $x_n$) 以及对应的观测，开始新一轮最小二乘优化。

（2）marg 前后

​ 已知的是：
$$\sum _{i=1}^5{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta \xi =-\sum _{i=1}^5{J}_i^T{\Sigma }_i^{-1}r$$
​ marg 前，变量 $x_m$ 以及对应测量 $S_m$ 构建的最小二乘信息矩阵为：

​ marg 后，变量 $x_m$ 的测量信息传递给了变量 $x_r$
$$b_p(k)=b_{mr}(k)-{\Lambda }_{rm}(k){\Lambda }_{mm}(k{)}^{-1}{b}_{mm}(k)$$

$${\Lambda }_p(k)={\Lambda }_{rr}(k)-{\Lambda }_{rm}(k){\Lambda }_{mm}^{-1}(k){\Lambda }_{mr}(k)$$

​ 下标 p 表示 prior. 即这些信息将构建一个关于 $x_r$ 的先验信息。

​ 我们可以从 $b_p(k)$, ${\Lambda }_p(k)$ 中反解出一个残差 $r_p(k)$ 和对应的雅克比矩 阵 $J_p(k)$. 需要注意的是，随着变量 $x_r(k)$ 的后续不断优化变化，残差 $r_p(k)$ 或者 $b_p(k)$ 也将跟着变化，但雅克比 $J_p(k)$ 则固定不变了。？？？

（3）新测量信息和旧测量信息构建新的系统

​ 在 $k^{\prime }$​ 时刻，新残差 $r_{27}$​ 和先验信息 $b_p(k)$​, ${\Lambda }_p(k)$​ 以及残差 $r_{56}$​ 构建新的最小二乘问题：
$$b(k^{\prime })={\Pi }^T{b}_p(k)-\sum _{(i,j)\in S_a(k^{\prime })}{J}_{ij}(k^{\prime }{)}^T{\Sigma }_{ij}^{-1}{r}_{ij}(k^{\prime })$$

$$\Lambda (k^{\prime })={\Pi }^T{\Lambda }_p(k)\Pi -\sum _{(i,j)\in S_a(k^{\prime })}{J}_{ij}(k^{\prime }{)}^T{\Sigma }_{ij}^{-1}{J}_{ij}(k^{\prime })$$

​ 其中：$\Pi =[I_{dim{x}_r}0]$ 将矩阵维度进行扩容，$S_a$ 用来表示除被 marg 掉的测量以外的其他测量，如 $r_{56},r_{27}$。

​ 出现的问题：

​ 由于被 marg 的变量以及对应的测量已被丢弃，先验信息 ${\Lambda }_p(k)$ 中关于 $x_r$ 的雅克比在后续求解中没法更新。

​ 但 $x_r$ 中的部分变量还跟其他残差有联系, 如 ${\xi }_2,{\xi }_5$。这些残差如 $r_{27}$ 对 ${\xi }_2$ 的雅克比会随着 ${\xi }_2$ 的迭代更新而不断在最新的线性化点处计算。

（4）滑动窗口算法导致的问题

​ 滑动窗口算法优化的时候，信息矩阵如上述公式变成了两部分，且这两部分计算雅克比时的线性化点（个人理解：时间变了，变量发生变化）不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误信息。

​ 滑动窗口算法中，对于同一个变量，不同残差对其计算雅克比矩阵时线性化点可能不一致，导致信息矩阵可以分成两部分，相当于在信息矩阵中多加了一些信息，使得其零空间出现了变化。

​ 解决办法：First Estimated Jacobian

​ FEJ 算法：不同残差对同一个状态求雅克比时，线性化点必须一致。 这样就能避免零空间退化而使得不可观变量变得可观。