对SLAM系统中的协方差矩阵与信息矩阵，以及舒尔补和边缘化的理解

首先关于什么是协方差矩阵与信息矩阵。
在零均值的多元高斯分布中有如下概率形式：$$p(x)=\frac{1}{Z}exp(-\frac{1}{2}{x}^{\top }{\Sigma }^{-1}x)$$其中$\Sigma$为协方差矩阵，而协方差矩阵的逆$\Lambda =\Sigma -1$则为信息矩阵。比如在$X$为三维变量时，协方差矩阵${\Sigma }_{ij}=E(x_i{x}_j)$为对应元素求期望。

---

样例：$v_i$相互独立，且各自服从协方差为${\sigma }_i^2$的高斯分布。

根据协方差的计算公式${\Sigma }_{ij}=E(x_i{x}_j)$，可求解得到：
$$\begin{gathered} {\Sigma }_{11}=E(x_1{x}_1)=E(w_1{v}_2+v_1)(w_1{v}_2+v_1) \\ =w_1^{2}E(v_2^2)+2w_{1}E(v_1{v}_2)+E(v_1^2) \\ =w_1^2{\sigma }_2^2+{\sigma }_1^2 \end{gathered}$$同理可得到整个协方差矩阵：
$$\Sigma =\left[\begin{array}{ccc}{w}_1^2{\sigma }_2^2+{\sigma }_1^2& w_1{\sigma }_2^2& w_1{w}_3{\sigma }_2^2\\ w_1{\sigma }_2^2& {\sigma }_2^2& w_3{\sigma }_2^2\\ w_1{w}_3{\sigma }_2^2& w_3{\sigma }_2^2& w_3^2{\sigma }_2^2+{\sigma }_3^2\end{array}\right]$$其信息矩阵的求解等于它的逆，直接求解较为困难，这里通过联合高斯分布计算得到协方差的逆。$$p(x_1,x_2,x_3)=p(x_2)p(x_1|x_2)p(x_3|x_2)$$带入如下形式：$$p(x)=\frac{1}{Z}exp(-\frac{1}{2}{x}^{\top }{\Sigma }^{-1}x)$$$$\begin{gathered} p(x_1,x_2,x_3)=\frac{1}{Z_1}exp(-\frac{x_2^2}{2{\sigma }_2^2})\frac{1}{Z}exp(-\frac{(x_1-w_1{x}_2{)}^2}{2{\sigma }_1^2})\frac{1}{Z}exp(-\frac{(x_3-w_3{x}_2{)}^2}{{\sigma }_3^2}) \\ =\frac{1}{2}exp(-\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_1^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& \frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]) \\ =\frac{1}{Z}exp(-\frac{1}{2}{x}^{\top }{\Sigma }^{-1}x) \end{gathered}$$由此得到协方差矩阵的逆：${\Sigma }^{-1}$，即信息矩阵。
协方差逆矩阵中如果坐标为$(i,j)$的元素为 0，表示元素 $i$ 和 $j$在其他变量固定的情况下条件独立。协方差中非对角元素${\Sigma }_{ij}>0$表示俩变量正相关，而在信息矩阵${\Lambda }_{ij}<0$表示正相关。

---

如果我们移除变量，信息矩阵或协方差矩阵如何变化。实际操作过程中并不会有这种颜色标记。这时，需要引入marginalization (边缘化) 和 Schur’s complement (舒尔补) 来解决这个问题.

舒尔补的定义：
给定任意矩阵块M，如下：
$$M=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$$

• 如果，矩阵快D是可逆的，则$A-B{D}^{-1}C$称之为D关于M的舒尔补。
• 如果，矩阵快A是可逆的，则$D-C{A}^{-1}B$称之为A关于M的舒尔补。

舒尔补的由来，将M矩阵变成上三角或者下三角的过程中：
$$\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& {\Delta }_A\end{array}\right]$$$$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]=\left[\begin{array}{cc}A& 0\\ C& {\Delta }_A\end{array}\right]$$其中，${\Delta }_A=D-C{A}^{-1}B$，联合上式，可将M矩阵变成对角形矩阵：
$$\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]=\left[\begin{array}{cc}A& 0\\ 0& {\Delta }_A\end{array}\right]$$矩阵M可写成：$$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}I& 0\\ C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& 0\\ 0& {\Delta }_A\end{array}\right]\left[\begin{array}{cc}I& A^{-1}B\\ 0& I\end{array}\right]$$因为$$\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]\left[\begin{array}{cc}I& A^{-1}B\\ 0& I\end{array}\right]=I$$由此可得到矩阵M的逆：$${\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_A^{-1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]$$

在百度百科中有一段背景很容易理解：
$$\begin{gathered} Ax+By=a \\ Cx+Dy=b \end{gathered}$$对第二个式子乘上$B{D}^{-1}$便可以得到$$B{D}^{-1}CX+By=B{D}^{-1}b$$然后与第一个式子相减：$$(A-B{D}^{-1}C)x=a-B{D}^{-1}b$$

---

舒尔补应用于多元高斯分布（例）：
假设多元变量$x$服从高斯分布，且有两部分组成：$x={\left[\begin{array}{cc}a& b\end{array}\right]}^{\top }$，变量之间构成的协方差矩阵为：$$K=\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]$$其中 $A=cov(a,a),D=cov(b,b),C=cov(a,b)$，由此变量$x$的概率分布为：$$\begin{gathered} P(a,b)=P(a)P(b|a)\propto exp\left(-\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^{\top }\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]\left[\begin{array}{c}a\\ b\end{array}\right]\right) \\ \propto exp\left(-\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^{\top }\left[\begin{array}{cc}I& -A^{-1}{C}^{\top }\\ 0& I\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_A^{-1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{c}a\\ b\end{array}\right]\right) \\ \propto \underset{p(a)}{exp\left(-\frac{1}{2}{a}^{\top }{A}^{-1}a\right)}\underset{p(b|a)}{exp\left(-\frac{1}{2}(b-A^{-1}{C}^{\top }b{)}^{\top }{\Delta }_A^{-1}(b-A^{-1}{C}^{\top }b)\right)} \end{gathered}$$使用舒尔补之后，便可以从多元高斯分布$P(a,b)$中分解得到边际概率$p(a)$和条件概率$p(b|a)$。
边际概率$P(a)\sim \mathcal{N}(0,A)$的协方差就是从联合分布中取对应的矩阵块就行了。$P(b|a)\sim \mathcal{N}(A^{-1}{C}^{\top }b,{\Delta }_A)$，协方差变为 $a$ 对应的舒尔补，均值也变了。
在基于优化的 SLAM 问题中，我们往往直接操作的是信息矩阵，而不是协方差矩阵。

因此假设我们已知信息矩阵：
$$\Lambda =K^{-1}={\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}$$根据舒尔补公式中的逆运算可得到：$${\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}{A}^{-1}+A^{-1}{C}^{\top }{\Delta }_A^{-1}C{A}^{-1}& -A^{-1}{C}^{\top }{\Delta }_A^{-1}\\ -{\Delta }_A^{-1}C{A}^{-1}& {\Delta }_A^{-1}\end{array}\right]\stackrel{△}{=}\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$在上面求解协方差的时候已经知道：
条件概率$P(b|a)\sim \mathcal{N}(A^{-1}{C}^{\top }b,{\Delta }_A)$，其协方差为${\Delta }_A$，则其信息矩阵$\Lambda =K^{-1}={\Delta }_A^{-1}={\Lambda }_{bb}$。
边际概率$P(a)\sim \mathcal{N}(0,A)$，其协方差为$A$，可通过消元得到其信息矩阵$\Lambda =A^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba}$。

综上，便可以使用舒尔补去除信息矩阵中的变量，例如去除例子中的$x_3$
$${\Sigma }^{-1}=\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_1^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& \frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]$$从联合分布 $P(x1,x2,x3)$ 中 marg 掉变量 $x_3$ ，即 $P(x1,x2)$ 对应的信息矩阵可以用上面红色公式得到:
$$\begin{gathered} K_2^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba} \\ ={\Lambda }_{aa}-\left[\begin{array}{c}0\\ -\frac{w_3}{{\sigma }_3^2}\end{array}\right]{\sigma }_3^2\left[\begin{array}{cc}0& -\frac{w_3}{{\sigma }_3^2}\end{array}\right] \\ ={\Lambda }_{aa}\left[\begin{array}{cc}0& 0\\ 0& \frac{w_3}{{\sigma }_3^2}\end{array}\right]=\left[\begin{array}{cc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}\end{array}\right] \end{gathered}$$可以发现，和$x_3$有关的蓝色部分已经被消除。