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

秃头队长
782 阅读3分钟

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

首先关于什么是协方差矩阵与信息矩阵。 在零均值的多元高斯分布中有如下概率形式：p(x)=1Zexp(−12x⊤Σ−1x)p(x)={1\over Z}exp(-{1\over 2}x^⊤Σ^{−1}x)其中ΣΣ为协方差矩阵，而协方差矩阵的逆Λ=Σ−1Λ = Σ−1则为信息矩阵。比如在XX为三维变量时，协方差矩阵Σij=E(xixj)Σ_{ij}=E(x_ix_j)为对应元素求期望。

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

在这里插入图片描述在这里插入图片描述

根据协方差的计算公式Σij=E(xixj)Σ_{ij}=E(x_ix_j)，可求解得到：

Σ11=E(x1x1)=E(w1v2+v1)(w1v2+v1)=w12E(v22)+2w1E(v1v2)+E(v12)=w12σ22+σ12Σ_{11}=E(x_1x_1)=E(w_1v_2+v_1)(w_1v_2+v_1) \\ =w_1^2E(v^2_2)+2w_1E(v_1v_2)+E(v_1^2) \\ =w^2_1 \sigma^2_2+\sigma^2_1

同理可得到整个协方差矩阵：

Σ=[w12σ22+σ12w1σ22w1w3σ22w1σ22σ22w3σ22w1w3σ22w3σ22w32σ22+σ32]Σ=\begin{bmatrix} w^2_1 \sigma^2_2+\sigma^2_1 & w_1\sigma^2_2 & w_1w_3\sigma^2_2 \\ w_1\sigma^2_2 & \sigma_2^2 & w_3\sigma^2_2 \\ w_1w_3\sigma^2_2 & w_3\sigma^2_2 & w_3^2\sigma^2_2+\sigma^2_3\\ \end{bmatrix}

其信息矩阵的求解等于它的逆，直接求解较为困难，这里通过联合高斯分布计算得到协方差的逆。

p(x1,x2,x3)=p(x2)p(x1∣x2)p(x3∣x2)p(x_1,x_2,x_3)=p(x_2)p(x_1|x_2)p(x_3|x_2)

带入如下形式：

p(x)=1Zexp(−12x⊤Σ−1x)p(x)={1\over Z}exp(-{1\over 2}x^⊤Σ^{−1}x)
p(x1,x2,x3)=1Z1exp(−x222σ22)1Zexp(−(x1−w1x2)22σ12)1Zexp(−(x3−w3x2)2σ32)=12exp(−12[x1x2x3][1σ12−w1σ120−w1σ12w12σ12+1σ22+w32σ12−w3σ320w3σ321σ32][x1x2x3])=1Zexp(−12x⊤Σ−1x)p(x_1,x_2,x_3)={\color{green}{1\over Z_1}exp(-{x_2^2\over 2\sigma^2_2})}{\color{red}{1\over Z}exp(-{(x_1-w_1x_2)^2\over 2\sigma_1^2})}{\color{blue}{1\over Z}exp(-{(x_3-w_3x_2)^2\over \sigma_3^2})} \\ ={1\over 2}exp(-{1\over 2} \begin{bmatrix} x_1 & x_2 & x_3\\ \end{bmatrix} \begin{bmatrix} {\color{red}{1\over \sigma_1^2} }& {\color{red}-{w_1\over \sigma_1^2} }& 0 \\{\color{red} -{w_1\over \sigma_1^2}} & {\color{red}{w^2_1\over \sigma_1^2}}+{\color{green}{1\over \sigma_2^2}}+{\color{blue}{w^2_3\over \sigma_1^2}} &{\color{blue} -{w_3\over \sigma_3^2} }\\ 0 & {\color{blue}{w_3\over \sigma_3^2} }& {\color{blue}{1\over \sigma_3^2}}\\ \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\ x_3\\ \end{bmatrix} ) \\ ={1\over Z}exp(-{1\over 2}x^⊤Σ^{−1}x)

由此得到协方差矩阵的逆：Σ−1Σ^{−1}，即信息矩阵。 协方差逆矩阵中如果坐标为(i,j)(i, j)的元素为 0，表示元素 ii 和 jj在其他变量固定的情况下条件独立。协方差中非对角元素Σij>0Σ_{ij}>0表示俩变量正相关，而在信息矩阵Λij<0Λ_{ij}<0表示正相关。

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

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

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

舒尔补的由来，将M矩阵变成上三角或者下三角的过程中：

[I0−CA−1I][ABCD]=[AB0ΔA]\begin{bmatrix} I &0 \\ -CA^{-1}&I\\ \end{bmatrix} \begin{bmatrix} A&B \\C&D\\ \end{bmatrix}= \begin{bmatrix} A&B \\0&\Delta_A\\ \end{bmatrix}
[ABCD][I−A−1B0I]=[A0CΔA]\begin{bmatrix} A&B \\C&D\\ \end{bmatrix} \begin{bmatrix} I &-A^{-1}B \\0 &I\\ \end{bmatrix} = \begin{bmatrix} A&0 \\C&\Delta_A\\ \end{bmatrix}

其中，ΔA=D−CA−1B\Delta_A=D-CA^{-1}B，联合上式，可将M矩阵变成对角形矩阵：

[I0−CA−1I][ABCD][I−A−1B0I]=[A00ΔA]\begin{bmatrix} I &0 \\ -CA^{-1}&I\\ \end{bmatrix} \begin{bmatrix} A&B \\C&D\\ \end{bmatrix} \begin{bmatrix} I &-A^{-1}B \\0 &I\\ \end{bmatrix} = \begin{bmatrix} A&0 \\0&\Delta_A\\ \end{bmatrix}

矩阵M可写成：

[ABCD]=[I0CA−1I][A00ΔA][IA−1B0I]\begin{bmatrix} A&B \\C&D\\ \end{bmatrix} = \begin{bmatrix} I &0 \\ CA^{-1}&I\\ \end{bmatrix}\begin{bmatrix} A&0 \\0&\Delta_A\\ \end{bmatrix}\begin{bmatrix} I &A^{-1}B \\0 &I\\ \end{bmatrix}

因为

[I−A−1B0I][IA−1B0I]=I\begin{bmatrix} I &-A^{-1}B \\0 &I\\ \end{bmatrix}\begin{bmatrix} I &A^{-1}B \\0 &I\\ \end{bmatrix}=I

由此可得到矩阵M的逆：

[ABCD]−1=[I−A−1B0I][A−100ΔA−1][I0−CA−1I]\begin{bmatrix} A&B \\C&D\\ \end{bmatrix} ^{-1}=\begin{bmatrix} I &-A^{-1}B \\0 &I\\ \end{bmatrix}\begin{bmatrix} A^{-1}&0 \\0&\Delta^{-1}_A\\ \end{bmatrix} \begin{bmatrix} I &0 \\- CA^{-1}&I\\ \end{bmatrix}

在百度百科中有一段背景很容易理解：

Ax+By=aCx+Dy=bAx+By=a \\ Cx+Dy=b

对第二个式子乘上BD−1BD^{-1}便可以得到

BD−1CX+By=BD−1bBD^{-1}CX+By=BD^{-1}b

然后与第一个式子相减：

(A−BD−1C)x=a−BD−1b(A-BD^{-1}C)x=a-BD^{-1}b

舒尔补应用于多元高斯分布（例）： 假设多元变量xx服从高斯分布，且有两部分组成：x=[ab]⊤x=\begin{bmatrix} a & b\\ \end{bmatrix}^{⊤}，变量之间构成的协方差矩阵为：

K=[AC⊤CD]K=\begin{bmatrix} A&C^⊤\\C&D\\ \end{bmatrix}

其中 A=cov(a,a),D=cov(b,b),C=cov(a,b)A=cov(a,a),D=cov(b,b),C=cov(a,b)，由此变量xx的概率分布为：

P(a,b)=P(a)P(b∣a)∝exp(−12[ab]⊤[AC⊤CD][ab])∝exp(−12[ab]⊤[I−A−1C⊤0I][A−100ΔA−1][I0−CA−1I][ab])∝exp(−12a⊤A−1a)⏟p(a)exp(−12(b−A−1C⊤b)⊤ΔA−1(b−A−1C⊤b))⏟p(b∣a)P(a,b)=P(a)P(b|a)∝exp\left({-{1\over 2}\begin{bmatrix} a \\ b\\ \end{bmatrix}^{⊤}\begin{bmatrix} A&C^⊤\\C&D\\ \end{bmatrix}\begin{bmatrix} a \\ b\\ \end{bmatrix}}\right) \\ ∝exp\left({-{1\over 2}\begin{bmatrix} a \\ b\\ \end{bmatrix}^{⊤}\begin{bmatrix} I &-A^{-1}C^⊤ \\0 &I\\ \end{bmatrix}\begin{bmatrix} A^{-1}&0 \\0&\Delta^{-1}_A\\ \end{bmatrix} \begin{bmatrix} I &0 \\- CA^{-1}&I\\ \end{bmatrix} \begin{bmatrix} a \\ b\\ \end{bmatrix}}\right) \\ ∝\underbrace{exp \left({-{1\over 2}a^⊤A^{-1}a}\right)}_{p(a)} \underbrace{exp\left({-{1\over 2}(b-A^{-1}C^⊤b)^⊤\Delta^{-1}_A(b-A^{-1}C^⊤b)}\right)}_{p(b|a)}

使用舒尔补之后，便可以从多元高斯分布P(a,b)P(a,b)中分解得到边际概率p(a)p(a)和条件概率p(b∣a)p(b|a)。 边际概率P(a)∼N(0,A)P(a) ∼\mathcal N (0,A)的协方差就是从联合分布中取对应的矩阵块就行了。P(b∣a)∼N(A−1C⊤b,ΔA)P(b|a) ∼\mathcal N (A^{−1}C^⊤b, \Delta_A)，协方差变为 aa 对应的舒尔补，均值也变了。 在基于优化的 SLAM 问题中，我们往往直接操作的是信息矩阵，而不是协方差矩阵。

因此假设我们已知信息矩阵：

Λ=K−1=[AC⊤CD]−1Λ=K^{-1}=\begin{bmatrix} A&C^⊤\\C&D\\ \end{bmatrix}^{-1}

根据舒尔补公式中的逆运算可得到：

[AC⊤CD]−1=[A−1+A−1C⊤ΔA−1CA−1−A−1C⊤ΔA−1−ΔA−1CA−1ΔA−1]=△[ΛaaΛabΛbaΛbb]\begin{bmatrix} A&C^⊤\\C&D\\ \end{bmatrix}^{-1}=\begin{bmatrix} A^{-1}+A^{-1}C^⊤\Delta_A^{-1}CA^{-1} & -A^{-1}C^⊤\Delta^{-1}_A\\-\Delta^{-1}_ACA^{-1}& \Delta^{-1}_A\\ \end{bmatrix} \overset{△}{=}\begin{bmatrix} Λ_{aa}&Λ_{ab}\\Λ_{ba}&Λ_{bb}\\ \end{bmatrix}

在上面求解协方差的时候已经知道： 条件概率P(b∣a)∼N(A−1C⊤b,ΔA)P(b|a) ∼\mathcal N (A^{−1}C^⊤b, \Delta_A)，其协方差为ΔA\Delta_A，则其信息矩阵Λ=K−1=ΔA−1=Λbb{\color{red}Λ=K^{-1}=\Delta_A^{-1}=Λ_{bb}}。 边际概率P(a)∼N(0,A)P(a) ∼\mathcal N (0,A)，其协方差为AA，可通过消元得到其信息矩阵Λ=A−1=Λaa−ΛabΛbb−1Λba{\color{red}Λ=A^{-1}=Λ_{aa}-Λ_{ab}Λ_{bb}^{-1}Λ_{ba}}。

综上，便可以使用舒尔补去除信息矩阵中的变量，例如去除例子中的x3x_3

Σ−1=[1σ12−w1σ120−w1σ12w12σ12+1σ22+w32σ12−w3σ320w3σ321σ32]Σ^{-1} = \left[ \begin{array}{c c|c} {\color{red}{1\over \sigma_1^2} }& {\color{red}-{w_1\over \sigma_1^2} }& 0 \\{\color{red} -{w_1\over \sigma_1^2}} & {\color{red}{w^2_1\over \sigma_1^2}}+{\color{green}{1\over \sigma_2^2}}+{\color{blue}{w^2_3\over \sigma_1^2}} &{\color{blue} -{w_3\over \sigma_3^2} } \\ \hline0 & {\color{blue}{w_3\over \sigma_3^2} }& {\color{blue}{1\over \sigma_3^2}}\\ \end{array}\right]

从联合分布 P(x1,x2,x3)P(x1, x2, x3) 中 marg 掉变量 x3x_3 ，即 P(x1,x2)P(x1, x2) 对应的信息矩阵可以用上面红色公式得到:

K2−1=Λaa−ΛabΛbb−1Λba=Λaa−[0−w3σ32]σ32[0−w3σ32]=Λaa[000w3σ32]=[1σ12−w1σ12−w1σ12w12σ12+1σ22]K_2^{-1}=Λ_{aa}-Λ_{ab}Λ_{bb}^{-1}Λ_{ba}\\=Λ_{aa}-\begin{bmatrix} 0 \\ -{w_3 \over \sigma^2_3}\\ \end{bmatrix}\sigma^2_3\begin{bmatrix} 0&-{w_3 \over \sigma^2_3}\\ \end{bmatrix}\\=Λ_{aa}\begin{bmatrix} 0&0 \\0& {w_3 \over \sigma^2_3}\\ \end{bmatrix}=\begin{bmatrix} {\color{red} {1 \over \sigma^2_1}} & {\color{red} -{w_1\over \sigma^2_1} }\\ {\color{red} -{w_1 \over \sigma^2_1} }& {\color{red}{w^2_1 \over \sigma^2_1}}+ {\color{green}{1 \over \sigma^2_2}}\\ \end{bmatrix}

可以发现，和x3x_3有关的

蓝色部分已经被消除。

avatar
文章
阅读
粉丝