SLAM基础——舒尔补介绍

1📖 舒尔补介绍

1-1🔖 舒尔补定义

给定任意的矩阵块 $\mathbf{M}$ ， 如下所示:
$$\mathbf{M}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]$$

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

1-2🔖 舒尔补的定理推导

将 $\mathrm{M}$ 矩阵分别变成上三角或者下三角形：
$$\begin{array}{l}\left[\begin{array}{cc}\mathbf{I}& 0\\ {-\mathbf{C}\mathbf{A}}^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]=\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ 0& {\Delta }_{\mathbf{A}}\end{array}\right]\\ \left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]=\left[\begin{array}{cc}\mathbf{A}& 0\\ \mathbf{C}& {\Delta }_{\mathbf{A}}\end{array}\right]\end{array}$$
其中： ${\Delta }_{\mathrm{A}}=\mathrm{D}-{\mathbf{C}\mathbf{A}}^{-1}\mathbf{B}$ 。联合起来， 将 $\mathbf{M}$ 变形成对角形:
$$\left[\begin{array}{cc}\mathbf{I}& 0\\ -{\mathbf{C}\mathbf{A}}^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]=\left[\begin{array}{cc}\mathbf{A}& 0\\ 0& {\Delta }_{\mathbf{A}}\end{array}\right]$$
反过来，我们又能从对角形恢复成矩阵 $\mathbf{M}$ :
$$\left[\begin{array}{cc}\mathbf{I}& 0\\ {\mathbf{C}\mathbf{A}}^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{A}& 0\\ 0& {\Delta }_{\mathbf{A}}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& {\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]=\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]$$

1-3 🔖 用途：快速求矩阵的逆

矩阵$\mathrm{M}$ 可写为：
$$\mathrm{M}=\left[\begin{array}{ll}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& 0\\ {\mathbf{C}\mathbf{A}}^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{A}& 0\\ 0& {\Delta }_{\mathbf{A}}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& {\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]$$
所以
$${\mathrm{M}}^{-1}={\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]}^{-1}=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}{\mathbf{A}}^{-1}& 0\\ 0& {\Delta }_{\mathbf{A}}^{-1}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& 0\\ -{\mathbf{C}\mathbf{A}}^{-1}& \mathbf{I}\end{array}\right]$$

依据提示：
$$\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& {\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]=\mathbf{I}$$

最终结果：
$${\mathrm{M}}^{-1}=\left[\begin{array}{cc}{A}^{-1}+A^{-1}B{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& -A^{-1}B{\Delta }_{\mathrm{A}}^{-1}\\ -{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& {\Delta }_{\mathrm{A}}^{-1}\end{array}\right]$$

1-4🔖用途：舒尔补在信息矩阵求解中的使用

假设我们已知信息矩阵：

另外，根据舒尔补公式可知，协方差矩阵各块和信息矩阵之间的关系有：

协方差矩阵：
$$\mathbf{\Sigma }=\left[\begin{array}{ll}A& C^{\top }\\ C& D\end{array}\right]$$
对应的信息矩阵

$${\mathbf{\Sigma }}^{-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 }_{\mathrm{A}}^{-1}C{A}^{-1}& -A^{-1}{C}^{\top }{\Delta }_{\mathrm{A}}^{-1}\\ -{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& {\Delta }_{\mathrm{A}}^{-1}\end{array}\right]\triangleq \left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$

注意：

中间那一步是利用舒尔补求逆的过程，这里直接使用了上一小结的结论带入，,具体过程参考上一小结。

其中：
$${\Delta }_{\mathrm{A}}=\mathrm{D}-{\mathbf{C}\mathbf{A}}^{-1}{\mathbf{C}}^{\top }$$

根据对应关系，不难得出：
$$\begin{gathered} {\Delta }_A^{-1}={\Lambda }_{bb} \\ {A}^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba} \end{gathered}$$
或者：
$$D^{-1}={\Lambda }_{bb}-{\Lambda }_{ba}{\Lambda }_{aa}^{-1}{\Lambda }_{ab}$$
这里的$A^{-1}$或者$D^{-1}$就是在下一次优化会使用的先验信息矩阵（又名：边际概率的信息矩阵）。

其他

关于边际概率和条件概率的使用，有兴趣的可以参考下一小节（先给出下一小节的结论）
$$P(\mathbf{a},\mathbf{b})=\mathcal{N}\left(\left[\begin{array}{l}{\mathbf{\mu }}_a\\ {\mathbf{\mu }}_b\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{aa}& {\mathbf{\Sigma }}_{ab}\\ {\mathbf{\Sigma }}_{ba}& {\mathbf{\Sigma }}_{bb}\end{array}\right]\right)={\mathcal{N}}^{-1}\left(\left[\begin{array}{l}{\eta }_a\\ {\eta }_b\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Lambda }}_{aa}& {\mathbf{\Lambda }}_{aa}\\ {\mathbf{\Lambda }}_{ba}& {\mathbf{\Lambda }}_{bb}\end{array}\right]\right)$$

以及
$$\begin{array}{ccc}& \text{ 边际概率 }& \text{ 条件概率 }\\ & p(\mathbf{a})=\int p(\mathbf{a},\mathbf{b})d\mathbf{b}& p(\mathbf{a}\mid \mathbf{b})=p(\mathbf{a},\mathbf{b})/p(\mathbf{b})\\ \text{ 协方差矩阵 }& \mathbf{\mu }={\mathbf{\mu }}_a& {\mathbf{\mu }}^{\prime }={\mathbf{\mu }}_a+{\Sigma }_{ab}{\Sigma }_{bb}^{-1}\left(\mathbf{b}-{\mathbf{\mu }}_b\right)\\ & \Sigma ={\Sigma }_{aa}& {\Sigma }^{\prime }={\Sigma }_{aa}-{\Sigma }_{ab}{\Sigma }_{bb}^{-1}{\Sigma }_{ba}\\ \text{ 信息矩阵 }& \begin{array}{c}\mathbf{\eta }={\mathbf{\eta }}_a-{\Lambda }_{a\beta }{\Lambda }_{bb}^{-1}{\mathbf{\eta }}_b\\ \Lambda ={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}{}^{-1}{\Lambda }_{ba}\end{array}& \begin{array}{c}{\mathbf{\eta }}^{\prime }={\mathbf{\eta }}_a-{\Lambda }_{ab}\mathbf{b}\\ {\Lambda }^{\prime }={\Lambda }_{aa}\end{array}\end{array}$$

1-5🔖用途： 舒尔补应用于多元高斯分布

通过舒尔补分解多元高斯分布

假设多元变量 $\mathrm{x}$ 服从高斯分布，且由两部分组成： $\mathbf{x}=\left[\begin{array}{c}a\\ b\end{array}\right]$, 变量之 间构成的协方差矩阵为：
$$\mathbf{K}=\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]$$
其中 $A=\mathrm{cov}(a,a),D=\mathrm{cov}(b,b),C=\mathrm{cov}(a,b)$. 由此变量 $\mathrm{x}$ 的概率分布为：
$$P(a,b)=P(a)P(b\mid a)\propto \exp \left(-\frac{1}{2}{\left[\begin{array}{l}a\\ b\end{array}\right]}^{\top }{\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}\left[\begin{array}{l}a\\ b\end{array}\right]\right)$$
利用舒尔补一节公式, 对高斯分布进行分解，得
$$\begin{array}{l}P(a,b)\\ \propto \exp \left(-\frac{1}{2}{\left[\begin{array}{l}a\\ b\end{array}\right]}^{\top }{\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}\left[\begin{array}{l}a\\ b\end{array}\right]\right)\\ \propto \exp \left(-\frac{1}{2}{\left[\begin{array}{l}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 }_{\mathrm{A}}^{-1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{l}a\\ b\end{array}\right]\right)\\ \propto \exp \left(-\frac{1}{2}\left[a^{\top }{\left(b-C{A}^{-1}a\right)}^{\top }\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_{\mathbf{A}}^{-1}\end{array}\right]\left[\begin{array}{c}a\\ b-C{A}^{-1}a\end{array}\right]\right)\\ \propto \exp \left(-\frac{1}{2}\left(a^{\top }{A}^{-1}a\right)+{\left(b-C{A}^{-1}a\right)}^{\top }{\Delta }_{\mathbf{A}}^{-1}\left(b-C{A}^{-1}a\right)\right)\\ \propto \underset{p(a)}{\underbrace{\exp \left(-\frac{1}{2}{a}^{\top }{A}^{-1}a\right)}}\underset{p(b]a)}{\underbrace{\exp \left(-\frac{1}{2}{\left(b-C{A}^{-1}a\right)}^{\top }{\Delta }_{\mathbf{A}}^{-1}\left(b-C{A}^{-1}a\right)\right)}}\end{array}$$
这意味着我们能从多元高斯分布 $\mathrm{P}(\mathrm{a},\mathrm{b})$ 中分解得到边际概率 $\mathrm{p}(\mathrm{a})$ 和 条件概率 $\mathrm{p}(\mathrm{b}|\mathrm{a})$ 。

边际概率和条件概率的信息矩阵

假设我们已知信息矩阵：
$${\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$
另外，由舒尔补矩阵求逆公式可知，协方差矩阵各块和信息矩阵之间有：
$${\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}{A}^{-1}+A^{-1}{C}^{\top }{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& -A^{-1}{C}^{\top }{\Delta }_{\mathrm{A}}^{-1}\\ -{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& {\Delta }_{\mathrm{A}}^{-1}\end{array}\right]\triangleq \left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$
由条件概率 $P(b\mid a)$ 的协方差为 ${\Delta }_A$ 以及公式, 易得其信息矩阵为
$${\Delta }_A^{-1}={\Lambda }_{bb}$$
由边际概率 $P(a)$ 的协方差为 $A$ 以及公式 , 易得其信息矩阵为：
$$A^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba}$$

总结

关于$\mathbf{P}(\mathbf{a})$
$$\begin{array}{l}P(a)={\int }_{b}P(a,b)\\ P(a)\propto \exp \left(-\frac{1}{2}{a}^{\top }{A}^{-1}a\right)\sim \mathcal{N}(0,A)\end{array}$$

启示：边际概率的协方差就是从联合分布中取对应的矩阵块就行了。

关于$\mathbf{P}(\mathbf{b}|\mathbf{a})$
$$P(b|a)\propto \exp \left(-\frac{1}{2}{\left(b-C{A}^{-1}a\right)}^{\top }{\Delta }_{\mathbf{A}}^{-1}\left(b-C{A}^{-1}a\right)\right)$$
启示： $P(b|a)\sim \mathcal{N}\left(C{A}^{-1}a,{\Delta }_A\right)$ 。协方差变为 $a$ 对应的舒尔补, 均值也变了。

最后
$$P(\mathbf{a},\mathbf{b})=\mathcal{N}\left(\left[\begin{array}{l}{\mathbf{\mu }}_a\\ {\mathbf{\mu }}_b\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{aa}& {\mathbf{\Sigma }}_{ab}\\ {\mathbf{\Sigma }}_{ba}& {\mathbf{\Sigma }}_{bb}\end{array}\right]\right)={\mathcal{N}}^{-1}\left(\left[\begin{array}{l}{\eta }_a\\ {\eta }_b\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Lambda }}_{aa}& {\mathbf{\Lambda }}_{aa}\\ {\mathbf{\Lambda }}_{ba}& {\mathbf{\Lambda }}_{bb}\end{array}\right]\right)$$

以及
$$\begin{array}{ccc}& \text{ 边际概率 }& \text{ 条件概率 }\\ & p(\mathbf{a})=\int p(\mathbf{a},\mathbf{b})d\mathbf{b}& p(\mathbf{a}\mid \mathbf{b})=p(\mathbf{a},\mathbf{b})/p(\mathbf{b})\\ \text{ 协方差矩阵 }& \mathbf{\mu }={\mathbf{\mu }}_a& {\mathbf{\mu }}^{\prime }={\mathbf{\mu }}_a+{\Sigma }_{ab}{\Sigma }_{bb}^{-1}\left(\mathbf{b}-{\mathbf{\mu }}_b\right)\\ & \Sigma ={\Sigma }_{aa}& {\Sigma }^{\prime }={\Sigma }_{aa}-{\Sigma }_{ab}{\Sigma }_{bb}^{-1}{\Sigma }_{ba}\\ \text{ 信息矩阵 }& \begin{array}{c}\mathbf{\eta }={\mathbf{\eta }}_a-{\Lambda }_{a\beta }{\Lambda }_{bb}^{-1}{\mathbf{\eta }}_b\\ \Lambda ={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}{}^{-1}{\Lambda }_{ba}\end{array}& \begin{array}{c}{\mathbf{\eta }}^{\prime }={\mathbf{\eta }}_a-{\Lambda }_{ab}\mathbf{b}\\ {\Lambda }^{\prime }={\Lambda }_{aa}\end{array}\end{array}$$

参考资料

深蓝学院手写vio课程