k时刻的位置和速度
x
k
=
[
p
k
v
k
]
x_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix}
xk=[pkvk]
运动方程
x
k
=
F
k
−
1
x
k
−
1
+
G
k
−
1
u
k
−
1
+
w
k
−
1
=
[
1
Δ
t
0
1
]
x
k
−
1
+
[
1
2
Δ
t
2
Δ
t
]
a
k
−
1
+
w
k
−
1
x_k = F_{k-1}x_{k-1} + G_{k-1}u_{k-1}+w_{k-1} \\ =\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{1}{2}\Delta t^2 \\ \Delta t \end{bmatrix} a_{k-1} + w_{k-1}
xk=Fk−1xk−1+Gk−1uk−1+wk−1=[10Δt1]xk−1+[21Δt2Δt]ak−1+wk−1
F k F_k Fk叫状态转移矩阵, G k G_k Gk叫控制输入矩阵, 噪声 w k ∼ N ( 0 , Q k ) w_k \sim N(0, Q_k) wk∼N(0,Qk)
测量方程
z
k
=
H
k
x
k
+
v
k
z_k = H_kx_k + v_k
zk=Hkxk+vk
H k H_k Hk为状态观测矩阵。 注意这里的 v k v_k vk是噪声不是速度, 后面我们只用 x k x_k xk来代表运动状态, 而不用 p k , v k p_k, v_k pk,vk来举例了, 所以后面的 v k v_k vk都是噪声 v k ∼ N ( 0 , R k ) v_k \sim N(0, R_k) vk∼N(0,Rk)
然后就可以进行我们的prediction 和 correction
我们用 x k x^k xk表示真实状态, x ^ k \hat{x}_k x^k表示通过运动方程预测的值(先验估计值a prior state estimate), x ˇ k \check{x}_k xˇk表示最优估计值(后验估计值a posteriori state estimate)
e
ˇ
k
=
x
k
−
x
ˇ
k
\check e_k = x_k - \check x_k
eˇk=xk−xˇk真实值与先验的误差
e
^
k
=
x
k
−
x
^
k
\hat e_k = x_k - \hat x_k
e^k=xk−x^k真实值与后验的误差
P
ˇ
k
=
c
o
v
(
e
ˇ
k
)
=
E
(
e
ˇ
k
∗
e
ˇ
k
T
)
\check P_k = cov(\check e_k) = E(\check e_k * \check e_k ^T)
Pˇk=cov(eˇk)=E(eˇk∗eˇkT)真实值与先验的协方差, 注意
e
ˇ
k
\check e_k
eˇk为列向量,
e
ˇ
k
∗
e
ˇ
k
T
\check e_k * \check e_k ^T
eˇk∗eˇkT为矩阵
P
^
k
=
c
o
v
(
e
^
k
)
=
E
(
e
^
k
∗
e
^
k
T
)
\hat P_k = cov(\hat e_k) = E(\hat e_k * \hat e_k^T)
P^k=cov(e^k)=E(e^k∗e^kT) 真实值与后验的协方差
推导一下后面那个等式
c
o
v
(
e
ˇ
k
)
=
E
(
e
ˇ
k
∗
e
ˇ
k
T
)
−
E
(
e
ˇ
k
)
E
(
e
ˇ
k
T
)
cov(\check e_k) = E(\check e_k * \check e_k ^T) - E(\check e_k) E(\check e_k^T)
cov(eˇk)=E(eˇk∗eˇkT)−E(eˇk)E(eˇkT)
其中
E
(
e
ˇ
k
)
=
E
(
x
k
−
x
ˇ
k
)
=
−
E
(
x
ˇ
k
)
=
0
E(\check e_k) = E(x_k - \check x_k) = -E(\check x_k) = 0
E(eˇk)=E(xk−xˇk)=−E(xˇk)=0 先验均值为0
prediction
更新先验
更新运动状态
x
ˇ
k
=
F
k
−
1
x
^
k
−
1
+
G
k
−
1
u
k
−
1
+
w
k
−
1
\mathbf{\check x_k = F_{k-1}\hat x_{k-1} + G_{k-1}u_{k-1}+w_{k-1}}
xˇk=Fk−1x^k−1+Gk−1uk−1+wk−1 , 这个很直观, 根据运动方程得到
更新协方差
P ˇ k = c o v ( e ˇ k ) = c o v ( e ˇ k , e ˇ k ) = c o v ( x ˇ k ) = F k − 1 c o v ( x ^ k − 1 ) F k − 1 T + c o v ( w k − 1 ) = F k − 1 P ^ k − 1 F k − 1 T + Q k − 1 \check P_k = cov(\check e_k) = cov(\check e_k, \check e_k) = cov(\check x_k) = F_{k-1}cov(\hat x_{k-1} ) F_{k-1}^T + cov(w_{k-1})= F_{k-1}\hat P_{k-1} F_{k-1}^T + Q_{k-1} Pˇk=cov(eˇk)=cov(eˇk,eˇk)=cov(xˇk)=Fk−1cov(x^k−1)Fk−1T+cov(wk−1)=Fk−1P^k−1Fk−1T+Qk−1
得到
P
ˇ
k
=
F
k
−
1
P
^
k
−
1
F
k
−
1
T
+
Q
k
−
1
\mathbf {\check P_k = F_{k-1}\hat P_{k-1} F_{k-1}^T + Q_{k-1}}
Pˇk=Fk−1P^k−1Fk−1T+Qk−1
稍微推导一下
c
o
v
(
e
ˇ
k
,
e
ˇ
k
)
=
c
o
v
(
x
k
−
x
ˇ
k
,
x
k
−
x
ˇ
k
)
=
c
o
v
(
−
x
ˇ
k
,
−
x
ˇ
k
)
=
c
o
v
(
x
ˇ
k
,
x
ˇ
k
)
cov(\check e_k, \check e_k) = cov(x_k - \check x_k, x_k - \check x_k)=cov(- \check x_k,- \check x_k)=cov(\check x_k,\check x_k)
cov(eˇk,eˇk)=cov(xk−xˇk,xk−xˇk)=cov(−xˇk,−xˇk)=cov(xˇk,xˇk)
常数与常数,变量与常数(
x
k
x_k
xk)的协方差为0, 协方差也满足负负得正
Correction
先算optimal gain
K
k
=
P
ˇ
k
H
k
T
(
H
k
P
ˇ
k
H
k
T
+
R
k
)
−
1
\mathbf{K_k = \check P_kH_k^T(H_k\check P_kH_k^T+R_k)^{-1}}
Kk=PˇkHkT(HkPˇkHkT+Rk)−1
这个
K
K
K怎么来的呢?
我们希望后验误差
e
^
k
2
\hat e_k^2
e^k2尽可能小,又由于
x
^
k
\hat x_k
x^k是随机变量,可以对后验误差求期望,这样把随机变量出现的概率也考虑进去了(outlier 概率小)。所以最终我们希望最小化
E
(
e
^
k
2
)
E(\hat e_k ^2)
E(e^k2), 而
E
(
e
^
k
2
)
=
t
r
a
c
e
(
E
(
e
^
k
∗
e
^
k
T
)
)
=
t
r
a
c
e
(
P
^
k
)
E(\hat e_k ^2)=trace( E(\hat e_k * \hat e_k ^T))=trace(\hat P_k)
E(e^k2)=trace(E(e^k∗e^kT))=trace(P^k), 也就是要最小化后验协方差
P
^
k
\hat P_k
P^k的trace
P ^ k = c o v ( e ^ k ) = c o v ( x k − x ^ k ) = c o v ( x k − ( x ˇ k + K k ( z k − H k x ˇ k ) ) ) = c o v ( x k − ( x ˇ k + K k ( H k x k + v k − H k x ˇ k ) ) ) = c o v ( ( I − K k H k ) ( x k − x ˇ k ) − K k v k ) = ( I − K k H k ) P ˇ k ( I − K k H k ) T + K k R k K k T = P ˇ k − K k H k P ˇ k − P ˇ k H k T K k T + K k ( H k P ˇ k H k T + R k ) K k T \begin{align*} \hat P_k &= cov(\hat e_k) = cov(x_k - \hat x_k) = cov(x_k - (\check x_k + K_k(z_k-H_k\check x_k)))\\ &=cov(x_k - (\check x_k + K_k(H_kx_k + v_k-H_k\check x_k)))\\ &=cov((I-K_kH_k)(x_k-\check x_k)-K_kv_k)\\ &=(I-K_kH_k)\check P_k(I-K_kH_k)^T + K_kR_kK_k^T\\ &= \check P_k - K_kH_k\check P_k - \check P_kH_k^TK_k^T+K_k(H_k\check P_kH_k^T+R_k)K_k^T \end{align*} P^k=cov(e^k)=cov(xk−x^k)=cov(xk−(xˇk+Kk(zk−Hkxˇk)))=cov(xk−(xˇk+Kk(Hkxk+vk−Hkxˇk)))=cov((I−KkHk)(xk−xˇk)−Kkvk)=(I−KkHk)Pˇk(I−KkHk)T+KkRkKkT=Pˇk−KkHkPˇk−PˇkHkTKkT+Kk(HkPˇkHkT+Rk)KkT
由于
d
(
t
r
(
B
A
C
)
)
d
A
=
B
T
C
T
\frac{d(tr(BAC))}{dA} = B^TC^T
dAd(tr(BAC))=BTCT, 所以
d
(
t
r
(
P
^
k
)
)
d
K
k
=
−
2
(
H
k
P
ˇ
k
)
T
+
2
K
k
(
H
k
P
ˇ
k
H
k
T
+
R
k
)
\begin{align*} \frac{d(tr(\hat P_k ))}{dK_k} = -2(H_k\check P_k)^T + 2K_k(H_k\check P_kH_k^T+R_k) \end{align*}
dKkd(tr(P^k))=−2(HkPˇk)T+2Kk(HkPˇkHkT+Rk)
令其导数为0, 就得到 K k K_k Kk的推导, 注意 P ˇ k \check P_k Pˇk是对称矩阵, 所以转置也是自己
K
K
K的直观理解
直观但不严谨的表示
K
k
=
(
x
^
k
−
x
ˇ
k
)
/
(
z
k
−
H
k
x
ˇ
k
)
K_k= (\hat x_k - \check x_k) / (z_k-H_k\check x_k)
Kk=(x^k−xˇk)/(zk−Hkxˇk)
(
x
^
k
−
x
ˇ
k
)
(\hat x_k - \check x_k)
(x^k−xˇk)可以理解为predicted error
(
z
k
−
H
k
x
ˇ
k
)
=
(
z
k
−
H
k
x
^
k
)
+
(
H
k
x
^
k
−
H
k
x
ˇ
k
)
(z_k-H_k\check x_k) = (z_k - H_k\hat x_k) + (H_k\hat x_k - H_k\check x_k)
(zk−Hkxˇk)=(zk−Hkx^k)+(Hkx^k−Hkxˇk),其中
(
z
k
−
H
k
x
^
k
)
(z_k - H_k\hat x_k)
(zk−Hkx^k) 可以理解为measurement error,
(
H
k
x
^
k
−
H
k
x
ˇ
k
)
(H_k\hat x_k - H_k\check x_k)
(Hkx^k−Hkxˇk)为predicted error
然后更新后验
x
^
k
=
x
ˇ
k
+
K
k
(
z
k
−
H
k
x
ˇ
k
)
\mathbf{\hat x_k = \check x_k + K_k(z_k-H_k\check x_k)}
x^k=xˇk+Kk(zk−Hkxˇk)
P
^
k
=
(
1
−
K
k
H
k
)
P
ˇ
k
\mathbf{\hat P_k = (1 - K_kH_k)\check P_k}
P^k=(1−KkHk)Pˇk
第二个式, 对第一个式两边求cov即可得
https://zhuanlan.zhihu.com/p/48876718
https://zhuanlan.zhihu.com/p/134595781
https://www.youtube.com/watch?v=LioOvUZ1MiM