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−1HkT(HkPk∣k−1HkT+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∣kHkTRk−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−KkHk]Pk∣k−1[I−KkHk]T+KkRkKkT(公式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−KkHk]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+HkTRk−1Hk]−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−1HkT(HkPk∣k−1HkT+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−KkHk]Pk∣k−1[I−KkHk]T+KkRkKkT(公式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−KkHk]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−KkHk]Pk∣k−1−[I−KkHk]Pk∣k−1[KkHk]T+KkRkKkT
即证明
[ 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−KkHk]Pk∣k−1[KkHk]T−KkRkKkT=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(HkPk∣k−1HkT+Rk)=Pk∣k−1HkT（式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(HkPk∣k−1HkT+Rk)KkT=Pk∣k−1HkTKkT(式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−KkHk]Kk(HkPk∣k−1HkT+Rk)KkT−KkRkKkT
我们可以发现 K k R k K k T K_kR_kK_k^T KkRkKkT项是可以约掉的所以先整理成下式子
[ 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−KkHk](KkHkPk∣k−1HkTKkT+KkRkKkT)−KkRkKkT
然后约掉 K k R k K k T K_kR_kK_k^T KkRkKkT项
( 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) (KkHkPk∣k−1HkTKkT+KkRkKkT)−KkRkKkT−KkHk(KkHkPk∣k−1HkTKkT+KkRkKkT)
( 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) (KkHkPk∣k−1HkTKkT)−KkHk(KkHkPk∣k−1HkTKkT+KkRkKkT)
提出公共项
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)] KkHk[(Pk∣k−1HkTKkT)−(KkHkPk∣k−1HkTKkT+KkRkKkT)]
再提出公共项
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 KkHk[(Pk∣k−1HkT)−(KkHkPk∣k−1HkT+KkRk)]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−1HkT(HkPk∣k−1HkT+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(HkPk∣k−1HkT+Rk)=Pk∣k−1HkT（式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−1HkT(HkPk∣k−1HkT+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−KkHk]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+HkTRk−1Hk]−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−1HkT(HkPk∣k−1HkT+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+HkTRk−1Hk]−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+HkTRk−1Hk)−1=Pk∣k−1−Pk∣k−1HkT(Rk+HkPk∣k−1HkT)−1HkPk∣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−1HkT(HkPk∣k−1HkT+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+HkTRk−1Hk]−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∣kHkTRk−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∣kPk∣k−1]Pk∣k−1HkT(HkPk∣k−1HkT+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+HkTRk−1Hk]]Pk∣k−1HkT(HkPk∣k−1HkT+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∣kHkT[I+Rk−1HkPk∣k−1HkT](HkPk∣k−1HkT+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∣kHkTRk−1[R+HkPk∣k−1HkT](HkPk∣k−1HkT+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∣kHkTRk−1[R
827
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
