卡尔曼滤波算法原理(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
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^k∣k−1=Fkx^k−1∣k−1
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 Pk∣k−1=FkPk−1∣k−1FkT+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=Pk∣k−1HkT(HkPk∣k−1HkT+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^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
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=Fkxk−1+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) wk∼N(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=Hk∗xk+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) vk∼N(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^k∣k−1=Fkx^k−1∣k−1
为状态估计方程，表示预测下一步状态。其中：
x ^ k ∣ k − 1 \hat x_{k|k-1} x^k∣k−1为k-1时刻对k时刻的状态预测。
x ^ k − 1 ∣ k − 1 \hat x_{k-1|k-1} x^k−1∣k−1在时刻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 Pk∣k−1=FkPk−1∣k−1FkT+Qk
为预测估计协方差方程，计算先验概率。其中：
P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1为k-1时刻的后验估计误差协方差矩阵，度量估计值的精确程度。
P k ∣ k − 1 P_{k|k-1} Pk∣k−1为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=Pk∣k−1HkT(HkPk∣k−1HkT+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^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
为更新状态估计方程，也就是最终的滤波状态结果。
可以使用测量残差来简化表达公式 y ^ = z k − H k x ^ k ∣ k − 1 \hat y = z_k - H_k \hat x_{k|k-1} y^=zk−Hkx^k∣k−1， 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^)=HkPk∣k−1HkT+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=Pk∣k−1HkTSk−1，状态估计更新可以简化为 x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^k∣k=x^k∣k−1+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} Pk∣k=(I−KkHk)Pk∣k−1
为更新估计协方差方程，计算得到后验概率。
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) Pk∣k−1=cov(xk−x^k∣k−1)=E((xk−x^k∣k−1)(xk−x^k∣k−1)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) Pk∣k=cov(xk−x^k∣k)=E((xk−x^k∣k)(xk−x^k∣k)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(zk−Hkx^k∣k−1)
从状态估计方程推 P k ∣ k − 1 P_{k|k-1} Pk∣k−1：
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}) Pk∣k−1=cov(xk−x^k∣k−1)
= 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(Fkxk−1+wk−Fkx^k−1∣k−1)
= 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(xk−1−x^k−1∣k−1))+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(xk−1−x^k−1∣k−1)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 =FkPk−1∣k−1FkT+Qk
从状态估计更新方程推 P k ∣ k P_{k|k} Pk∣k：
P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pk∣k=cov(xk−x^k∣k)
= 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(xk−x^k∣k−1−Kk(zk−Hkx^k∣k−1))
= 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(xk−x^k∣k−1−Kk(Hkxk+vk−Hkx^k∣k−1))
= 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(xk−x^k∣k−1−KkHk(xk−x^k∣k−1)−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((I−KkHk)(xk−x^k∣k−1)−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((I−KkHk)(xk−x^k∣k−1))+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 =(I−KkHk)cov(xk−x^k∣k−1)(I−KkHk)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 =(I−KkHk)Pk∣k−1(I−KkHk)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}) Pk∣k=cov(xk−x^k∣k)，当 P k ∣ k P_{k|k} Pk∣k矩阵的均方误差为最小时，即可求出最优卡尔曼增益。
矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差，即是求矩阵的迹的最小值，对矩阵的迹求导，导数为0时，迹最小。矩阵的迹求导有相关公式，可参考该文章。协方差 P k ∣ k P_{k|k} Pk∣k是对称矩阵。
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 Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)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 =(Pk∣k−1−KkHkPk∣k−1)(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 =Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkHkPk∣k−1(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 =Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+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(Pk∣k)=tr(Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T++Kk(HkPk∣k−1HkT+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(Pk∣k−1)−tr(KkHkPk∣k−1)−tr(Pk∣k−1(KkHk)T)+tr(Kk(HkPk∣k−1HkT+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(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+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(Pk∣k)=d Kkd [tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+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(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)
当矩阵导数是0的时候得到 P k ∣ k P_{k|k} Pk∣k的迹（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(Pk∣k)=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+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=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=Pk∣k−1HkTSk−1
最优卡尔曼增益化简 P k ∣ k P_{k|k} Pk∣k
K k S k = ( H k P k ∣ k − 1 ) T K_kS_k = (H_k P_{k|k-1})^T KkSk=(HkPk∣k−1)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=(HkPk∣k−1)TKkT=Pk∣k−1(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 Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+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 =Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(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 =Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Pk∣k−1(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} =Pk∣k−1−KkHkPk∣k−1
= ( I − K k H k ) P k ∣ k − 1 \qquad = (I - K_k H_k )P_{k|k-1} =(I−KkHk)Pk∣k−1
该简化式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1只能在最优卡尔曼增益时才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，则必须使用上面未简化后的后验误差协方差公式 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 Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT。
2 自适应卡尔曼滤波AKF
平时不用，暂时附上别人的文章
自动驾驶-自适应卡尔曼滤波AKF
Sage-Husa自适应卡尔曼滤波
主要是使用历史固定帧的数据去更新R和Q矩阵，只更新R矩阵的做法比较多，并且只用了加权求和的方法，动态调节参数没有使用。
3 扩展卡尔曼滤波EKF
解决非线性问题，如极坐标系的雷达，观测到的是径向距离和角度，这个时候的观测矩阵 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^k∣k−1=f(xk−1,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 Pk∣k−1=JFPk−1∣k−1JFT+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=∂x∂f∣x^k−1∣k−1,uk
J H = ∂ h ∂ x ∣ x ^ k ∣ k − 1 J_H = \frac {\partial h}{\partial x} |_{\hat x_{k|k-1}} JH=∂x∂h∣x^k∣k−1
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=zk−h(x^k∣k−1,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=JHPk∣k−1HkT+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=Pk∣k−1JHTSk−1
x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^k∣k=x^k∣k−1+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} Pk∣k=(I−KkJH)Pk∣k−1
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 关于无迹变换
无迹变换的步骤如下图，在原高斯分布中按规则采样固定点；采样完后进行非线性变换；对非线性后的结果加权求和，得到加权后的均值和方差。
| |
|
|
|---|
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)i−n 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=1∑2n+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=1∑2n+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} χk−1=(μk−1μk−1+((n+k)Pk−1)iμk−1−((n+k)Pk−1)i−n)(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} χk∣k−1∗[i]=g(χk−1[i])(4.2)
- 加权均值和方差
计算状态空间下的加权均值 μ k ∣ k − 1 \mu_{k|k-1} μk∣k−1和加权方差 P k ∣ k − 1 P_{k|k-1} Pk∣k−1。
μ 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} μk∣k−1=i=1∑2n+1w[i]χk∣k−1∗[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} Pk∣k−1=i=1∑2n+1w[i](χk∣k−1∗[i]−μk∣k−1)(χk∣k−1∗[i]−μk∣k−1)T+Q(4.4)
4.4 更新
- 预测更新
对预测估计的状态量再做一遍无迹变换，映射到测量空间，一般会为降低计算量忽略采样这步(4.5)，直接使用 χ k ∣ k − 1 = χ k ∣ k − 1 ∗ \chi_{k|k-1} = \chi^{*}_{k|k-1} χk∣k−1=χk∣k−1∗，一定程度上会降低精度。
χ 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} χk∣k−1=(μk∣k−1μk∣k−1+((n+k)Pk∣k−1)iμk∣k−1−((n+k)Pk∣k−1)i−n)(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} Zk∣k−1∗[i]=h(χk∣k−1[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} zk∣k−1=i=1∑2n+1w[i]Zk∣k−1[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} Sk∣k−1=i=1∑2n+1w[i](Zk∣k−1[i]−zk∣k−1)(Zk∣k−1[i]−zk∣k−1)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} Tk∣k−1=i=1∑2n+1w[i](χk∣k−1[i]−μk∣k−1)(Zk∣k−1[i]−zk∣k−1)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=Tk∣k−1Sk∣k−1−1(4.10) -
更新状态估计
其中 z k + 1 z_{k+1} zk+1是当前的观测量， z k ∣ k − 1 z_{k|k-1} zk∣k−1是预测映射在观测空间的伪观测量
μ 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=μk∣k−1+Kk(zk+1−zk∣k−1)(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=Pk∣k−1+KkSk∣k−1KkT(4.12)
4.5 总结
总的来看，就是两步无迹变换+计算卡尔曼增益+状态更新，一共十二个公式。
UKF精度优于EKF，相当于二阶泰勒展开，但速度相对略慢一些。
UKF不需要计算雅各比矩阵，省去了求导的过程。
UKF和PF很相似，区别是无迹变换的粒子是固定的，而粒子滤波的粒子是随机的。
附上源码：
UKF_C++
UKF_python