卡尔曼滤波算法原理(KF,EKF,AKF,UKF)

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)

主要是KF、EKF、UKF算法公式推导,直接看公式会比较枯燥,建议推导一下。

新增文章卡尔曼运动模型公式推导CTRV+CTRA,主要是EKF的CTRV、CTRA两个运动模型的公式推导,以及困扰我很久的Q矩阵推导,一直不明白为什么要用 Δ t \Delta t Δt来设置Q矩阵

  • 滤波器
    • 主要运动模型
  • KF
    • CV
    • CA
  • EKF
    • CTRV
    • CTRA
  • UKF
    • CTRV
    • CTRA

参考文章及代码

  • Kalman-Python
    十分清晰的各种kalman原理和相应的Python代码,用来理解学习很方便,其实把这个项目跑一遍就差不多了。

  • CarND-EKF, CarND-UKF, SFND_UKF
    udacity的kalman的C++框架,具体实现要补充,可在github搜相应的项目,有别人完善好的,如:ukf实现

  • 从贝叶斯滤波到卡尔曼滤波
    从贝叶斯滤波理论直接推导

  • 卡尔曼滤波算法详细推导
    非常详细,公式推导写的很明白

  • 卡尔曼滤波-维基百科
    维基百科写的也非常好,有关于最优卡尔曼增益的简化推导

1 卡尔曼滤波KF

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)_第1张图片

1.1 KF公式

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 \hat x_{k|k-1} = F_k \hat x_{k-1|k-1} x^kk1=Fkx^k1k1
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k Pkk1=FkPk1k1FkT+Qk
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|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} Kk=Pkk1HkT(HkPkk1HkT+Rk)1
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat x_{k|k} = \hat x_{k|k-1} + K_k (z_k - H_k \hat x_{k|k-1}) x^kk=x^kk1+Kk(zkHkx^kk1)
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pkk=(IKkHk)Pkk1

1.2 已知假设

已知假设,卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式:

  • 状态估计方程
    x k = F k x k − 1 + B k u k + w k x_{k} = F_k x_{k-1} + B_ku_k + w_k xk=Fkxk1+Bkuk+wk
    其中: F k F_k Fk为状态变换模型(矩阵/向量),运动学一般是矩阵(状态转移矩阵)。
    B k B_k Bk是作用在控制器向量 u k u_k uk上的输入-控制模型。一般运动学中没有这项,因为对于检测的目标的是无法测量其内部的控制量,所以简化为0。
    w k ∼ N ( 0 , Q k ) w_k \sim N(0,Q_k) wkN(0,Qk)是过程噪声。均值为0。 x k x_{k} xk对应的就是高斯分布的均值,因此这项可简化为0。

  • 状态估计转移方程
    z k = H k ∗ x k + v k z_k = H_k * x_k + v_k zk=Hkxk+vk
    表示k时刻真实状态 x k x_k xk的一个测量,其中: H k H_k Hk为观测模型(观测矩阵),它把真实状态空间映射成观测空间, v k ∼ N ( 0 , R k ) v_k \sim N(0,R_k) vkN(0,Rk)是观测噪声。
    初始状态以及每一时刻的噪声 x 0 , w 1 , . . . , w k , v 1... v k {x_0, w_1, ..., w_k, v1 ... v_k} x0,w1,...,wk,v1...vk都认为是互相独立的

从已知假设出发,可以计算相应的协方差矩阵,这个也是公式推导的主要部分。后文在详细阐述KF的五个公式后进行推导各自的协方差矩阵,会介绍如何计算出最优卡尔曼增益K值。

1.3 预测:

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 \hat x_{k|k-1} = F_k \hat x_{k-1|k-1} x^kk1=Fkx^k1k1
为状态估计方程,表示预测下一步状态。其中:
x ^ k ∣ k − 1 \hat x_{k|k-1} x^kk1为k-1时刻对k时刻的状态预测。
x ^ k − 1 ∣ k − 1 \hat x_{k-1|k-1} x^k1k1在时刻k-1的状态估计。
F k F_k Fk为状态转移矩阵。

P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k Pkk1=FkPk1k1FkT+Qk
为预测估计协方差方程,计算先验概率。其中:
P k − 1 ∣ k − 1 P_{k-1|k-1} Pk1k1为k-1时刻的后验估计误差协方差矩阵,度量估计值的精确程度。
P k ∣ k − 1 P_{k|k-1} Pkk1为k-1时刻到k时刻的估计误差协方差矩阵。
Q k Q_k Qk为过程噪声协方差矩阵,越大说明越不相信预测。实际工程中可自己调参,也可以自适应(AKF)。

1.4 更新:

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|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} Kk=Pkk1HkT(HkPkk1HkT+Rk)1
为卡尔曼增益方程 K k K_k Kk为最优卡尔曼增益。
R k R_k Rk为测量噪声协方差矩阵,越大说明越不相信观测。实际工程中测量噪声由厂商提供,也可以自己调参,也可以自适应(AKF)。

x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat x_{k|k} = \hat x_{k|k-1} + K_k (z_k - H_k \hat x_{k|k-1}) x^kk=x^kk1+Kk(zkHkx^kk1)
为更新状态估计方程,也就是最终的滤波状态结果。
可以使用测量残差来简化表达公式 y ^ = z k − H k x ^ k ∣ k − 1 \hat y = z_k - H_k \hat x_{k|k-1} y^=zkHkx^kk1 S k = c o v ( y ^ ) = H k P k ∣ k − 1 H k T + R k S_k = cov(\hat y) = H_k P_{k|k-1} H^T_k + R_k Sk=cov(y^)=HkPkk1HkT+Rk
即卡尔曼增益可以简化为 K k = P k ∣ k − 1 H k T S k − 1 K_k = P_{k|k-1}H^T_k S_k^{-1} Kk=Pkk1HkTSk1,状态估计更新可以简化为 x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^kk=x^kk1+Kky^

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pkk=(IKkHk)Pkk1
为更新估计协方差方程,计算得到后验概率。

1.5 公式推导

下文公式预警!!虽然多但是是很简单的!慢慢看!

已知: Q k Q_k Qk R k R_k Rk一般为固定值,高级卡尔曼滤波可以用自适应。

Q k = c o v ( w k ) = E ( w k w k T ) Q_k = cov(w_k) = E(w_kw_k^T) Qk=cov(wk)=E(wkwkT)
R k = c o v ( v k ) = E ( v k v k T ) R_k = cov(v_k) = E(v_kv_k^T) Rk=cov(vk)=E(vkvkT)
P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) = E ( ( x k − x ^ k ∣ k − 1 ) ( x k − x ^ k ∣ k − 1 ) T ) P_{k|k-1} = cov(x_k - \hat x_{k|k-1}) = E((x_k - \hat x_{k|k-1})(x_k - \hat x_{k|k-1})^T) Pkk1=cov(xkx^kk1)=E((xkx^kk1)(xkx^kk1)T)
P k ∣ k = c o v ( x k − x ^ k ∣ k ) = E ( ( x k − x ^ k ∣ k ) ( x k − x ^ k ∣ k ) T ) P_{k|k} = cov(x_k - \hat x_{k|k}) = E((x_k - \hat x_{k|k})(x_k - \hat x_{k|k})^T) Pkk=cov(xkx^kk)=E((xkx^kk)(xkx^kk)T)
S k = c o v ( y ^ ) = c o v ( z k − H k x ^ k ∣ k − 1 ) S_k = cov(\hat y) = cov(z_k - H_k \hat x_{k|k-1}) Sk=cov(y^)=cov(zkHkx^kk1)

从状态估计方程推 P k ∣ k − 1 P_{k|k-1} Pkk1

P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) P_{k|k-1} = cov(x_k - \hat x_{k|k-1}) Pkk1=cov(xkx^kk1)
= c o v ( F k x k − 1 + w k − F k x ^ k − 1 ∣ k − 1 ) \qquad \quad = cov(F_k x_{k-1} + w_k - F_k \hat x_{k-1|k-1}) =cov(Fkxk1+wkFkx^k1k1)
= c o v ( F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) ) + c o v ( w k ) \qquad \quad = cov(F_k (x_{k-1}-\hat x_{k-1|k-1})) + cov(w_k) =cov(Fk(xk1x^k1k1))+cov(wk)
= F k c o v ( x k − 1 − x ^ k − 1 ∣ k − 1 ) F k T + Q k \qquad \quad = F_k cov(x_{k-1}-\hat x_{k-1|k-1})F_k^T + Q_k =Fkcov(xk1x^k1k1)FkT+Qk
= F k P k − 1 ∣ k − 1 F k T + Q k \qquad \quad = F_k P_{k-1|k-1} F_k^T + Q_k =FkPk1k1FkT+Qk

从状态估计更新方程推 P k ∣ k P_{k|k} Pkk

P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pkk=cov(xkx^kk)
= c o v ( x k − x ^ k ∣ k − 1 − K k ( z k − H k x ^ k ∣ k − 1 ) ) \qquad = cov(x_k - \hat x_{k|k-1} - K_k (z_k - H_k \hat x_{k|k-1})) =cov(xkx^kk1Kk(zkHkx^kk1))
= c o v ( x k − x ^ k ∣ k − 1 − K k ( H k x k + v k − H k x ^ k ∣ k − 1 ) ) \qquad = cov(x_k - \hat x_{k|k-1} - K_k (H_k x_k + v_k - H_k \hat x_{k|k-1})) =cov(xkx^kk1Kk(Hkxk+vkHkx^kk1))
= c o v ( x k − x ^ k ∣ k − 1 − K k H k ( x k − x ^ k ∣ k − 1 ) − K k v k ) \qquad = cov(x_k - \hat x_{k|k-1} - K_k H_k( x_k - \hat x_{k|k-1}) - K_k v_k) =cov(xkx^kk1KkHk(xkx^kk1)Kkvk)
= c o v ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k ) \qquad = cov((I - K_k H_k)( x_k - \hat x_{k|k-1}) - K_k v_k) =cov((IKkHk)(xkx^kk1)Kkvk)
= c o v ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) ) + c o v ( K k v k ) \qquad = cov((I - K_k H_k)( x_k - \hat x_{k|k-1})) + cov(K_k v_k) =cov((IKkHk)(xkx^kk1))+cov(Kkvk)
= ( I − K k H k ) c o v ( x k − x ^ k ∣ k − 1 ) ( I − K k H k ) T + K k c o v ( v k ) K k T \qquad = (I - K_k H_k)cov( x_k - \hat x_{k|k-1})(I - K_k H_k)^T + K_k cov(v_k) K_k^T =(IKkHk)cov(xkx^kk1)(IKkHk)T+Kkcov(vk)KkT
= ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T \qquad = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T =(IKkHk)Pkk1(IKkHk)T+KkRkKkT

这一公式对于任何卡尔曼增益 K k {K_k} Kk都成立。如果 K k {K_k} Kk是最优卡尔曼增益,则可以进一步简化

基于上式推导最优卡尔曼增益 K k {K_k} Kk

卡尔曼滤波器是最小均方误差估计器,后验状态误差估计是指 P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pkk=cov(xkx^kk),当 P k ∣ k P_{k|k} Pkk矩阵的均方误差为最小时,即可求出最优卡尔曼增益。

矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差,即是求矩阵的迹的最小值,对矩阵的迹求导,导数为0时,迹最小。矩阵的迹求导有相关公式,可参考该文章。协方差 P k ∣ k P_{k|k} Pkk对称矩阵

P k ∣ 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|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pkk=(IKkHk)Pkk1(IKkHk)T+KkRkKkT
= ( P k ∣ k − 1 − K k H k P k ∣ k − 1 ) ( I − ( K k H k ) T ) + K k R k K k T \qquad = (P_{k|k-1} - K_k H_k P_{k|k-1})(I - (K_k H_k)^T) + K_k R_k K_k^T =(Pkk1KkHkPkk1)(I(KkHk)T)+KkRkKkT
= P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k H k P k ∣ k − 1 ( K k H k ) T + K k R k K k T \qquad = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k H_k P_{k|k-1}(K_k H_k)^T + K_k R_k K_k^T =Pkk1KkHkPkk1Pkk1(KkHk)T+KkHkPkk1(KkHk)T+KkRkKkT
= P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T \qquad = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T =Pkk1KkHkPkk1Pkk1(KkHk)T+Kk(HkPkk1HkT+Rk)KkT

t r ( P k ∣ k ) = t r ( P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + + K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pkk)=tr(Pkk1KkHkPkk1Pkk1(KkHk)T++Kk(HkPkk1HkT+Rk)KkT)
= t r ( P k ∣ k − 1 ) − t r ( K k H k P k ∣ k − 1 ) − t r ( P k ∣ k − 1 ( K k H k ) T ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) \qquad = tr(P_{k|k-1} )- tr(K_k H_k P_{k|k-1}) -tr( P_{k|k-1} (K_k H_k)^T) + tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) =tr(Pkk1)tr(KkHkPkk1)tr(Pkk1(KkHk)T)+tr(Kk(HkPkk1HkT+Rk)KkT)
= t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) \qquad = tr(P_{k|k-1} )- 2tr(K_k H_k P_{k|k-1})+ tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) =tr(Pkk1)2tr(KkHkPkk1)+tr(Kk(HkPkk1HkT+Rk)KkT)

d   t r ( P k ∣ k ) d   K k = d   [ t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) ] d   K k \frac {d \ tr(P_{k|k})} {d\ K_k} =\frac {d\ {[tr(P_{k|k-1}) - 2tr(K_k H_k P_{k|k-1}) + tr(K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T)]}} {d\ K_k} d Kkd tr(Pkk)=d Kkd [tr(Pkk1)2tr(KkHkPkk1)+tr(Kk(HkPkk1HkT+Rk)KkT)]
= − 2 ( H k P k ∣ k − 1 ) T + 2 K k ( H k P k ∣ k − 1 H k T + R k ) \qquad \quad =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) =2(HkPkk1)T+2Kk(HkPkk1HkT+Rk)

当矩阵导数是0的时候得到 P k ∣ k P_{k|k} Pkk的迹(trace)的最小值:
t r ( P k ∣ k ) d K k = − 2 ( H k P k ∣ k − 1 ) T + 2 K k ( H k P k ∣ k − 1 H k T + R k ) = 0 \frac {tr(P_{k|k})} {dK_k} =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) = 0 dKktr(Pkk)=2(HkPkk1)T+2Kk(HkPkk1HkT+Rk)=0

K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 = P k ∣ k − 1 H k T S k − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} = P_{k|k-1}H^T_k S_k^{-1} Kk=Pkk1HkT(HkPkk1HkT+Rk)1=Pkk1HkTSk1

最优卡尔曼增益化简 P k ∣ k P_{k|k} Pkk

K k S k = ( H k P k ∣ k − 1 ) T K_kS_k = (H_k P_{k|k-1})^T KkSk=(HkPkk1)T

K k S k K k T = ( H k P k ∣ k − 1 ) T K k T = P k ∣ k − 1 ( K k H k ) T K_k S_k K^T_k = (H_k P_{k|k-1})^T K^T_k = P_{k|k-1} (K_k H_k)^T KkSkKkT=(HkPkk1)TKkT=Pkk1(KkHk)T

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T Pkk=Pkk1KkHkPkk1Pkk1(KkHk)T+Kk(HkPkk1HkT+Rk)KkT
= P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k S k K k T \qquad = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k S_k K^T_k =Pkk1KkHkPkk1Pkk1(KkHk)T+KkSkKkT
= P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + P k ∣ k − 1 ( K k H k ) T \qquad = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + P_{k|k-1} (K_k H_k)^T =Pkk1KkHkPkk1Pkk1(KkHk)T+Pkk1(KkHk)T
= P k ∣ k − 1 − K k H k P k ∣ k − 1 \qquad = P_{k|k-1} - K_k H_k P_{k|k-1} =Pkk1KkHkPkk1
= ( I − K k H k ) P k ∣ k − 1 \qquad = (I - K_k H_k )P_{k|k-1} =(IKkHk)Pkk1

该简化式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pkk=(IKkHk)Pkk1只能在最优卡尔曼增益时才成立。如果算术精度总是很低而导致数值稳定性出现问题,或者特意使用非最优卡尔曼增益,则必须使用上面未简化后的后验误差协方差公式 P k ∣ 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|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pkk=(IKkHk)Pkk1(IKkHk)T+KkRkKkT

2 自适应卡尔曼滤波AKF

平时不用,暂时附上别人的文章
自动驾驶-自适应卡尔曼滤波AKF
Sage-Husa自适应卡尔曼滤波
主要是使用历史固定帧的数据去更新R和Q矩阵,只更新R矩阵的做法比较多,并且只用了加权求和的方法,动态调节参数没有使用。

3 扩展卡尔曼滤波EKF

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)_第2张图片

解决非线性问题,如极坐标系的雷达,观测到的是径向距离和角度,这个时候的观测矩阵 H H H就是非线性的函数。(极坐标系可以转笛卡尔坐标系,Apollo融合跟踪里面对雷达物体用的是笛卡尔坐标系做KF)
EKF和KF的区别主要是状态转换模型和观测模型可以是非线性的,可以使用泰勒展开式替换为线性函数,两个协方差矩阵 P P P H H H要使用雅各比矩阵计算每个状态量的一阶偏导。

因为主要是状态估计方程和状态估计转移方程的两个地方有些区别,所以EKF协方差矩阵的公式推导还是跟KF一样的。

3.1预测

x ^ k ∣ k − 1 = f ( x k − 1 , u k , 0 ) \hat x_{k|k-1} = f(x_{k-1},u_k,0) x^kk1=f(xk1,uk,0)

P k ∣ k − 1 = J F P k − 1 ∣ k − 1 J F T + Q k P_{k|k-1} = J_FP_{k-1|k-1}J^T_F + Q_k Pkk1=JFPk1k1JFT+Qk

3.2 使用Jacobians矩阵更新模型

主要是下面两个矩阵会和KF有区别,也就是过程模型(矩阵)和测量模型(矩阵),需要进行雅各比矩阵求导计算。
因为每个运动模型的雅各比矩阵求导是不一样的,所以新增了关于不同运动模型求导雅各比矩阵的文章:卡尔曼运动模型公式推导CTRV+CTRA

J F = ∂ f ∂ x ∣ x ^ k − 1 ∣ k − 1 , u k J_F = \frac {\partial f}{\partial x} |_{\hat x_{k-1|k-1}, u_k} JF=xfx^k1k1,uk

J H = ∂ h ∂ x ∣ x ^ k ∣ k − 1 J_H = \frac {\partial h}{\partial x} |_{\hat x_{k|k-1}} JH=xhx^kk1

3.3 更新

y ^ k = z k − h ( x ^ k ∣ k − 1 , 0 ) \hat y_k = z_k - h(\hat x_{k|k-1},0) y^k=zkh(x^kk1,0)

S k = J H P k ∣ k − 1 H k T + R k S_k = J_H P_{k|k-1} H^T_k + R_k Sk=JHPkk1HkT+Rk

K k = P k ∣ k − 1 J H T S k − 1 K_k = P_{k|k-1}J^T_H S_k^{-1} Kk=Pkk1JHTSk1

x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^kk=x^kk1+Kky^

P k ∣ k = ( I − K k J H ) P k ∣ k − 1 P_{k|k} = (I - K_k J_H) P_{k|k-1} Pkk=(IKkJH)Pkk1

4 无迹卡尔曼滤波UKF

对于非线性问题的处理,过程模型F和测量模型H是非线性的,EKF是求一阶全导数得到线性模型,来近似非线性模型;而UKF是直接寻找一个与真实分布近似的高斯分布,没有用线性表征。

4.1 参考文章及代码

概率机器人——扩展卡尔曼滤波、无迹卡尔曼滤波

无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++

自动驾驶感知融合-无迹卡尔曼滤波(Lidar&Radar)

从贝叶斯滤波到无迹卡尔曼滤波

https://www.cse.sc.edu/~terejanu/files/tutorialUKF.pdf

http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/pdf/slam06-ukf-4.pdf

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

https://github.com/ndrplz/self-driving-car

4.2 关于无迹变换

无迹变换的步骤如下图,在原高斯分布中按规则采样固定点;采样完后进行非线性变换;对非线性后的结果加权求和,得到加权后的均值和方差。

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)_第3张图片
1.原高斯分布中按一定规则采样

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)_第4张图片
2.采样点经过非线性变换

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)_第5张图片
3.加权统计变换结果

4.2.1 原高斯分布中按一定规则采样

无迹变换主要包括两种形式:一般形式的无迹变换比例无迹变换。两种无迹变换的区别主要体现在采样规则和权重计算上。比例无迹变换是对前者做出的改进,但在代码中一般使用一般形式的无迹变换。两者的区别该文章(从贝叶斯滤波到无迹卡尔曼滤波)有详细介绍,以下内容就简单介绍一般形式的无迹变换

μ \mu μ为原始状态量, χ \chi χ为采样扩展后的状态量,n维随机向量 χ ∼ ( μ , P ) \chi \sim (\mu , P) χ(μ,P),一般是采取2n+1个点,n和状态量的个数有关。

  • 关于权重
    总和为1

w [ i ] = { k k + n   i = 1 1 2 ( k + n )   i = 2 , . . . , 2 n + 1 w^{[i]} = \begin{cases} \frac {k}{k + n}& \ i=1\\ \frac {1}{2(k + n)} & \ i=2,...,2n+1 \end{cases} w[i]={k+nk2(k+n)1 i=1 i=2,...,2n+1

  • 计算sigma point
    关于sigma point的采样,可以理解为是在高斯分布的周围左右采点。状态量n个,采样点2n+1个。

χ [ i ] = { μ   i = 1 μ + ( ( n + k ) P ) i   i = 2 , . . . , n + 1 μ − ( ( n + k ) P ) i − n   i = n + 2 , . . . , 2 n + 1 \chi^{[i]} = \begin{cases} \mu& \ i=1\\ \mu + (\sqrt{(n+k)P})_i & \ i=2,...,n+1 \\ \mu - (\sqrt{(n+k)P})_{i-n} & \ i=n+2,...,2n+1 \end{cases} χ[i]=μμ+((n+k)P )iμ((n+k)P )in i=1 i=2,...,n+1 i=n+2,...,2n+1

其中:
k k k表示采样点距离均值 μ \mu μ多远,越远权重越小。对于高斯问题一般取 n + k = 3 n+k=3 n+k=3,所以关于运动模型基本取这个值。

χ [ i ] \chi^{[i]} χ[i]表示第i列,是n*(2n+1)的矩阵。

P P P矩阵是方差矩阵,开根号求解可以使用Cholesky分解。

4.2.2 采样点经过非线性变换

对采样点的每一列进行非线性变换,比如在CTRV运动模型中,使用 x k + 1 = x k + v k w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] x_{k+1} = x_k + \frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]} xk+1=xk+wvk[sin(wΔt+θ)sin(θ)]来进行变换。当然其他模型的线性变换也是适用的。相当于KF里的状态转移方程,一步预测。

χ ′ [ i ] = g ( χ [ i ] ) \chi^{'[i]} = g(\chi^{[i]}) χ[i]=g(χ[i])

4.2.3 加权统计变换结果

μ ′ = ∑ i = 1 2 n + 1 w [ i ] χ ′ [ i ] \mu' = \sum^{2n+1}_{i=1}{w^{[i]} \chi^{'[i]}} μ=i=12n+1w[i]χ[i]

P ′ = ∑ i = 1 2 n + 1 w [ i ] ( χ ′ [ i ] − μ ′ ) ( χ ′ [ i ] − μ ′ ) T P'= \sum^{2n+1}_{i=1} w^{[i]} ({\chi^{'[i]} -\mu')(\chi^{'[i]}-\mu')^T} P=i=12n+1w[i](χ[i]μ)(χ[i]μ)T

4.3 预测

对原始状态量做一遍无迹变换

  • 预测sigma point
    μ \mu μ为原始状态量,也就是均值, χ \chi χ为采样扩展后的状态量。在UKF的CTRV运动模型中,一般会加入过程噪声 a a , a w = 0 a_a,a_w = 0 aa,aw=0

χ k − 1 = ( μ k − 1 μ k − 1 + ( ( n + k ) P k − 1 ) i μ k − 1 − ( ( n + k ) P k − 1 ) i − n ) (4.1) \chi_{k-1} = (\mu_{k-1} \quad \mu_{k-1}+(\sqrt{(n+k)P_{k-1}})_i \quad \mu_{k-1}- (\sqrt{(n+k)P_{k-1}})_{i-n} ) \tag{4.1} χk1=(μk1μk1+((n+k)Pk1 )iμk1((n+k)Pk1 )in)(4.1)

χ k ∣ k − 1 ∗ [ i ] = g ( χ k − 1 [ i ] ) (4.2) \chi_{k|k-1}^{*[i]} = g(\chi_{k-1}^{[i]}) \tag{4.2} χkk1[i]=g(χk1[i])(4.2)

  • 加权均值和方差
    计算状态空间下的加权均值 μ k ∣ k − 1 \mu_{k|k-1} μkk1和加权方差 P k ∣ k − 1 P_{k|k-1} Pkk1

μ k ∣ k − 1 = ∑ i = 1 2 n + 1 w [ i ] χ k ∣ k − 1 ∗ [ i ] (4.3) \mu_{k|k-1} = \sum^{2n+1}_{i=1}{w^{[i]} \chi^{*[i]}_{k|k-1}} \tag{4.3} μkk1=i=12n+1w[i]χkk1[i](4.3)

P k ∣ k − 1 = ∑ i = 1 2 n + 1 w [ i ] ( χ k ∣ k − 1 ∗ [ i ] − μ k ∣ k − 1 ) ( χ k ∣ k − 1 ∗ [ i ] − μ k ∣ k − 1 ) T + Q (4.4) P_{k|k-1}= \sum^{2n+1}_{i=1} w^{[i]} {(\chi^{*[i]}_{k|k-1} -\mu_{k|k-1} )(\chi^{*[i]}_{k|k-1} - \mu_{k|k-1} )^T} + Q \tag{4.4} Pkk1=i=12n+1w[i](χkk1[i]μkk1)(χkk1[i]μkk1)T+Q(4.4)

4.4 更新

  • 预测更新
    对预测估计的状态量再做一遍无迹变换,映射到测量空间,一般会为降低计算量忽略采样这步(4.5),直接使用 χ k ∣ k − 1 = χ k ∣ k − 1 ∗ \chi_{k|k-1} = \chi^{*}_{k|k-1} χkk1=χkk1,一定程度上会降低精度。
    χ k ∣ k − 1 = ( μ k ∣ k − 1 μ k ∣ k − 1 + ( ( n + k ) P k ∣ k − 1 ) i μ k ∣ k − 1 − ( ( n + k ) P k ∣ k − 1 ) i − n ) (4.5) \chi_{k|k-1} = (\mu_{k|k-1} \quad \mu_{k|k-1}+(\sqrt{(n+k)P_{k|k-1}})_i \quad \mu_{k|k-1}- (\sqrt{(n+k)P_{k|k-1}})_{i-n} ) \tag{4.5} χkk1=(μkk1μkk1+((n+k)Pkk1 )iμkk1((n+k)Pkk1 )in)(4.5)

Z k ∣ k − 1 ∗ [ i ] = h ( χ k ∣ k − 1 [ i ] ) (4.6) Z_{k|k-1}^{*[i]} = h(\chi_{k|k-1}^{[i]}) \tag{4.6} Zkk1[i]=h(χkk1[i])(4.6)

z k ∣ k − 1 = ∑ i = 1 2 n + 1 w [ i ] Z k ∣ k − 1 [ i ] (4.7) z_{k|k-1} = \sum^{2n+1}_{i=1} {w^{[i]} Z^{[i]}_{k|k-1}} \tag{4.7} zkk1=i=12n+1w[i]Zkk1[i](4.7)

S k ∣ k − 1 = ∑ i = 1 2 n + 1 w [ i ] ( Z k ∣ k − 1 [ i ] − z k ∣ k − 1 ) ( Z k ∣ k − 1 [ i ] − z k ∣ k − 1 ) T + R (4.8) S_{k|k-1} = \sum^{2n+1}_{i=1} w^{[i]}{ (Z^{[i]}_{k|k-1} - z_{k|k-1}) (Z^{[i]}_{k|k-1} - z_{k|k-1})^T + R} \tag{4.8} Skk1=i=12n+1w[i](Zkk1[i]zkk1)(Zkk1[i]zkk1)T+R(4.8)

  • 计算sigma point在状态空间和测量空间的互相关函数(矩阵)
    T k ∣ k − 1 = ∑ i = 1 2 n + 1 w [ i ] ( χ k ∣ k − 1 [ i ] − μ k ∣ k − 1 ) ( Z k ∣ k − 1 [ i ] − z k ∣ k − 1 ) T (4.9) T_{k|k-1} = \sum^{2n+1}_{i=1} w^{[i]} {(\chi^{[i]}_{k|k-1} - \mu_{k|k-1}) (Z^{[i]}_{k|k-1} - z_{k|k-1})^T } \tag{4.9} Tkk1=i=12n+1w[i](χkk1[i]μkk1)(Zkk1[i]zkk1)T(4.9)

  • 计算卡尔曼增益
    K k = T k ∣ k − 1 S k ∣ k − 1 − 1 (4.10) K_{k} = T_{k|k-1} S_{k|k-1}^{-1} \tag{4.10} Kk=Tkk1Skk11(4.10)

  • 更新状态估计
    其中 z k + 1 z_{k+1} zk+1是当前的观测量, z k ∣ k − 1 z_{k|k-1} zkk1是预测映射在观测空间的伪观测量
    μ k = μ k ∣ k − 1 + K k ( z k + 1 − z k ∣ k − 1 ) (4.11) \mu_{k} = \mu_{k|k-1} + K_{k} (z_{k+1} - z_{k|k-1}) \tag{4.11} μk=μkk1+Kk(zk+1zkk1)(4.11)

  • 更新状态协方差矩阵
    P k = P k ∣ k − 1 + K k S k ∣ k − 1 K k T (4.12) P_{k} = P_{k|k-1} + K_{k} S_{k|k-1} K_{k}^T \tag{4.12} Pk=Pkk1+KkSkk1KkT(4.12)

4.5 总结

总的来看,就是两步无迹变换+计算卡尔曼增益+状态更新,一共十二个公式。
UKF精度优于EKF,相当于二阶泰勒展开,但速度相对略慢一些。
UKF不需要计算雅各比矩阵,省去了求导的过程。
UKF和PF很相似,区别是无迹变换的粒子是固定的,而粒子滤波的粒子是随机的。

附上源码:
UKF_C++
UKF_python

你可能感兴趣的:(滤波器,算法,协方差,自动驾驶,卡尔曼滤波算法)