舒尔补在SLAM中的应用
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总结
边际概率对于协方差矩阵的操作是很容易的，但不好操作信息矩阵。条件概率恰好相反，对于信息矩阵容易操作，不好操作协方差矩阵。
表格总结如下
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)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。
该部分内容还有很多细节，以后在继续补充。
1345
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
