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
更新先验
更新运动状态
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, 协方差也满足负负得正
先算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