【卡尔曼滤波算法剖析】1.6 检测误差与迹
文章目录
卡尔曼滤波算法
卡尔曼滤波是一种基于“预测–更新”递归框架的最优线性估计器：以状态转移模型先验递推系统状态及其协方差，再以观测数据通过最小化估计协方差进行后验校正，从而在过程与测量噪声并存的环境下给出最小均方误差意义下的最优估计。
1.6 检测误差与迹
由于4.1.5的结论可知，寻找合适的
K
k
K_k
Kk，使得最终结果的误差最小，即估计值
x
^
k
\hat{x}_k
x^k趋近于实际值
x
k
x_k
xk。而
K
k
K_k
Kk与检测误差相关。
定义检测误差
e
k
e_k
ek，如式
(
37
)
(37)
(37)所示：
e
k
=
x
k
−
x
^
k
(37)
e_k = x_k - \hat{x}_k \tag{37}
ek=xk−x^k(37)
e
k
e_k
ek：检测误差。概率分布为
p
(
e
k
)
∼
(
0
,
P
)
p(e_k) \sim (0, P)
p(ek)∼(0,P)，其中，
0
0
0为期望，
P
P
P为
e
k
e_k
ek的协方差矩阵，如式
(
38
)
(38)
(38)所示：
P
=
E
[
e
⋅
e
T
]
(38)
P = E\left[ e \cdot e^T \right] \tag{38}
P=E[e⋅eT](38)
如果有两项，则如式
(
39
)
(39)
(39)所示：
P
=
[
σ
e
1
2
σ
e
1
σ
e
2
σ
e
2
σ
e
1
σ
e
2
2
]
(39)
P = \begin{bmatrix} \sigma_{e_1}^2 & \sigma_{e_1}\sigma_{e_2} \\ \sigma_{e_2}\sigma_{e_1} & \sigma_{e_2}^2 \end{bmatrix} \tag{39}
P=[σe12σe2σe1σe1σe2σe22](39)
说明：如果估计值与实际误差最小，即误差的方差最小，即越接近于期望值
0
0
0。
问题转化为
P
P
P的迹
(
t
r
(
P
)
)
(tr(P))
(tr(P)) 最小，即协方差矩阵的对角线相加后的值最小，如式
(
40
)
(40)
(40)所示：
t
r
(
P
)
=
σ
e
1
2
+
σ
e
2
2
(40)
tr(P) = \sigma_{e_1}^2 + \sigma_{e_2}^2 \tag{40}
tr(P)=σe12+σe22(40)
由式
(
38
)
(38)
(38)，得式(41)所示：
P
=
E
[
e
⋅
e
T
]
P
=
E
[
(
x
k
−
x
^
k
)
⋅
(
x
k
−
x
^
k
)
T
]
(41)
\begin{aligned} P &= E\left[ e \cdot e^T \right] \\ P &= E\left[ \left( x_k - \hat{x}_k \right) \cdot \left( x_k - \hat{x}_k \right)^T \right] \end{aligned} \tag{41}
PP=E[e⋅eT]=E[(xk−x^k)⋅(xk−x^k)T](41)
由式
(
34
)
(34)
(34)：代入协方差矩阵计算得式
(
42
)
(42)
(42)所示：
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
⟹
x
k
−
x
^
k
=
x
k
−
[
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
]
x
k
−
x
^
k
=
x
k
−
x
^
k
−
−
K
k
z
k
+
K
k
H
x
^
k
−
(42)
\begin{aligned} \hat{x}_k &= \hat{x}_k^- + K_k \left( z_k - H \hat{x}_k^- \right) \implies \\ x_k - \hat{x}_k &= x_k - \left[ \hat{x}_k^- + K_k \left( z_k - H \hat{x}_k^- \right) \right] \\ x_k - \hat{x}_k &= x_k - \hat{x}_k^- - K_k z_k + K_k H \hat{x}_k^- \end{aligned} \tag{42}
x^kxk−x^kxk−x^k=x^k−+Kk(zk−Hx^k−)⟹=xk−[x^k−+Kk(zk−Hx^k−)]=xk−x^k−−Kkzk+KkHx^k−(42)
由4.1.3小节可知，状态空间方程如式
(
43
)
(43)
(43)所示：
{
X
˙
k
=
A
X
k
−
1
+
B
U
k
+
w
k
−
1
Z
k
=
H
X
k
+
v
k
(43)
\begin{cases} \dot{X}_k = AX_{k - 1} + BU_k + w_{k - 1} \\ Z_k = HX_k + v_k \end{cases} \tag{43}
{X˙k=AXk−1+BUk+wk−1Zk=HXk+vk(43)
所以，得式(44)所示：
x
k
−
x
^
k
=
x
k
−
x
^
k
−
−
K
k
z
k
+
K
k
H
x
^
k
−
=
(
I
−
K
k
H
)
(
x
k
−
x
^
k
−
)
−
K
k
v
k
(44)
\begin{aligned} x_k - \hat{x}_k &= x_k - \hat{x}_k^- - K_k z_k + K_k H \hat{x}_k^- \\ &= \left( I - K_k H \right) \left( x_k - \hat{x}_k^- \right) - K_k v_k \end{aligned} \tag{44}
xk−x^k=xk−x^k−−Kkzk+KkHx^k−=(I−KkH)(xk−x^k−)−Kkvk(44)
由式
(
37
)
(37)
(37)可知，定义先验误差
e
k
−
e_k^-
ek−，如式(45)所示：
e
k
−
=
(
x
k
−
x
^
k
−
)
(45)
e_k^- = \left( x_k - \hat{x}_k^- \right) \tag{45}
ek−=(xk−x^k−)(45)
由式
(
39
)
(39)
(39)、
(
42
)
(42)
(42)、
(
43
)
(43)
(43)得，如式
(
46
)
(46)
(46)所示：
P
=
E
[
e
⋅
e
T
]
P
=
E
[
(
x
k
−
x
^
k
)
⋅
(
x
k
−
x
^
k
)
T
]
P
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
⋅
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
T
]
(46)
\begin{aligned} P &= E\left[ e \cdot e^T \right] \\ P &= E\left[ \left( x_k - \hat{x}_k \right) \cdot \left( x_k - \hat{x}_k \right)^T \right] \\ P &= E\left[ \left[ \left( I - K_k H \right) e_k^- - K_k v_k \right] \cdot \left[ \left( I - K_k H \right) e_k^- - K_k v_k \right]^T \right] \end{aligned} \tag{46}
PPP=E[e⋅eT]=E[(xk−x^k)⋅(xk−x^k)T]=E[[(I−KkH)ek−−Kkvk]⋅[(I−KkH)ek−−Kkvk]T](46)
矩阵计算的一般公式，为式
(
47
)
(47)
(47)所示：
(
A
B
)
T
=
B
T
A
T
(
A
+
B
)
T
=
A
T
+
B
T
(47)
\begin{aligned} (AB)^T &= B^T A^T \tag{47} \\ (A + B)^T &= A^T + B^T \end{aligned}
(AB)T(A+B)T=BTAT=AT+BT(47)
所以计算，如式
(
48
)
(48)
(48)所示：
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
T
=
[
(
I
−
K
k
H
)
e
k
−
]
T
−
(
K
k
v
k
)
T
=
e
k
−
T
(
I
−
K
k
H
)
T
−
v
k
T
K
k
T
(48)
\begin{aligned} \left[ \left( I - K_k H \right) e_k^- - K_k v_k \right]^T &= \left[ \left( I - K_k H \right) e_k^- \right]^T - \left( K_k v_k \right)^T \\ &= e_k^{-T} \left( I - K_k H \right)^T - v_k^T K_k^T \end{aligned} \tag{48}
[(I−KkH)ek−−Kkvk]T=[(I−KkH)ek−]T−(Kkvk)T=ek−T(I−KkH)T−vkTKkT(48)
因此，最终如式
(
49
)
(49)
(49)所示：
P
k
=
E
[
e
⋅
e
T
]
P
k
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
(49)
\begin{aligned} P_k &= E\left[ e \cdot e^T \right] \\ P_k &= E\left[ \left( I - K_k H \right) e_k^- e_k^{-T} \left( I - K_k H \right)^T \right] - E\left[ \left( I - K_k H \right) e_k^- v_k^T K_k^T \right] \\ &\quad - E\left[ K_k v_k e_k^{-T} \left( I - K_k H \right)^T \right] + E\left[ K_k v_k v_k^T K_k^T \right] \end{aligned} \tag{49}
PkPk=E[e⋅eT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvkek−T(I−KkH)T]+E[KkvkvkTKkT](49)
因为
E
(
A
B
)
=
E
(
A
)
E
(
B
)
E(AB) = E(A)E(B)
E(AB)=E(A)E(B)（若A、B事件独立）
所以表达，如式
(
50
)
(50)
(50)所示：
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
E
(
e
k
−
v
k
T
)
K
k
T
=
(
I
−
K
k
H
)
E
(
e
k
−
)
E
(
v
k
T
)
K
k
T
(50)
\begin{aligned} E\left[ \left( I - K_k H \right) e_k^- v_k^T K_k^T \right] &= \left( I - K_k H \right) E\left( e_k^- v_k^T \right) K_k^T \\ &= \left( I - K_k H \right) E\left( e_k^- \right) E\left( v_k^T \right) K_k^T \end{aligned} \tag{50}
E[(I−KkH)ek−vkTKkT]=(I−KkH)E(ek−vkT)KkT=(I−KkH)E(ek−)E(vkT)KkT(50)
因为
E
(
e
k
−
)
=
0
,
E
(
v
k
T
)
=
0
E(e_k^-)=0,E(v_k^T)=0
E(ek−)=0,E(vkT)=0。
所以，得式
(
51
)
(51)
(51)所示：
P
k
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
P
k
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
P
k
=
(
I
−
K
k
H
)
E
[
e
k
−
e
k
−
T
]
(
I
−
K
k
H
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
(51)
\begin{aligned} P_k &= E\left[ \left( I - K_k H \right) e_k^- e_k^{-T} \left( I - K_k H \right)^T \right] - E\left[ \left( I - K_k H \right) e_k^- v_k^T K_k^T \right] \\ &\quad - E\left[ K_k v_k e_k^{-T} \left( I - K_k H \right)^T \right] + E\left[ K_k v_k v_k^T K_k^T \right] \\ P_k &= E\left[ \left( I - K_k H \right) e_k^- e_k^{-T} \left( I - K_k H \right)^T \right] + E\left[ K_k v_k v_k^T K_k^T \right] \\ P_k &= \left( I - K_k H \right) E\left[ e_k^- e_k^{-T} \right] \left( I - K_k H \right)^T + K_k E\left[ v_k v_k^T \right] K_k^T \end{aligned} \tag{51}
PkPkPk=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvkek−T(I−KkH)T]+E[KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]+E[KkvkvkTKkT]=(I−KkH)E[ek−ek−T](I−KkH)T+KkE[vkvkT]KkT(51)
定义
P
k
−
P_k^-
Pk−为先验误差的协方差，如式
(
52
)
(52)
(52)所示：
P
k
−
=
E
(
e
k
−
e
k
−
T
)
(52)
P_k^- = E\left( e_k^- e_k^{-T} \right) \tag{52}
Pk−=E(ek−ek−T)(52)
因为
E
[
v
k
v
k
T
]
=
R
E\left[ v_k v_k^T \right] = R
E[vkvkT]=R。所以误差的协方差矩阵
P
k
P_k
Pk如式
(
53
)
(53)
(53)所示：
P
k
=
(
I
−
K
k
H
)
E
[
e
k
−
e
k
−
T
]
(
I
−
K
k
H
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
(53)
\begin{aligned} P_k &= \left( I - K_k H \right) E\left[ e_k^- e_k^{-T} \right] \left( I - K_k H \right)^T + K_k E\left[ v_k v_k^T \right] K_k^T \\ P_k &= P_k^- - K_k H P_k^- - P_k^- H^T K_k^T + K_k H P_k^- H^T K_k^T + K_k R K_k^T \end{aligned} \tag{53}
PkPk=(I−KkH)E[ek−ek−T](I−KkH)T+KkE[vkvkT]KkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT(53)
式
(
53
)
(53)
(53)中，有如下关系，如式
(
54
)
(54)
(54)所示：
(
P
k
−
H
T
K
k
T
)
T
=
P
k
−
T
(
H
T
K
k
T
)
T
=
P
k
−
T
(
K
k
T
)
T
(
H
T
)
T
=
P
k
−
T
K
k
H
=
K
k
H
P
k
−
T
(54)
\begin{aligned} \left( P_k^- H^T K_k^T \right)^T &= P_k^{-T} \left( H^T K_k^T \right)^T \\ &= P_k^{-T} \left( K_k^T \right)^T \left( H^T \right)^T = P_k^{-T} K_k H \\ &= K_k H P_k^{-T} \end{aligned} \tag{54}
(Pk−HTKkT)T=Pk−T(HTKkT)T=Pk−T(KkT)T(HT)T=Pk−TKkH=KkHPk−T(54)
回归到问题：
P
k
P_k
Pk的迹
(
t
r
(
P
)
)
(tr(P))
(tr(P))最小，即误差的协方差矩阵的对角线相加后的值最小，如式
(
55
)
(55)
(55)所示：
t
r
(
P
)
=
σ
e
1
2
+
σ
e
2
2
(55)
tr(P) = \sigma_{e_1}^2 + \sigma_{e_2}^2 \tag{55}
tr(P)=σe12+σe22(55)
下面求
t
r
(
P
)
tr(P)
tr(P)，如式(56)所示：
t
r
(
P
k
)
=
t
r
(
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
)
=
t
r
(
P
k
−
)
+
t
r
(
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
)
+
t
r
(
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
)
∵
t
r
(
X
)
=
t
r
(
X
T
)
,
K
k
H
P
k
−
=
(
P
k
−
H
T
K
k
T
)
T
∴
t
r
(
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
)
=
−
2
t
r
(
K
k
H
P
k
−
)
∴
t
r
(
P
k
)
=
t
r
(
P
k
−
)
−
2
t
r
(
K
k
H
P
k
−
)
+
t
r
(
K
k
H
P
k
−
H
T
K
k
T
)
+
t
r
(
K
k
R
K
k
T
)
(56)
\begin{aligned} tr(P_k) &= tr\left( P_k^- - K_k H P_k^- - P_k^- H^T K_k^T + K_k H P_k^- H^T K_k^T + K_k R K_k^T \right) \\ &= tr\left( P_k^- \right) + tr\left( -K_k H P_k^- - P_k^- H^T K_k^T \right) + tr\left( K_k H P_k^- H^T K_k^T + K_k R K_k^T \right) \\ \because tr(X) &= tr\left( X^T \right), K_k H P_k^- = \left( P_k^- H^T K_k^T \right)^T \\ \therefore tr\left( -K_k H P_k^- - P_k^- H^T K_k^T \right) &= -2tr\left( K_k H P_k^- \right) \\ \therefore tr(P_k) &= tr\left( P_k^- \right) - 2tr\left( K_k H P_k^- \right) + tr\left( K_k H P_k^- H^T K_k^T \right) + tr\left( K_k R K_k^T \right) \end{aligned} \tag{56}
tr(Pk)∵tr(X)∴tr(−KkHPk−−Pk−HTKkT)∴tr(Pk)=tr(Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT)=tr(Pk−)+tr(−KkHPk−−Pk−HTKkT)+tr(KkHPk−HTKkT+KkRKkT)=tr(XT),KkHPk−=(Pk−HTKkT)T=−2tr(KkHPk−)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)(56)
P
k
P_k
Pk的迹
(
t
r
(
P
)
)
(tr(P))
(tr(P))最小，因此对
P
k
P_k
Pk求导：
基本公式：①
d
[
t
r
(
A
B
)
]
d
A
=
B
T
(57)
\frac{d\left[ tr(AB) \right]}{dA} = B^T \tag{57}
dAd[tr(AB)]=BT(57)
证明上式：①
A
=
[
a
11
a
12
a
21
a
22
]
,
B
=
[
b
11
b
12
b
21
b
22
]
A
B
=
[
a
11
b
11
+
a
12
b
21
a
21
b
12
+
a
22
b
22
]
t
r
(
A
B
)
=
a
11
b
11
+
a
12
b
21
+
a
21
b
12
+
a
22
b
22
d
[
t
r
(
A
B
)
]
d
A
=
[
∂
[
t
r
(
A
B
)
]
∂
a
11
∂
[
t
r
(
A
B
)
]
∂
a
12
∂
[
t
r
(
A
B
)
]
∂
a
21
∂
[
t
r
(
A
B
)
]
∂
a
22
]
=
[
b
11
b
21
b
12
b
22
]
=
B
T
(58)
\begin{aligned} A &= \begin{bmatrix} a_{11} & a_{12} \\ a_{21} & a_{22} \end{bmatrix}, \quad B = \begin{bmatrix} b_{11} & b_{12} \\ b_{21} & b_{22} \end{bmatrix} \\ AB &= \begin{bmatrix} a_{11}b_{11} + a_{12}b_{21} & \\ & a_{21}b_{12} + a_{22}b_{22} \end{bmatrix} \\ tr(AB) &= a_{11}b_{11} + a_{12}b_{21} + a_{21}b_{12} + a_{22}b_{22} \\ \frac{d\left[ tr(AB) \right]}{dA} &= \begin{bmatrix} \frac{\partial \left[ tr(AB) \right]}{\partial a_{11}} & \frac{\partial \left[ tr(AB) \right]}{\partial a_{12}} \\ \frac{\partial \left[ tr(AB) \right]}{\partial a_{21}} & \frac{\partial \left[ tr(AB) \right]}{\partial a_{22}} \end{bmatrix} = \begin{bmatrix} b_{11} & b_{21} \\ b_{12} & b_{22} \end{bmatrix} = B^T \end{aligned} \tag{58}
AABtr(AB)dAd[tr(AB)]=[a11a21a12a22],B=[b11b21b12b22]=[a11b11+a12b21a21b12+a22b22]=a11b11+a12b21+a21b12+a22b22=[∂a11∂[tr(AB)]∂a21∂[tr(AB)]∂a12∂[tr(AB)]∂a22∂[tr(AB)]]=[b11b12b21b22]=BT(58)
基本公式：②
d
[
t
r
(
A
B
A
T
)
]
d
A
=
2
A
B
(59)
\frac{d\left[ tr(ABA^T) \right]}{dA} = 2AB \tag{59}
dAd[tr(ABAT)]=2AB(59)
也可按照①中方法证明，得：
d
(
t
r
(
P
k
)
)
d
K
k
=
0
(60)
\frac{d\left( tr(P_k) \right)}{dK_k} = 0 \tag{60}
dKkd(tr(Pk))=0(60)
因此得，式(61)所示：
d
[
t
r
(
P
k
)
]
d
K
k
=
d
[
t
r
(
P
k
−
)
−
2
t
r
(
K
k
H
P
k
−
)
+
t
r
(
K
k
H
P
k
−
H
T
K
k
T
)
+
t
r
(
K
k
R
K
k
T
)
]
d
K
k
d
[
t
r
(
P
k
)
]
d
K
k
=
0
−
2
(
H
P
k
−
)
T
+
2
(
K
k
H
P
k
−
H
T
)
+
2
K
k
R
0
−
2
(
H
P
k
−
)
T
+
2
(
K
k
H
P
k
−
H
T
)
+
2
K
k
R
=
0
⟹
−
(
H
P
k
−
)
T
+
(
K
k
H
P
k
−
H
T
)
+
K
k
R
=
0
−
P
k
−
T
H
T
+
K
k
(
H
P
k
−
H
T
+
R
)
=
0
K
k
(
H
P
k
−
H
T
+
R
)
=
P
k
−
T
H
T
∵
(
P
k
−
T
=
P
k
−
)
K
k
=
P
k
−
T
H
T
(
H
P
k
−
H
T
+
R
)
=
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
(61)
\begin{aligned} \frac{d\left[ tr(P_k) \right]}{dK_k} &= \frac{d\left[ tr\left( P_k^- \right) - 2tr\left( K_k H P_k^- \right) + tr\left( K_k H P_k^- H^T K_k^T \right) + tr\left( K_k R K_k^T \right) \right]}{dK_k} \\ \frac{d\left[ tr(P_k) \right]}{dK_k} &= 0 - 2\left( H P_k^- \right)^T + 2\left( K_k H P_k^- H^T \right) + 2 K_k R \\ 0 - 2\left( H P_k^- \right)^T + 2\left( K_k H P_k^- H^T \right) + 2 K_k R &= 0 \implies \\ -\left( H P_k^- \right)^T + \left( K_k H P_k^- H^T \right) + K_k R &= 0 \\ -P_k^{-T} H^T + K_k \left( H P_k^- H^T + R \right) &= 0 \\ K_k \left( H P_k^- H^T + R \right) &= P_k^{-T} H^T \\ \because (P_k^{-T} = P_k^-) \\ K_k &= \frac{P_k^{-T} H^T}{\left( H P_k^- H^T + R \right)} = \frac{P_k^- H^T}{\left( H P_k^- H^T + R \right)} \end{aligned} \tag{61}
dKkd[tr(Pk)]dKkd[tr(Pk)]0−2(HPk−)T+2(KkHPk−HT)+2KkR−(HPk−)T+(KkHPk−HT)+KkR−Pk−THT+Kk(HPk−HT+R)Kk(HPk−HT+R)∵(Pk−T=Pk−)Kk=dKkd[tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)]=0−2(HPk−)T+2(KkHPk−HT)+2KkR=0⟹=0=0=Pk−THT=(HPk−HT+R)Pk−THT=(HPk−HT+R)Pk−HT(61)
参考资料
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
21
0- 0
已为社区贡献3条内容
所有评论(0)