【符号约定】
随机变量
X
=
[
X
m
T
,
X
r
T
]
T
\Chi=[\Chi^T_m,\Chi^T_r]^T
X=[XmT,XrT]T;
下标
m
,
r
m,r
m,r:分别表示margin, reserve;
Σ
,
Λ
\Sigma,\Lambda
Σ,Λ:分别表示协方差矩阵、信息矩阵;
p
(
X
)
=
p
(
X
m
,
X
r
)
p(\Chi)=p(\Chi_m,\Chi_r)
p(X)=p(Xm,Xr),表示联合概率;
p
(
X
r
)
=
∫
p
(
X
m
,
X
r
)
d
X
m
p(\Chi_r)=\int p(\Chi_m,\Chi_r)d\Chi_m
p(Xr)=∫p(Xm,Xr)dXm,表示边际概率。
假设随机变量服从零均值高斯分布。
已知联合概率 p ( X ) = p ( X m , X r ) p(\Chi)=p(\Chi_m,\Chi_r) p(X)=p(Xm,Xr)对应的信息矩阵 Λ \Lambda Λ,求边际概率 p ( X r ) p(\Chi_r) p(Xr)对应的信息矩阵 Λ r ′ \Lambda'_r Λr′。
解:首先,
Λ
\Lambda
Λ按变量的维数分块:
Λ
=
[
Λ
m
m
Λ
m
r
Λ
r
m
Λ
r
r
]
\Lambda=\begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ \Lambda_{rm} & \Lambda_{rr} \\ \end{bmatrix}
Λ=[ΛmmΛrmΛmrΛrr]
我们知道把
p
(
X
)
p(\Chi)
p(X)对应的信息矩阵求逆,将得到
p
(
X
)
p(\Chi)
p(X)对应的协方差矩阵(反之亦然):
(1)
Λ
−
1
=
[
Σ
m
m
Σ
m
r
Σ
r
m
Σ
r
r
]
=
Σ
\Lambda^{-1}=\begin{bmatrix} \Sigma_{mm} & \Sigma_{mr} \\ \Sigma_{rm} & \Sigma_{rr} \\ \end{bmatrix}=\Sigma \tag{1}
Λ−1=[ΣmmΣrmΣmrΣrr]=Σ(1)
式中, Σ r r \Sigma_{rr} Σrr即为边际概率 p ( X r ) p(\Chi_r) p(Xr)对应的协方差矩阵。【从高斯联合概率分布中,显然可得】
此时,我们把该
Σ
r
r
\Sigma_{rr}
Σrr求逆,将得到边际概率
p
(
X
r
)
p(\Chi_r)
p(Xr)对应的信息矩阵
Λ
r
′
\Lambda'_r
Λr′,即为所求!
Λ
r
′
=
Σ
r
r
−
1
\Lambda'_r=\Sigma^{-1}_{rr}
Λr′=Σrr−1
不难发现使用这种思路,我们求了两次逆。下面使用一些技巧:
对于公式(1),做一些变形构造
Σ
r
r
−
1
\Sigma^{-1}_{rr}
Σrr−1:
(2)
Λ
=
[
Λ
m
m
Λ
m
r
Λ
r
m
Λ
r
r
]
=
[
Σ
m
m
Σ
m
r
Σ
r
m
Σ
r
r
]
−
1
\Lambda=\begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ \Lambda_{rm} & \Lambda_{rr} \\ \end{bmatrix} =\begin{bmatrix} \Sigma_{mm} & \Sigma_{mr} \\ \Sigma_{rm} & \Sigma_{rr} \\ \end{bmatrix}^{-1} \tag{2}
Λ=[ΛmmΛrmΛmrΛrr]=[ΣmmΣrmΣmrΣrr]−1(2)
开始变形之前,给出两个公式(可以展开验证一下),
第一个:
[
Σ
m
m
Σ
m
r
Σ
r
m
Σ
r
r
]
=
[
I
Σ
m
r
Σ
r
r
−
1
0
I
]
[
Σ
m
m
−
Σ
m
r
Σ
r
r
−
1
Σ
r
m
0
0
Σ
r
r
]
[
I
0
Σ
r
r
−
1
Σ
r
m
I
]
\begin{bmatrix} \Sigma_{mm} & \Sigma_{mr} \\ \Sigma_{rm} & \Sigma_{rr} \\ \end{bmatrix} = \begin{bmatrix} I & \Sigma_{mr} \Sigma^{-1}_{rr} \\ 0 & I \end{bmatrix} \begin{bmatrix} \Sigma_{mm}-\Sigma_{mr}\Sigma^{-1}_{rr}\Sigma_{rm} & 0 \\ 0 & \Sigma_{rr} \\ \end{bmatrix} \begin{bmatrix} I & 0 \\ \Sigma^{-1}_{rr}\Sigma_{rm} & I \end{bmatrix}
[ΣmmΣrmΣmrΣrr]=[I0ΣmrΣrr−1I][Σmm−ΣmrΣrr−1Σrm00Σrr][IΣrr−1Σrm0I]
我们定义
Δ
Σ
r
r
≜
Σ
m
m
−
Σ
m
r
Σ
r
r
−
1
Σ
r
m
\Delta_{\Sigma_{rr}} \triangleq \Sigma_{mm}-\Sigma_{mr}\Sigma^{-1}_{rr}\Sigma_{rm}
ΔΣrr≜Σmm−ΣmrΣrr−1Σrm为
Σ
r
r
\Sigma_{rr}
Σrr关于
Σ
\Sigma
Σ的舒尔补。
第二个(求上式的逆):
[
Σ
m
m
Σ
m
r
Σ
r
m
Σ
r
r
]
−
1
=
[
I
0
−
Σ
r
r
−
1
Σ
r
m
I
]
[
Δ
Σ
r
r
−
1
0
0
Σ
r
r
−
1
]
[
I
−
Σ
m
r
Σ
r
r
−
1
0
I
]
=
[
Δ
Σ
r
r
−
1
−
Δ
Σ
r
r
−
1
Σ
m
r
Σ
r
r
−
1
−
Σ
r
r
−
1
Σ
r
m
Δ
Σ
r
r
−
1
Σ
r
r
−
1
+
Σ
r
r
−
1
Σ
r
m
Δ
Σ
r
r
−
1
Σ
m
r
Σ
r
r
−
1
]
\begin{aligned} \begin{bmatrix} \Sigma_{mm} & \Sigma_{mr} \\ \Sigma_{rm} & \Sigma_{rr} \\ \end{bmatrix}^{-1} &= \begin{bmatrix} I & 0 \\ -\Sigma^{-1}_{rr}\Sigma_{rm} & I \end{bmatrix} \begin{bmatrix} \Delta_{\Sigma_{rr}}^{-1} & 0 \\ 0 & \Sigma_{rr}^{-1} \\ \end{bmatrix} \begin{bmatrix} I & -\Sigma_{mr} \Sigma^{-1}_{rr} \\ 0 & I \end{bmatrix} \\ &= \begin{bmatrix} \Delta_{\Sigma_{rr}}^{-1} & -\Delta_{\Sigma_{rr}}^{-1} \Sigma_{mr} \Sigma_{rr}^{-1} \\ -\Sigma_{rr}^{-1} \Sigma_{rm} \Delta_{\Sigma_{rr}}^{-1} & \Sigma_{rr}^{-1}+\Sigma_{rr}^{-1} \Sigma_{rm} \Delta_{\Sigma_{rr}}^{-1} \Sigma_{mr} \Sigma_{rr}^{-1}\\ \end{bmatrix} \end{aligned}
[ΣmmΣrmΣmrΣrr]−1=[I−Σrr−1Σrm0I][ΔΣrr−100Σrr−1][I0−ΣmrΣrr−1I]=[ΔΣrr−1−Σrr−1ΣrmΔΣrr−1−ΔΣrr−1ΣmrΣrr−1Σrr−1+Σrr−1ΣrmΔΣrr−1ΣmrΣrr−1]
将上式展开,代入公式(2),得:
[
Λ
m
m
Λ
m
r
Λ
r
m
Λ
r
r
]
=
[
Δ
Σ
r
r
−
1
−
Δ
Σ
r
r
−
1
Σ
m
r
Σ
r
r
−
1
−
Σ
r
r
−
1
Σ
r
m
Δ
Σ
r
r
−
1
Σ
r
r
−
1
+
Σ
r
r
−
1
Σ
r
m
Δ
Σ
r
r
−
1
Σ
m
r
Σ
r
r
−
1
]
\begin{aligned} \begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ \Lambda_{rm} & \Lambda_{rr} \\ \end{bmatrix} &= \begin{bmatrix} \Delta_{\Sigma_{rr}}^{-1} & -\Delta_{\Sigma_{rr}}^{-1} \Sigma_{mr} \Sigma_{rr}^{-1} \\ -\Sigma_{rr}^{-1} \Sigma_{rm} \Delta_{\Sigma_{rr}}^{-1} & \Sigma_{rr}^{-1}+\Sigma_{rr}^{-1} \Sigma_{rm} \Delta_{\Sigma_{rr}}^{-1} \Sigma_{mr} \Sigma_{rr}^{-1}\\ \end{bmatrix} \end{aligned}
[ΛmmΛrmΛmrΛrr]=[ΔΣrr−1−Σrr−1ΣrmΔΣrr−1−ΔΣrr−1ΣmrΣrr−1Σrr−1+Σrr−1ΣrmΔΣrr−1ΣmrΣrr−1]
对应元素相等,发现:
Σ
r
r
−
1
=
Λ
r
r
−
Λ
r
m
Λ
m
m
−
1
Λ
m
r
\Sigma^{-1}_{rr}=\Lambda_{rr}-\Lambda_{rm}\Lambda_{mm}^{-1}\Lambda_{mr}
Σrr−1=Λrr−ΛrmΛmm−1Λmr
即为所求。
对比第一种思路,发现求逆的次数少了,且求逆的规模小了!
【应用】
设
k
k
k时刻的最小二乘优化之后,现将状态中的
X
m
\Chi_m
Xmmarg掉:
Λ
X
=
−
b
[
Λ
m
m
Λ
m
r
Λ
r
m
Λ
r
r
]
[
X
m
X
r
]
=
−
[
b
m
b
r
]
\begin{aligned} \Lambda\Chi &=-b \\ \begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ \Lambda_{rm} & \Lambda_{rr} \\ \end{bmatrix} \begin{bmatrix} \Chi_{m} \\ \Chi_{r} \end{bmatrix} &= -\begin{bmatrix} b_{m} \\ b_{r} \end{bmatrix} \end{aligned}
ΛX[ΛmmΛrmΛmrΛrr][XmXr]=−b=−[bmbr]
marg后,变量
X
m
\Chi_m
Xm的测量信息传递给了变量
X
r
\Chi_r
Xr,并构成
X
r
\Chi_r
Xr的先验信息:
[
I
0
−
Λ
r
m
Λ
m
m
−
1
I
]
[
Λ
m
m
Λ
m
r
Λ
r
m
Λ
r
r
]
[
X
m
X
r
]
=
−
[
I
0
−
Λ
r
m
Λ
m
m
−
1
I
]
[
b
m
b
r
]
[
Λ
m
m
Λ
m
r
0
Λ
r
r
−
Λ
r
m
Λ
m
m
−
1
Λ
m
r
]
[
X
m
X
r
]
=
−
[
b
m
b
r
−
Λ
r
m
Λ
m
m
−
1
b
m
]
\begin{aligned} \begin{bmatrix} I & 0 \\ -\Lambda_{rm}\Lambda_{mm}^{-1} & I\\ \end{bmatrix} \begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ \Lambda_{rm} & \Lambda_{rr} \\ \end{bmatrix} \begin{bmatrix} \Chi_{m} \\ \Chi_{r} \end{bmatrix} &= - \begin{bmatrix} I & 0 \\ -\Lambda_{rm}\Lambda_{mm}^{-1} & I\\ \end{bmatrix} \begin{bmatrix} b_{m} \\ b_{r} \end{bmatrix} \\ \begin{bmatrix} \Lambda_{mm} & \Lambda_{mr} \\ 0 & \Lambda_{rr}-\Lambda_{rm}\Lambda_{mm}^{-1}\Lambda_{mr}\\ \end{bmatrix} \begin{bmatrix} \Chi_{m} \\ \Chi_{r} \end{bmatrix} &= - \begin{bmatrix} b_{m} \\ b_{r}-\Lambda_{rm}\Lambda_{mm}^{-1} b_{m} \end{bmatrix} \end{aligned}
[I−ΛrmΛmm−10I][ΛmmΛrmΛmrΛrr][XmXr][Λmm0ΛmrΛrr−ΛrmΛmm−1Λmr][XmXr]=−[I−ΛrmΛmm−10I][bmbr]=−[bmbr−ΛrmΛmm−1bm]
只取第二行: Λ r ′ X r = − b r ′ \Lambda'_r\Chi_r=-b'_r Λr′Xr=−br′
设
k
′
k'
k′时刻,加入了新的状态
X
n
e
w
\Chi_{new}
Xnew及对应的观测,开始新一轮的最小二乘优化:
b
(
k
′
)
=
Π
T
b
r
′
(
k
)
+
Σ
(
i
,
j
∈
S
)
J
i
j
T
(
k
′
)
Σ
i
j
−
1
r
i
j
(
k
′
)
Λ
(
k
′
)
=
Π
T
Λ
r
′
(
k
)
Π
+
Σ
(
i
,
j
∈
S
)
J
i
j
T
(
k
′
)
Σ
i
j
−
1
J
i
j
(
k
′
)
\begin{aligned} b(k') &=\Pi^Tb'_r(k)+\Sigma_{(i,j\in \mathcal{S})} J^T_{ij}(k') \Sigma^{-1}_{ij} r_{ij}(k') \\ \Lambda(k') &= \Pi^T \Lambda'_r(k) \Pi+ \Sigma_{(i,j\in \mathcal{S})} J^T_{ij}(k') \Sigma^{-1}_{ij} J_{ij}(k') \end{aligned}
b(k′)Λ(k′)=ΠTbr′(k)+Σ(i,j∈S)JijT(k′)Σij−1rij(k′)=ΠTΛr′(k)Π+Σ(i,j∈S)JijT(k′)Σij−1Jij(k′)
新残差加上之前marg留下的信息,构成新的最小二乘系统。