神来之笔
X ⊕ = X ⊖ + K ( z − H X ⊖ ) X^{\oplus}=X^{\ominus}+K(z-HX^\ominus) X⊕=X⊖+K(z−HX⊖)
有了这个之后,其余的都是基于这个来发展的。
首先, K是基于观测矩阵 H H H和先验协方差 P ⊖ P^{\ominus} P⊖ 的
K = K ( H , P ⊖ ) K=K(H,P^{\ominus}) K=K(H,P⊖)
可以考察后验协方差 P ⊕ P^\oplus P⊕来确定 K 的数值
P
⊕
=
<
(
X
−
X
⊕
)
2
>
=
<
(
X
−
X
⊖
−
K
(
Z
−
H
X
⊖
)
)
2
>
=
C
+
K
2
H
P
⊖
H
′
−
2
K
H
P
⊖
+
K
2
σ
观
测
P^\oplus=<(X-X^\oplus)^2>\\=<(X-X^\ominus-K(Z-HX^\ominus))^2> \\=C+K^2HP^{\ominus}H'-2KHP^\ominus+K^2\sigma_{观测}
P⊕=<(X−X⊕)2>=<(X−X⊖−K(Z−HX⊖))2>=C+K2HP⊖H′−2KHP⊖+K2σ观测
从而得到
K
=
(
H
P
⊖
H
′
+
σ
观
测
)
−
1
H
P
⊖
K=(HP^{\ominus}H'+\sigma_{观测})^{-1}HP^{\ominus}
K=(HP⊖H′+σ观测)−1HP⊖
而协方差的先验
P
⊖
P^{\ominus}
P⊖可以通过状态的先验
X
⊖
X^{\ominus}
X⊖来得到:
P
⊖
=
<
(
X
−
X
⊖
)
2
>
P^{\ominus}=<(X-X^\ominus)^2>
P⊖=<(X−X⊖)2>
Geschreiben am Nov 28, 2022
上述是一种选择,
另一种参考"slam 14"里面的理解:N(x+,P+) ~ N(Cx-,Q) N(x-,P-), 鲜艳和相似,不太能理解,但是这个等式可以理解。关于其一次二次系数的对等可以得出一些关于x-,x+,P-,P+,Q,R,之间的联系:
P ⊕ − 1 = C Q − 1 C ′ + P ⊖ − 1 {P^\oplus}^{-1}=CQ^{-1}C'+{P^\ominus}^{-1} P⊕−1=CQ−1C′+P⊖−1
x ⊕ P ⊕ − 1 = C x ⊖ Q − 1 + x ⊖ − 1 P ⊖ − 1 x^{\oplus}{P^\oplus}^{-1}=Cx^{\ominus}Q^{-1}+{x^{\ominus}}^{-1}{P^\ominus}^{-1} x⊕P⊕−1=Cx⊖Q−1+x⊖−1P⊖−1
刺客的疑问是什么是已知,什么是未知?貌似很显然:根据第一个式子,P+算出来了,根据第二个式子,x+算出来了,不久完了?