|
K
k
=
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1(公式1)
K
k
=
P
k
∣
k
H
k
T
R
k
?
1
K_k=P_{k|k}H_k^TR_k^{-1}
Kk?=Pk∣k?HkT?Rk?1?(公式2)
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
[
I
?
K
k
H
k
]
T
+
K
k
R
k
K
k
T
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}[I-K_kH_k]^T+K_kR_kK_k^T
Pk∣k?=[I?Kk?Hk?]Pk∣k?1?[I?Kk?Hk?]T+Kk?Rk?KkT?(公式3)
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}
Pk∣k?=[I?Kk?Hk?]Pk∣k?1?(公式4)
P
k
∣
k
=
[
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
]
?
1
P_{k|k} = [P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k]^{-1}
Pk∣k?=[Pk∣k?1?1?+HkT?Rk?1?Hk?]?1(公式5)
由13推4
K
k
=
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1(公式1)
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
[
I
?
K
k
H
k
]
T
+
K
k
R
k
K
k
T
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}[I-K_kH_k]^T+K_kR_kK_k^T
Pk∣k?=[I?Kk?Hk?]Pk∣k?1?[I?Kk?Hk?]T+Kk?Rk?KkT?(公式3)
推出：
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}
Pk∣k?=[I?Kk?Hk?]Pk∣k?1?(公式4)
由公式3展开
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
?
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
[
K
k
H
k
]
T
+
K
k
R
k
K
k
T
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}-[I-K_{k}H_{k}]P_{k|k-1}[K_kH_k]^T+K_kR_kK_k^T
Pk∣k?=[I?Kk?Hk?]Pk∣k?1??[I?Kk?Hk?]Pk∣k?1?[Kk?Hk?]T+Kk?Rk?KkT?
即证明
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
[
K
k
H
k
]
T
?
K
k
R
k
K
k
T
=
0
[I-K_{k}H_{k}]P_{k|k-1}[K_kH_k]^T-K_kR_kK_k^T = 0
[I?Kk?Hk?]Pk∣k?1?[Kk?Hk?]T?Kk?Rk?KkT?=0（式1.2）
由公式1可知
K
k
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
=
P
k
∣
k
?
1
H
k
T
K_k(H_kP_{k|k-1}H_k^T+R_k)=P_{k|k-1}H_k^T
Kk?(Hk?Pk∣k?1?HkT?+Rk?)=Pk∣k?1?HkT?（式1.3）
（式1.3）左右两端同时乘以
K
k
T
K_k^T
KkT?
K
k
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
K
k
T
=
P
k
∣
k
?
1
H
k
T
K
k
T
K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T=P_{k|k-1}H_k^TK_k^T
Kk?(Hk?Pk∣k?1?HkT?+Rk?)KkT?=Pk∣k?1?HkT?KkT?(式1.4)
把（1.4）式带入（1.2）中
[
I
?
K
k
H
k
]
K
k
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
K
k
T
?
K
k
R
k
K
k
T
[I-K_{k}H_{k}]K_k(H_kP_{k|k-1}H_k^T+R_k)K_k^T-K_kR_kK_k^T
[I?Kk?Hk?]Kk?(Hk?Pk∣k?1?HkT?+Rk?)KkT??Kk?Rk?KkT?
我们可以发现
K
k
R
k
K
k
T
K_kR_kK_k^T
Kk?Rk?KkT?项是可以约掉的所以先整理成下式子
[
I
?
K
k
H
k
]
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
+
K
k
R
k
K
k
T
)
?
K
k
R
k
K
k
T
[I-K_{k}H_{k}](K_kH_kP_{k|k-1}H_k^TK_k^T+K_kR_kK_k^T)-K_kR_kK_k^T
[I?Kk?Hk?](Kk?Hk?Pk∣k?1?HkT?KkT?+Kk?Rk?KkT?)?Kk?Rk?KkT?
然后约掉
K
k
R
k
K
k
T
K_kR_kK_k^T
Kk?Rk?KkT?项
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
+
K
k
R
k
K
k
T
)
?
K
k
R
k
K
k
T
?
K
k
H
k
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
+
K
k
R
k
K
k
T
)
(K_kH_kP_{k|k-1}H_k^TK_k^T+K_kR_kK_k^T)-K_kR_kK_k^T-K_{k}H_{k}(K_kH_kP_{k|k-1}H_k^TK_k^T+K_kR_kK_k^T)
(Kk?Hk?Pk∣k?1?HkT?KkT?+Kk?Rk?KkT?)?Kk?Rk?KkT??Kk?Hk?(Kk?Hk?Pk∣k?1?HkT?KkT?+Kk?Rk?KkT?)
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
)
?
K
k
H
k
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
+
K
k
R
k
K
k
T
)
(K_kH_kP_{k|k-1}H_k^TK_k^T)-K_{k}H_{k}(K_kH_kP_{k|k-1}H_k^TK_k^T+K_kR_kK_k^T)
(Kk?Hk?Pk∣k?1?HkT?KkT?)?Kk?Hk?(Kk?Hk?Pk∣k?1?HkT?KkT?+Kk?Rk?KkT?)
提出公共项
K
k
H
k
[
(
P
k
∣
k
?
1
H
k
T
K
k
T
)
?
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
K
k
T
+
K
k
R
k
K
k
T
)
]
K_kH_k[(P_{k|k-1}H_k^TK_k^T)-(K_kH_kP_{k|k-1}H_k^TK_k^T+K_kR_kK_k^T)]
Kk?Hk?[(Pk∣k?1?HkT?KkT?)?(Kk?Hk?Pk∣k?1?HkT?KkT?+Kk?Rk?KkT?)]
再提出公共项
K
k
H
k
[
(
P
k
∣
k
?
1
H
k
T
)
?
(
K
k
H
k
P
k
∣
k
?
1
H
k
T
+
K
k
R
k
)
]
K
k
T
K_kH_k[(P_{k|k-1}H_k^T)-(K_kH_kP_{k|k-1}H_k^T+K_kR_k)]K_k^T
Kk?Hk?[(Pk∣k?1?HkT?)?(Kk?Hk?Pk∣k?1?HkT?+Kk?Rk?)]KkT?（公式1.5）
再来回顾下
K
k
=
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1(公式1)的变形
K
k
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
=
P
k
∣
k
?
1
H
k
T
K_k(H_kP_{k|k-1}H_k^T+R_k)=P_{k|k-1}H_k^T
Kk?(Hk?Pk∣k?1?HkT?+Rk?)=Pk∣k?1?HkT?（式1.3）
将公式1.3带入到公式1.5中
可以发现公式1.5完美的为0，也就证明式子1.2是成立的，也就是说可以由1 3 推出公式4
由1 4推5
K
k
=
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1(公式1)
P
k
∣
k
=
[
I
?
K
k
H
k
]
P
k
∣
k
?
1
P_{k|k} = [I-K_{k}H_{k}]P_{k|k-1}
Pk∣k?=[I?Kk?Hk?]Pk∣k?1?(公式4)
推出
P
k
∣
k
=
[
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
]
?
1
P_{k|k} = [P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k]^{-1}
Pk∣k?=[Pk∣k?1?1?+HkT?Rk?1?Hk?]?1(公式5)
由把公式1直接带入公式4中可得
P
k
∣
k
=
[
I
?
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
H
k
]
P
k
∣
k
?
1
P_{k|k} = [I-P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}H_{k}]P_{k|k-1}
Pk∣k?=[I?Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1Hk?]Pk∣k?1?(公式2.1)
引入矩阵逆引理并把公式5用矩阵逆引理打开
P
k
∣
k
=
[
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
]
?
1
P_{k|k} = [P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k]^{-1}
Pk∣k?=[Pk∣k?1?1?+HkT?Rk?1?Hk?]?1(公式5)
(
A
+
B
C
D
)
?
1
=
A
?
1
?
A
?
1
B
(
C
?
1
+
D
A
?
1
B
)
?
1
D
A
?
1
(A+BCD)^{-1} = A^{-1}-A^{-1}B(C^{-1}+DA^{-1}B)^{-1}DA^{-1}
(A+BCD)?1=A?1?A?1B(C?1+DA?1B)?1DA?1
(
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
)
?
1
=
P
k
∣
k
?
1
?
P
k
∣
k
?
1
H
k
T
(
R
k
+
H
k
P
k
∣
k
?
1
H
k
T
)
?
1
H
k
P
k
∣
k
?
1
(P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k)^{-1} = P_{k|k-1}-P_{k|k-1}H_k^T(R_k+H_kP_{k|k-1}H_k^T)^{-1}H_kP_{k|k-1}
(Pk∣k?1?1?+HkT?Rk?1?Hk?)?1=Pk∣k?1??Pk∣k?1?HkT?(Rk?+Hk?Pk∣k?1?HkT?)?1Hk?Pk∣k?1?（式2.2）
则证明公式1和公式4推出公式5可转化为公式1和公式4推出式2.2的问题
且公式2.1（把公式1带入公式4中得到的）等于式2.2所以综上1、4可以推出5
证毕
由1 5推2
K
k
=
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1(公式1)
P
k
∣
k
=
[
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
]
?
1
P_{k|k} = [P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k]^{-1}
Pk∣k?=[Pk∣k?1?1?+HkT?Rk?1?Hk?]?1(公式5)
推出
K
k
=
P
k
∣
k
H
k
T
R
k
?
1
K_k=P_{k|k}H_k^TR_k^{-1}
Kk?=Pk∣k?HkT?Rk?1?(公式2)
证明：
K
k
=
[
P
k
∣
k
P
k
∣
k
?
1
]
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=[P_{k|k}P_{k|k}^{-1}]P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=[Pk∣k?Pk∣k?1?]Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1（公式3.3）
带入公式5
K
k
=
[
P
k
∣
k
[
P
k
∣
k
?
1
?
1
+
H
k
T
R
k
?
1
H
k
]
]
P
k
∣
k
?
1
H
k
T
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=[P_{k|k} [P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k]]P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=[Pk∣k?[Pk∣k?1?1?+HkT?Rk?1?Hk?]]Pk∣k?1?HkT?(Hk?Pk∣k?1?HkT?+Rk?)?1
化简得到
K
k
=
P
k
∣
k
H
k
T
[
I
+
R
k
?
1
H
k
P
k
∣
k
?
1
H
k
T
]
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k}H_k^T [I+R_k^{-1}H_kP_{k|k-1}H_k^T](H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?HkT?[I+Rk?1?Hk?Pk∣k?1?HkT?](Hk?Pk∣k?1?HkT?+Rk?)?1
提出一个
R
k
?
1
R_k^{-1}
Rk?1?
K
k
=
P
k
∣
k
H
k
T
R
k
?
1
[
R
+
H
k
P
k
∣
k
?
1
H
k
T
]
(
H
k
P
k
∣
k
?
1
H
k
T
+
R
k
)
?
1
K_k=P_{k|k}H_k^T R_k^{-1}[R+H_kP_{k|k-1}H_k^T](H_kP_{k|k-1}H_k^T+R_k)^{-1}
Kk?=Pk∣k?HkT?Rk?1?[R+Hk?Pk∣k?1?HkT?](Hk?Pk∣k?1?HkT?+Rk?)?1
所以成立
K
k
=
P
k
∣
k
H
k
T
R
k
?
1
[
R
K_k=P_{k|k}H_k^T R_k^{-1}[R
Kk?=Pk∣k?HkT?Rk?1?[R
|