卡尔曼滤波算法原理(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=Fkx^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
Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_kPk∣k−1=FkPk−1∣k−1FkT+Qk
Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_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+Kk(zk−Hkx^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)
Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}Pk∣k=(I−KkHk)Pk∣k−1

1.2 已知假设

已知假设，卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：

状态估计方程
xk=Fkxk−1+Bkuk+wkx_{k} = F_k x_{k-1} + B_ku_k + w_k xk=Fkxk−1+Bkuk+wk
其中：FkF_kFk为状态变换模型（矩阵/向量），运动学一般是矩阵（状态转移矩阵）。
BkB_kBk是作用在控制器向量uku_kuk上的输入-控制模型。一般运动学中没有这项，因为对于检测的目标的是无法测量其内部的控制量，所以简化为0。
wk∼N(0,Qk)w_k \sim N(0,Q_k)wk∼N(0,Qk)是过程噪声。均值为0。xkx_{k}xk对应的就是高斯分布的均值，因此这项可简化为0。
状态估计转移方程
zk=Hk∗xk+vkz_k = H_k * x_k + v_kzk=Hk∗xk+vk
表示k时刻真实状态xkx_kxk的一个测量，其中：HkH_kHk为观测模型(观测矩阵)，它把真实状态空间映射成观测空间，vk∼N(0,Rk)v_k \sim N(0,R_k)vk∼N(0,Rk)是观测噪声。
初始状态以及每一时刻的噪声x0,w1,...,wk,v1...vk{x_0, w_1, ..., w_k, v1 ... v_k}x0,w1,...,wk,v1...vk都认为是互相独立的

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

1.3 预测：

x^k∣k−1=Fkx^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的状态估计。
FkF_kFk为状态转移矩阵。

Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_kPk∣k−1=FkPk−1∣k−1FkT+Qk
为预测估计协方差方程，计算先验概率。其中：
Pk−1∣k−1P_{k-1|k-1}Pk−1∣k−1为k-1时刻的后验估计误差协方差矩阵，度量估计值的精确程度。
Pk∣k−1P_{k|k-1}Pk∣k−1为k-1时刻到k时刻的估计误差协方差矩阵。
QkQ_kQk为过程噪声协方差矩阵，越大说明越不相信预测。实际工程中可自己调参，也可以自适应（AKF）。

1.4 更新：

Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_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
为卡尔曼增益方程。KkK_kKk为最优卡尔曼增益。
RkR_kRk为测量噪声协方差矩阵，越大说明越不相信观测。实际工程中测量噪声由厂商提供，也可以自己调参，也可以自适应（AKF）。

x^k∣k=x^k∣k−1+Kk(zk−Hkx^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^=zk−Hkx^k∣k−1\hat y = z_k - H_k \hat x_{k|k-1}y^=zk−Hkx^k∣k−1，Sk=cov(y^)=HkPk∣k−1HkT+RkS_k = cov(\hat y) = H_k P_{k|k-1} H^T_k + R_kSk=cov(y^)=HkPk∣k−1HkT+Rk。
即卡尔曼增益可以简化为Kk=Pk∣k−1HkTSk−1K_k = P_{k|k-1}H^T_k S_k^{-1}Kk=Pk∣k−1HkTSk−1，状态估计更新可以简化为x^k∣k=x^k∣k−1+Kky^\hat x_{k|k} = \hat x_{k|k-1} + K_k \hat yx^k∣k=x^k∣k−1+Kky^

Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}Pk∣k=(I−KkHk)Pk∣k−1
为更新估计协方差方程，计算得到后验概率。

1.5 公式推导

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

已知：QkQ_kQk和RkR_kRk一般为固定值，高级卡尔曼滤波可以用自适应。

Qk=cov(wk)=E(wkwkT)Q_k = cov(w_k) = E(w_kw_k^T)Qk=cov(wk)=E(wkwkT)
Rk=cov(vk)=E(vkvkT)R_k = cov(v_k) = E(v_kv_k^T)Rk=cov(vk)=E(vkvkT)
Pk∣k−1=cov(xk−x^k∣k−1)=E((xk−x^k∣k−1)(xk−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)
Pk∣k=cov(xk−x^k∣k)=E((xk−x^k∣k)(xk−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)
Sk=cov(y^)=cov(zk−Hkx^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)

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

Pk∣k−1=cov(xk−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)
=cov(Fkxk−1+wk−Fkx^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)
=cov(Fk(xk−1−x^k−1∣k−1))+cov(wk)\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)
=Fkcov(xk−1−x^k−1∣k−1)FkT+Qk\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
=FkPk−1∣k−1FkT+Qk\qquad \quad = F_k P_{k-1|k-1} F_k^T + Q_k=FkPk−1∣k−1FkT+Qk

从状态估计更新方程推Pk∣kP_{k|k}Pk∣k：

Pk∣k=cov(xk−x^k∣k)P_{k|k} = cov(x_k - \hat x_{k|k})Pk∣k=cov(xk−x^k∣k)
=cov(xk−x^k∣k−1−Kk(zk−Hkx^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))
=cov(xk−x^k∣k−1−Kk(Hkxk+vk−Hkx^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))
=cov(xk−x^k∣k−1−KkHk(xk−x^k∣k−1)−Kkvk)\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)
=cov((I−KkHk)(xk−x^k∣k−1)−Kkvk)\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)
=cov((I−KkHk)(xk−x^k∣k−1))+cov(Kkvk)\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−KkHk)cov(xk−x^k∣k−1)(I−KkHk)T+Kkcov(vk)KkT\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−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT\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

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

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

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

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

Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkTP_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^TPk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT
=(Pk∣k−1−KkHkPk∣k−1)(I−(KkHk)T)+KkRkKkT\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
=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkHkPk∣k−1(KkHk)T+KkRkKkT\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
=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT\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

tr(Pk∣k)=tr(Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T++Kk(HkPk∣k−1HkT+Rk)KkT)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)
=tr(Pk∣k−1)−tr(KkHkPk∣k−1)−tr(Pk∣k−1(KkHk)T)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)\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)
=tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)\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)

dtr(Pk∣k)dKk=d[tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)]dKk\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(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)\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的时候得到Pk∣kP_{k|k}Pk∣k的迹（trace）的最小值：
tr(Pk∣k)dKk=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)=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) = 0dKktr(Pk∣k)=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)=0

Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=Pk∣k−1HkTSk−1K_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

最优卡尔曼增益化简Pk∣kP_{k|k}Pk∣k

KkSk=(HkPk∣k−1)TK_kS_k = (H_k P_{k|k-1})^TKkSk=(HkPk∣k−1)T

KkSkKkT=(HkPk∣k−1)TKkT=Pk∣k−1(KkHk)TK_k S_k K^T_k = (H_k P_{k|k-1})^T K^T_k = P_{k|k-1} (K_k H_k)^TKkSkKkT=(HkPk∣k−1)TKkT=Pk∣k−1(KkHk)T

Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkTP_{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^TPk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT
=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkSkKkT\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
=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Pk∣k−1(KkHk)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
=Pk∣k−1−KkHkPk∣k−1\qquad = P_{k|k-1} - K_k H_k P_{k|k-1}=Pk∣k−1−KkHkPk∣k−1
=(I−KkHk)Pk∣k−1\qquad = (I - K_k H_k )P_{k|k-1}=(I−KkHk)Pk∣k−1

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

2 自适应卡尔曼滤波AKF

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

3 扩展卡尔曼滤波EKF

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

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

3.1预测

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

Pk∣k−1=JFPk−1∣k−1JFT+QkP_{k|k-1} = J_FP_{k-1|k-1}J^T_F + Q_kPk∣k−1=JFPk−1∣k−1JFT+Qk

3.2 使用Jacobians矩阵更新模型

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

JF=∂f∂x∣x^k−1∣k−1,ukJ_F = \frac {\partial f}{\partial x} |_{\hat x_{k-1|k-1}, u_k}JF=∂x∂f∣x^k−1∣k−1,uk

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

3.3 更新

y^k=zk−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)

Sk=JHPk∣k−1HkT+RkS_k = J_H P_{k|k-1} H^T_k + R_kSk=JHPk∣k−1HkT+Rk

Kk=Pk∣k−1JHTSk−1K_k = P_{k|k-1}J^T_H S_k^{-1}Kk=Pk∣k−1JHTSk−1

x^k∣k=x^k∣k−1+Kky^\hat x_{k|k} = \hat x_{k|k-1} + K_k \hat yx^k∣k=x^k∣k−1+Kky^

Pk∣k=(I−KkJH)Pk∣k−1P_{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 关于无迹变换

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

1.原高斯分布中按一定规则采样	2.采样点经过非线性变换	3.加权统计变换结果

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

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

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

关于权重
总和为1

w[i]={kk+ni=112(k+n)i=2,...,2n+1w^{[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)ii=2,...,n+1μ−((n+k)P)i−ni=n+2,...,2n+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

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

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

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

4.2.2 采样点经过非线性变换

对采样点的每一列进行非线性变换，比如在CTRV运动模型中，使用xk+1=xk+vkw[sin(wΔt+θ)−sin(θ)]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=12n+1w[i]χ′[i]\mu' = \sum^{2n+1}_{i=1}{w^{[i]} \chi^{'[i]}}μ′=i=1∑2n+1w[i]χ′[i]

P′=∑i=12n+1w[i](χ′[i]−μ′)(χ′[i]−μ′)TP'= \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运动模型中，一般会加入过程噪声aa,aw=0a_a,a_w = 0aa,aw=0。

χk−1=(μk−1μk−1+((n+k)Pk−1)iμk−1−((n+k)Pk−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和加权方差Pk∣k−1P_{k|k-1}Pk∣k−1。

μk∣k−1=∑i=12n+1w[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)

Pk∣k−1=∑i=12n+1w[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)Pk∣k−1)iμk∣k−1−((n+k)Pk∣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)

Zk∣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)

zk∣k−1=∑i=12n+1w[i]Zk∣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)

Sk∣k−1=∑i=12n+1w[i](Zk∣k−1[i]−zk∣k−1)(Zk∣k−1[i]−zk∣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在状态空间和测量空间的互相关函数(矩阵)
Tk∣k−1=∑i=12n+1w[i](χk∣k−1[i]−μk∣k−1)(Zk∣k−1[i]−zk∣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)
计算卡尔曼增益
Kk=Tk∣k−1Sk∣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)
更新状态估计
其中zk+1z_{k+1}zk+1是当前的观测量，zk∣k−1z_{k|k-1}zk∣k−1是预测映射在观测空间的伪观测量
μk=μk∣k−1+Kk(zk+1−zk∣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)
更新状态协方差矩阵
Pk=Pk∣k−1+KkSk∣k−1KkT(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