首先摆放一下前面的求解结果：
(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/k−1HkT(HkPk/k−1HkT+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=(I−KkHk)Pk/k−1(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=(I−KkHk)Pk/k−1(I−KkHk)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/k−1−Pk/k−1HkT(HkPk/k−1HkT+Rk)−1HkPk/k−1=Pk/k−1−Pk/k−1HkT(Rk+HkPk/k−1HkT)−1HkPk/k−1(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=A−1−A−1B(C−1+DA−1B)−1DA−1
可将公式(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/k−1−1+HkTRk−1Hk)−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/k−1HkT=Kk(HkPk/k−1HkT+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/k−1=(HkPk/k−1HkT+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/k−1−Kk(HkPk/k−1HkT+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/k−1HkT(HkPk/k−1HkT+Rk)−1=[(HkPk/k−1HkT+Rk)(HkT)−1Pk/k−1−1]−1=[Hk+Rk(HkT)−1Pk/k−1−1]−1=[Rk(HkT)−1(HkTRk−1Hk+Pk/k−1−1)]−1=(HkTRk−1Hk+Pk/k−1−1)−1HkTRk−1(5)
将公式(4)代入公式(5)：
K k = P k H k T R k − 1 K_k = P_k H_k^T R_k^{-1} Kk=PkHkTRk−1
总结一下等价形式，对于滤波增益：
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/k−1HkT(HkPk/k−1HkT+Rk)−1=PkHkTRk−1
对于协方差阵：
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=(I−KkHk)Pk/k−1=Pk/k−1−Kk(HkPk/k−1HkT+Rk)KkT=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT=(Pk/k−1−1+HkTRk−1Hk)−1
扫一扫
1646
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
