025卡尔曼滤波中滤波增益与协方差阵的等价形式

  首先摆放一下前面的求解结果:
(1) K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 \tag{1} K_k = P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1} Kk=Pk/k1HkT(HkPk/k1HkT+Rk)1(1) (2) P k = ( I − K k H k ) P k / k − 1 \tag{2} P_k = (I - K_k H_k ) P_{k/k-1} Pk=(IKkHk)Pk/k1(2) P 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 = (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\ Pk=(IKkHk)Pk/k1(IKkHk)T+KkRkKkT


  公式(1)代入公式(2)展开的:
(3) P k = P k / k − 1 − 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 − 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 \tag{3} \begin{aligned} P_k &= P_{k/k-1} - 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-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} \\ \end{aligned} Pk=Pk/k1Pk/k1HkT(HkPk/k1HkT+Rk)1HkPk/k1=Pk/k1Pk/k1HkT(Rk+HkPk/k1HkT)1HkPk/k1(3)

  比较矩阵求逆引理:
( 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=A1A1B(C1+DA1B)1DA1
  可将公式(3)转换为:

(4) P k = ( P k / k − 1 − 1 + H k T R k − 1 H k ) − 1 \tag{4} P_k = (P_{k/k-1}^{-1} + H_k^T R_k^{-1} H_k) ^{-1} Pk=(Pk/k11+HkTRk1Hk)1(4)


  前面已求得:
P k / k − 1 H k T = K k ( H k P k / k − 1 H k T + R k ) P_{k/k-1} H_k^T = K_k(H_k P_{k/k-1} H_k^T + R_k) Pk/k1HkT=Kk(HkPk/k1HkT+Rk)
  两边转置:
H k P k / k − 1 = ( H k P k / k − 1 H k T + R k ) K k T H_k P_{k/k-1} = (H_k P_{k/k-1} H_k^T + R_k)K^T_k HkPk/k1=(HkPk/k1HkT+Rk)KkT
  将公式(2)并将上式代入可得:

P k = P k / k − 1 − K k ( H k P k / k − 1 H k T + R k ) K k T P_k = P_{k/k-1} - K_k (H_k P_{k/k-1} H_k^T + R_k)K^T_k Pk=Pk/k1Kk(HkPk/k1HkT+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 = [ ( H k P k / k − 1 H k T + R k ) ( H k T ) − 1 P k / k − 1 − 1 ] − 1 = [ H k + R k ( H k T ) − 1 P k / k − 1 − 1 ] − 1 = [ R k ( H k T ) − 1 ( H k T R k − 1 H k + P k / k − 1 − 1 ) ] − 1 = ( H k T R k − 1 H k + P k / k − 1 − 1 ) − 1 H k T R k − 1 \tag{5} \begin{aligned} K_k &= 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} H_k^T + R_k)(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [H_k + R_k(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [R_k(H_k^T)^{-1}( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})]^{-1}\\ \\ &= ( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})^{-1}H_k^T R_k^{-1}\\ \end{aligned} Kk=Pk/k1HkT(HkPk/k1HkT+Rk)1=[(HkPk/k1HkT+Rk)(HkT)1Pk/k11]1=[Hk+Rk(HkT)1Pk/k11]1=[Rk(HkT)1(HkTRk1Hk+Pk/k11)]1=(HkTRk1Hk+Pk/k11)1HkTRk1(5)

  将公式(4)代入公式(5):

K k = P k H k T R k − 1 K_k = P_k H_k^T R_k^{-1} Kk=PkHkTRk1


  总结一下等价形式,对于滤波增益:

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 H k T R k − 1 \begin{aligned} 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 H_k^T R_k^{-1} \end{aligned} KkKk=Pk/k1HkT(HkPk/k1HkT+Rk)1=PkHkTRk1

  对于协方差阵:

P k = ( I − K k H k ) P k / k − 1 P k = P k / k − 1 − K k ( H k P k / k − 1 H k T + R k ) K k T P 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 = ( P k / k − 1 − 1 + H k T R k − 1 H k ) − 1 \begin{aligned} P_k &= (I - K_k H_k ) P_{k/k-1} \\ \\ P_k &= P_{k/k-1} - K_k (H_k P_{k/k-1} H_k^T + R_k)K^T_k\\ \\ P_k &= (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\ \\ P_k &= (P_{k/k-1}^{-1} + H_k^T R_k^{-1} H_k) ^{-1}\\ \end{aligned} PkPkPkPk=(IKkHk)Pk/k1=Pk/k1Kk(HkPk/k1HkT+Rk)KkT=(IKkHk)Pk/k1(IKkHk)T+KkRkKkT=(Pk/k11+HkTRk1Hk)1

  • 5
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值