舒尔补在SLAM中的应用

分手后的思念是犯贱 2021-11-16 16:02 253阅读 0赞

### 舒尔补在SLAM中的应用 ###

*  1.舒尔补的定义
 *  2.舒尔补的由来
 *  3.舒尔补在多元高斯分布中的应用
 *   *  3.1 多元变量的高斯分布
     *  3.2 边缘概率和条件概率的协方差矩阵
     *  3.3 边缘概率和条件概率的信息矩阵
     *  3.4总结
 *  4. 舒尔补在vslam中的应用

# 1.舒尔补的定义 #

对于任意的矩阵 M M M，如下所示  
 (1) M = \[ A B C D \] M = \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\]\\tag\{1\} M=\[ACBD\](1)

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

# 2.舒尔补的由来 #

在将 M M M变为上三角和下三角的过程中，都会遇到舒尔补：  
 (2) \[ I 0 − C A − 1 I \] \[ A B C D \] = \[ A B 0 Δ A \] \\left\[ \\begin\{matrix\} I& 0\\\\ -CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} A& B\\\\ 0& \\Delta A \\\\ \\end\{matrix\}\\right\] \\tag\{2\} \[I−CA−10I\]\[ACBD\]=\[A0BΔA\](2)  
 (3) \[ A B C D \] \[ I − A − 1 B 0 I \] = \[ A 0 C Δ A \] \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& -A^\{-1\}B\\\\ 0& I \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} A& 0\\\\ C& \\Delta A \\\\ \\end\{matrix\}\\right\] \\tag\{3\} \[ACBD\]\[I0−A−1BI\]=\[AC0ΔA\](3)  
其中： Δ A = D − C A − 1 B \\Delta A =D-CA^\{-1\}B ΔA=D−CA−1B。将两式联合起来，将M变形为对角形：  
 (4) \[ I 0 − C A − 1 I \] \[ A B C D \] \[ I − A − 1 B 0 I \] = \[ A 0 0 Δ A \] \\left\[ \\begin\{matrix\} I& 0\\\\ -CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& -A^\{-1\}B\\\\ 0& I \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} A& 0\\\\ 0& \\Delta A \\\\ \\end\{matrix\}\\right\] \\tag\{4\} \[I−CA−10I\]\[ACBD\]\[I0−A−1BI\]=\[A00ΔA\](4)

反过来，可以从对角形恢复 M M M：  
 (5) \[ I 0 C A − 1 I \] \[ A 0 0 Δ A \] \[ I A − 1 B 0 I \] = \[ A B C D \] \\left\[ \\begin\{matrix\} I& 0\\\\ CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A& 0\\\\ 0& \\Delta A \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& A^\{-1\}B\\\\ 0& I \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] \\tag\{5\} \[ICA−10I\]\[A00ΔA\]\[I0A−1BI\]=\[ACBD\](5)

舒尔补可以快速求解矩阵的逆

因为  
 (6) M = \[ A B C D \] = \[ I 0 C A − 1 I \] \[ A 0 0 Δ A \] \[ I A − 1 B 0 I \] M= \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} I& 0\\\\ CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A& 0\\\\ 0& \\Delta A \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& A^\{-1\}B\\\\ 0& I \\\\ \\end\{matrix\}\\right\] \\tag\{6\} M=\[ACBD\]=\[ICA−10I\]\[A00ΔA\]\[I0A−1BI\](6)  
所以  
 (7) M − 1 = \[ A B C D \] = \[ I − A − 1 B 0 I \] \[ A − 1 0 0 Δ A − 1 \] \[ I 0 − C A − 1 I \] M^\{-1\}= \\left\[ \\begin\{matrix\} A& B\\\\ C& D \\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} I& -A^\{-1\}B\\\\ 0& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A^\{-1\}& 0\\\\ 0& \\Delta A^\{-1\} \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& 0\\\\ -CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\tag\{7\} M−1=\[ACBD\]=\[I0−A−1BI\]\[A−100ΔA−1\]\[I−CA−10I\](7)

# 3.舒尔补在多元高斯分布中的应用 #

## 3.1 多元变量的高斯分布 ##

假设多元变量  x x x 服从高斯分布，且由两部分组成： x = \[ a , b \] T x =\[a,b\]^T x=\[a,b\]T，变量之间构成之间的协方差矩阵为：  
 (8) K = \[ A C T C D \] K = \\left\[ \\begin\{matrix\} A& C^T\\\\ C& D \\\\ \\end\{matrix\}\\right\] \\tag\{8\} K=\[ACCTD\](8)

其中 A = c o v ( a , a ) , D = c o v ( b , b ) , C = c o v ( a , b ) A=cov(a,a) ,D = cov(b,b) , C = cov(a,b) A=cov(a,a),D=cov(b,b),C=cov(a,b)。所以变量 x x x的概率分布为  
 (9) P ( a , b ) = P ( a ) P ( b ∣ a ) ∝ e x p ( − 1 2 \[ a b \] T \[ A C T C D \] − 1 \[ a b \] ) P(a,b) = P(a)P(b|a)\\propto exp\\left( -\\frac\{1\}\{2\}\\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\]^T \\left\[ \\begin\{matrix\} A& C^T\\\\ C& D \\\\ \\end\{matrix\}\\right\]^\{-1\} \\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\] \\right)\\tag\{9\} P(a,b)=P(a)P(b∣a)∝exp(−21\[ab\]T\[ACCTD\]−1\[ab\])(9)

利用舒尔补对上式进行分解，则有  
 (10) P ( a , b ) ∝ e x p ( − 1 2 \[ a b \] T \[ A C T C D \] − 1 \[ a b \] ) ∝ e x p ( − 1 2 \[ a b \] T \[ I − A − 1 C T 0 I \] \[ A − 1 0 0 Δ A − 1 \] \[ I 0 − C A − 1 I \] \[ a b \] ) ∝ e x p ( − 1 2 \[ a T ( b − C A − 1 a ) T \] \[ A − 1 0 0 Δ A − 1 \] \[ a b − C A − 1 a \] ) ∝ e x p ( − 1 2 ( a T A − 1 a ) + ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) ∝ e x p ( − 1 2 ( a T A − 1 a ) ) e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(a,b) \\propto exp\\left( -\\frac\{1\}\{2\}\\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\]^T \\left\[ \\begin\{matrix\} A& C^T\\\\ C& D \\\\ \\end\{matrix\}\\right\]^\{-1\} \\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\] \\right) \\\\ \\propto exp\\left( -\\frac\{1\}\{2\}\\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\]^T \\left\[ \\begin\{matrix\} I& -A^\{-1\}C^T\\\\ 0& I \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A^\{-1\}& 0\\\\ 0& \\Delta A^\{-1\} \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} I& 0\\\\ -CA^\{-1\}& I \\\\ \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} a\\\\ b\\\\ \\end\{matrix\} \\right\] \\right) \\\\ \\propto exp\\left( -\\frac\{1\}\{2\} \\left\[ \\begin\{matrix\} a^T & (b-CA^\{-1\}a)^T\\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} A^\{-1\}& 0\\\\ 0& \\Delta A^\{-1\} \\\\ \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} a\\\\ b-CA^\{-1\}a \\\\ \\end\{matrix\}\\right\] \\right) \\\\ \\propto exp\\left( -\\frac\{1\}\{2\} (a^TA^\{-1\}a)+ (b-CA^\{-1\}a)^T\\Delta A(b-CA^\{-1\}a) \\right) \\\\ \\propto exp\\left( -\\frac\{1\}\{2\} (a^TA^\{-1\}a)\\right) exp\\left(-\\frac\{1\}\{2\} (b-CA^\{-1\}a)^T\\Delta A(b-CA^\{-1\}a) \\right) \\\\ \\tag\{10\} P(a,b)∝exp(−21\[ab\]T\[ACCTD\]−1\[ab\])∝exp(−21\[ab\]T\[I0−A−1CTI\]\[A−100ΔA−1\]\[I−CA−10I\]\[ab\])∝exp(−21\[aT(b−CA−1a)T\]\[A−100ΔA−1\]\[ab−CA−1a\])∝exp(−21(aTA−1a)\+(b−CA−1a)TΔA(b−CA−1a))∝exp(−21(aTA−1a))exp(−21(b−CA−1a)TΔA(b−CA−1a))(10)  
所以有 (11) P ( a ) = e x p ( − 1 2 ( a T A − 1 a ) ) P(a) =exp\\left( -\\frac\{1\}\{2\}(a^TA^\{-1\}a)\\right)\\tag\{11\} P(a)=exp(−21(aTA−1a))(11)  
 (12) P ( b ∣ a ) = e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(b|a) =exp\\left(-\\frac\{1\}\{2\} (b-CA^\{-1\}a)^T\\Delta A(b-CA^\{-1\}a) \\right) \\tag\{12\} P(b∣a)=exp(−21(b−CA−1a)TΔA(b−CA−1a))(12)

这意味着我们能从多元高斯分布 P ( a , b ) P(a,b) P(a,b)中分解得到边界概率 P ( a ) P(a) P(a)和条件概率P(b|a)。

## 3.2 边缘概率和条件概率的协方差矩阵 ##

对于边缘概率 P ( a ) P(a) P(a)，有  
 (13) P ( a ) = ∫ P ( a , b ) d b P(a) = \\int P(a,b)db\\tag\{13\} P(a)=∫P(a,b)db(13)  
 (14) P ( a ) = e x p ( − 1 2 ( a T A − 1 a ) ) ∼ N ( 0 , A ) P(a) =exp\\left( -\\frac\{1\}\{2\}(a^TA^\{-1\}a)\\right) \\sim N(0,A)\\tag\{14\} P(a)=exp(−21(aTA−1a))∼N(0,A)(14)  
特点：边缘概率 P ( a ) P(a) P(a)的协方差就是从联合概率分布的协方差矩阵中取对应的矩阵块即可

对于条件概率 P ( b ∣ a ) P(b|a) P(b∣a)，有  
 (15) P ( b ∣ a ) = e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(b|a) =exp\\left(-\\frac\{1\}\{2\} (b-CA^\{-1\}a)^T\\Delta A(b-CA^\{-1\}a) \\right) \\tag\{15\} P(b∣a)=exp(−21(b−CA−1a)TΔA(b−CA−1a))(15)  
特点：条件概率 P ( b ∣ a ) ∼ N ( C A − 1 a , Δ A ) P(b|a)\\sim N(CA^\{-1\}a,\\Delta A) P(b∣a)∼N(CA−1a,ΔA)，协方差为 a a a对应的舒尔补 Δ A \\Delta A ΔA，均值为 C A − 1 a CA^\{-1\}a CA−1a。

## 3.3 边缘概率和条件概率的信息矩阵 ##

信息矩阵是协方差矩阵的逆，所以变量 x x x的信息矩阵为  
 (16) K − 1 = \[ A C T C D \] − 1 = \[ Λ a a Λ a b Λ b a Λ b b \] K^\{-1\} = \\left\[ \\begin\{matrix\} A& C^T\\\\ C& D \\\\ \\end\{matrix\}\\right\] ^\{-1\} = \\left\[ \\begin\{matrix\} \\Lambda\_\{aa\}& \\Lambda\_\{ab\}\\\\ \\Lambda\_\{ba\}& \\Lambda\_\{bb\} \\\\ \\end\{matrix\}\\right\] \\tag\{16\} K−1=\[ACCTD\]−1=\[ΛaaΛbaΛabΛbb\](16)  
由公式(7)可知，信息矩阵与协方差矩阵元素之间的关系为  
 (17) K − 1 = \[ A − 1 + A − 1 C T Δ A − 1 C A − 1 − A − 1 C T Δ A − 1 − Δ A − 1 C A − 1 Δ A − 1 \] = \[ Λ a a Λ a b Λ b a Λ b b \] K^\{-1\} = \\left\[ \\begin\{matrix\} A^\{-1\}+A^\{-1\}C^T\\Delta A^\{-1\}CA^\{-1\}& -A^\{-1\}C^T\\Delta A^\{-1\}\\\\ -\\Delta A^\{-1\}CA^\{-1\}& \\Delta A^\{-1\}\\\\ \\end\{matrix\}\\right\] = \\left\[ \\begin\{matrix\} \\Lambda\_\{aa\}& \\Lambda\_\{ab\}\\\\ \\Lambda\_\{ba\}& \\Lambda\_\{bb\} \\\\ \\end\{matrix\}\\right\] \\tag\{17\} K−1=\[A−1\+A−1CTΔA−1CA−1−ΔA−1CA−1−A−1CTΔA−1ΔA−1\]=\[ΛaaΛbaΛabΛbb\](17)  
由(14)知 边缘概率的协方差矩阵为 A A A，所以其对应的信息矩阵为 A − 1 A^\{-1\} A−1，根据式(17)可知  
 (18) A − 1 = A − 1 + A − 1 C T Δ A − 1 C A − 1 − ( − A − 1 C T Δ A − 1 ( Δ A − 1 ) − 1 − Δ A − 1 C A − 1 ) = Λ a a − Λ a b Λ b b − 1 Λ b a A^\{-1\} = A^\{-1\}+A^\{-1\}C^T\\Delta A^\{-1\}CA^\{-1\}-(-A^\{-1\}C^T\\Delta A^\{-1\}( \\Delta A^\{-1\})^\{-1\} -\\Delta A^\{-1\}CA^\{-1\}) =\\Lambda\_\{aa\}-\\Lambda\_\{ab\}\\Lambda\_\{bb\}^\{-1\}\\Lambda\_\{ba\}\\tag\{18\} A−1=A−1\+A−1CTΔA−1CA−1−(−A−1CTΔA−1(ΔA−1)−1−ΔA−1CA−1)=Λaa−ΛabΛbb−1Λba(18)  
即边缘概率 P ( a ) P(a) P(a)的信息矩阵为 Λ a a − Λ a b Λ b b − 1 Λ b a \\Lambda\_\{aa\}-\\Lambda\_\{ab\}\\Lambda\_\{bb\}^\{-1\}\\Lambda\_\{ba\} Λaa−ΛabΛbb−1Λba。

由式(15)可知条件概率 P ( b ∣ a ) P(b|a) P(b∣a)的协方差矩阵为 Δ A \\Delta A ΔA，所以其信息矩阵为 Δ A − 1 = Λ b b \\Delta A^\{-1\} =\\Lambda\_\{bb\} ΔA−1=Λbb。

## 3.4总结 ##

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

表格总结如下  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]

# 4. 舒尔补在vslam中的应用 #

随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。

但是该如何移除旧的状态变量呢？

直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。即根据舒尔补的在边缘概率方面得到的结论，从 P ( x 1 , x 2 , x 3 , . . . . . , x n ) P(x\_\{1\},x\_\{2\},x\_\{3\},.....,x\_\{n\}) P(x1,x2,x3,.....,xn)的协方差矩阵或信息矩阵中求得 P ( x 2 , x 3 , . . . . . , x n ) P(x\_\{2\},x\_\{3\},.....,x\_\{n\}) P(x2,x3,.....,xn)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。

该部分内容还有很多细节，以后在继续补充。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]: /images/20211116/c9f6bfee2d904024a6279e5322a5eb4b.png