舒爾補在SLAM中的應用

• 1.舒爾補的定義
• 2.舒爾補的由來
• 3.舒爾補在多元高斯分布中的應用
• • 3.1 多元變量的高斯分布
  • 3.2 邊緣概率和條件概率的協方差矩陣
  • 3.3 邊緣概率和條件概率的信息矩陣
  • 3.4總結
• 4. 舒爾補在vslam中的應用

1.舒爾補的定義

對于任意的矩陣$M$，如下所示
$$M=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\text{\hspace{1em}(1)}$$

如果矩陣塊$D$是可逆的，則 $A?B{D}^{?1}C$ 稱之為 $D$ 關于$M$的舒爾補。
如果矩陣塊$A$是可逆的，則 $D?C{A}^{?1}B$ 稱之為 $A$ 關于$M$的舒爾補。

2.舒爾補的由來

在將$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]\text{\hspace{1em}(2)}$$
$$\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]\text{\hspace{1em}(3)}$$
其中：$\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]\text{\hspace{1em}(4)}$$

反過來，可以從對角形恢復$M$：
$$\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}A& B\\ C& D\end{array}\right]\text{\hspace{1em}(5)}$$

舒爾補可以快速求解矩陣的逆

因為
$$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]\text{\hspace{1em}(6)}$$
所以
$$M^{?1}=\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}^{?1}& 0\\ 0& \Delta A^{?1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ ?C{A}^{?1}& I\end{array}\right]\text{\hspace{1em}(7)}$$

3.舒爾補在多元高斯分布中的應用

3.1 多元變量的高斯分布

假設多元變量 $x$ 服從高斯分布，且由兩部分組成：$x=[a,b{]}^T$，變量之間構成之間的協方差矩陣為：
$$K=\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]\text{\hspace{1em}(8)}$$

其中$A=cov(a,a),D=cov(b,b),C=cov(a,b)$。所以變量$x$的概率分布為
$$P(a,b)=P(a)P(b|a)\propto exp\left(?\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^T{\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{?1}\left[\begin{array}{c}a\\ b\end{array}\right]\right)\text{\hspace{1em}(9)}$$

利用舒爾補對上式進行分解，則有
$$\begin{gathered} P(a,b)\propto exp\left(?\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^T{\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{?1}\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]}^T\left[\begin{array}{cc}I& ?A^{?1}{C}^T\\ 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 exp\left(?\frac{1}{2}\left[\begin{array}{cc}{a}^T& (b?C{A}^{?1}a{)}^T\end{array}\right]\left[\begin{array}{cc}{A}^{?1}& 0\\ 0& \Delta 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}(a^T{A}^{?1}a)+(b?C{A}^{?1}a{)}^T\Delta A(b?C{A}^{?1}a)\right) \\ \propto exp\left(?\frac{1}{2}(a^T{A}^{?1}a)\right)exp\left(?\frac{1}{2}(b?C{A}^{?1}a{)}^T\Delta A(b?C{A}^{?1}a)\right) \\ \text{\hspace{1em}(10)} \end{gathered}$$
所以有$$P(a)=exp\left(?\frac{1}{2}(a^T{A}^{?1}a)\right)\text{\hspace{1em}(11)}$$
$$P(b|a)=exp\left(?\frac{1}{2}(b?C{A}^{?1}a{)}^T\Delta A(b?C{A}^{?1}a)\right)\text{\hspace{1em}(12)}$$

這意味著我們能從多元高斯分布$P(a,b)$中分解得到邊界概率$P(a)$和條件概率P(b|a)。

3.2 邊緣概率和條件概率的協方差矩陣

對于邊緣概率$P(a)$，有
$$P(a)=\int P(a,b)db\text{\hspace{1em}(13)}$$
$$P(a)=exp\left(?\frac{1}{2}(a^T{A}^{?1}a)\right)～N(0,A)\text{\hspace{1em}(14)}$$
特點：邊緣概率$P(a)$的協方差就是從聯合概率分布的協方差矩陣中取對應的矩陣塊即可

對于條件概率$P(b|a)$，有
$$P(b|a)=exp\left(?\frac{1}{2}(b?C{A}^{?1}a{)}^T\Delta A(b?C{A}^{?1}a)\right)\text{\hspace{1em}(15)}$$
特點：條件概率$P(b|a)～N(C{A}^{?1}a,\Delta A)$，協方差為$a$對應的舒爾補$\Delta A$，均值為$C{A}^{?1}a$。

3.3 邊緣概率和條件概率的信息矩陣

信息矩陣是協方差矩陣的逆，所以變量$x$的信息矩陣為
$$K^{?1}={\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{?1}=\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]\text{\hspace{1em}(16)}$$
由公式(7)可知，信息矩陣與協方差矩陣元素之間的關系為
$$K^{?1}=\left[\begin{array}{cc}{A}^{?1}+A^{?1}{C}^T\Delta A^{?1}C{A}^{?1}& ?A^{?1}{C}^T\Delta A^{?1}\\ ?\Delta A^{?1}C{A}^{?1}& \Delta A^{?1}\end{array}\right]=\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]\text{\hspace{1em}(17)}$$
由(14)知 邊緣概率的協方差矩陣為$A$，所以其對應的信息矩陣為$A^{?1}$，根據式(17)可知
$$A^{?1}=A^{?1}+A^{?1}{C}^T\Delta A^{?1}C{A}^{?1}?(?A^{?1}{C}^T\Delta A^{?1}(\Delta A^{?1}{)}^{?1}?\Delta A^{?1}C{A}^{?1})={\Lambda }_{aa}?{\Lambda }_{ab}{\Lambda }_{bb}^{?1}{\Lambda }_{ba}\text{\hspace{1em}(18)}$$
即邊緣概率$P(a)$的信息矩陣為${\Lambda }_{aa}?{\Lambda }_{ab}{\Lambda }_{bb}^{?1}{\Lambda }_{ba}$。

由式(15)可知條件概率$P(b|a)$的協方差矩陣為$\Delta A$，所以其信息矩陣為$\Delta A^{?1}={\Lambda }_{bb}$。

3.4總結

邊際概率對于協方差矩陣的操作是很容易的，但不好操作信息矩陣。條件概率恰好相反，對于信息矩陣容易操作，不好操作協方差矩陣。

表格總結如下

4. 舒爾補在vslam中的應用

隨著 VSLAM 系統不斷往新環境探索，就會有新的相機姿態以及看到新的環境特征，最小二乘殘差就會越來越多，信息矩陣越來越大，計算量將不斷增加。 為了保持優化變量的個數在一定范圍內，需要使用滑動窗口算法動態增加或移除優化變量。

但是該如何移除舊的狀態變量呢？

直接丟棄變量和對應的測量值，會損失信息。正確的做法是使用邊際概率，將丟棄變量所攜帶的信息傳遞給剩余變量。即根據舒爾補的在邊緣概率方面得到的結論，從$P(x_1,x_2,x_3,.....,x_n)$的協方差矩陣或信息矩陣中求得$P(x_2,x_3,.....,x_n)$的協方差矩陣或信息矩陣。該過程稱之為邊緣化。這是滑動窗口算法中非常重要的理論基礎。

該部分內容還有很多細節，以后在繼續補充。

總結

以上是生活随笔為你收集整理的舒尔补在SLAM中的应用的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。