【自动驾驶】学习卡尔曼滤波（一）——线性卡尔曼滤波
文章目录
- 参考资料
- 1. 基本概念
-
- 1.1 先验概率和后验概率
- 1.2 贝叶斯公式
- 2. 卡尔曼滤波
-
- 2.1 卡尔曼滤波主要公式
- 2.2 已知的前提假设
- 2.3 预测
- 2.4 更新
- 2.5 推导
- 3. 卡尔曼滤波在无人驾驶汽车感知模块的应用
-
- 3.1 无人车感知模块的传感器
- 3.2 基于卡尔曼滤波的行人位置估算
参考资料
- 如何使用卡尔曼滤波（Kalman Filtering）实现对物体运动轨迹的预测？
- 卡尔曼滤波与目标追踪
- 卡尔曼滤波算法原理
文章大部分内容是根据参考资料整理所得，旨在于更方便学习记忆查找，不作其他用途。
1. 基本概念
- 卡尔曼滤波（Kalman filtering, KF）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
- 卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。在很多工程应用（雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题。
- 为了解决普通KF仅能处理线性数据的问题，从而就有了用于解决非线性数据的基于KF的推展：拓展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）。KF主要应用于系统方程和量测方程为线性的的场景，EKF应用于轻度非线性场景，而UKF则在强非线性情况下有更好的表现。
1.1 先验概率和后验概率
-
先验概率： 根据以往经验和分析得到的概率。也可以认为是无条件概率。
其中，先验信息一般来源于经验跟历史资料。比如林丹跟某选手对决，解说一般会根据林丹历次比赛的成绩对此次比赛的胜负做个大致的判断。再比如，某工厂每天都要对产品进行质检，以评估产品的不合格率θ，经过一段时间后便会积累大量的历史资料，这些历史资料便是先验知识，有了这些先验知识，便在决定对一个产品是否需要每天质检时便有了依据，如果以往的历史资料显示，某产品的不合格率只有0.01%，便可视为信得过产品或免检产品，只每月抽检一两次，从而省去大量的人力物力。
-
后验概率：后验概率是基于新的信息，修正原来的先验概率后所获得的更接近实际情况的概率估计。直观意义上后验概率就是条件概率。
1.2 贝叶斯公式
2. 卡尔曼滤波
该部分主要来自知乎文章。
2.1 卡尔曼滤波主要公式
或者如下面的文献材料中通常意义的卡尔曼滤波过程：
上面两种形式只是符号不一样。
2.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_{k} u_{k}+w_{k} xk=Fkxk−1+Bkuk+wk
其中： F k F_{k} Fk为状态变换矩阵，即状态转移矩阵。 B k B_{k} Bk是作用在控制器向量 u k u_{k} uk上的输入-控制矩阵。 B k u k B_{k} u_{k} Bkuk这一项是指我们在追踪一个物体的状态的时候把它内部的控制也考虑进去了，一般运动学中没有这项，因为对于检测的目标是无法测量其内部的控制量，所以简化为0。 w k ∼ N ( 0 , Q k ) w_{k} \sim N\left(0, Q_{k}\right) wk∼N(0,Qk)是过程噪声，均值为0， Q k Q_{k} Qk为过程噪声协方差矩阵。 -
状态估计转移方程
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 v_{k} vk是测量噪声（观测噪声）。 初始状态以及每一时刻的噪声（过程噪声与测量噪声）都认为是互相独立的，即 x 0 , w 1 , … , w k , v 1 … v k x_{0}, w_{1}, \ldots, w_{k}, v 1 \ldots v_{k} x0,w1,…,wk,v1…vk是互相独立的。
2.3 预测
第一个公式为状态估计方程，用来预测下一步状态（故 F k F_{k} Fk也叫预测矩阵）。其中： x ^ k ∣ k − 1 \hat{x}_{k \mid k-1} x^k∣k−1为k-1时刻对k时刻的状态预测。 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1 \mid k-1} x^k−1∣k−1在时刻k-1的状态估计。
第二个公式为预测估计协方差方程，计算先验概率。其中： P k − 1 ∣ k − 1 P_{k-1 \mid k-1} Pk−1∣k−1为k-1时刻的后验估计误差协方差矩阵，度量估计值的精确程度。 P k ∣ k − 1 P_{k \mid k-1} Pk∣k−1为k-1时刻到k时刻的估计误差协方差矩阵。 Q k Q_{k} Qk为过程噪声协方差矩阵，越大说明越不相信预测。
2.4 更新
-
第一个公式为卡尔曼增益方程。 K k K_k Kk为最优卡尔曼增益。 R k R_k Rk为测量噪声协方差矩阵，越大说明越不相信观测。实际工程中测量噪声由厂商提供，也可以自己调参，也可以自适应（AKF）。
-
第二个公式在更新状态估计方程，也就是最终的滤波状态结果。 可以使用测量残差来简化表达公式
y ^ = z k − H k x ^ k ∣ k − 1 S k = cov ( y ^ ) = H k P k ∣ k − 1 H k T + R k \hat{y}=z_{k}-H_{k} \hat{x}_{k \mid k-1}\\ S_{k}=\operatorname{cov}(\hat{y})=H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k} y^=zk−Hkx^k∣k−1Sk=cov(y^)=HkPk∣k−1HkT+Rk
所以第1，2个公式可以简化为：
K k = P k ∣ k − 1 H k T S k − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ K_{k}=P_{k \mid k-1} H_{k}^{T} S_{k}^{-1}\\ \hat{x}_{k \mid k}=\hat{x}_{k \mid k-1}+K_{k} \hat{y} Kk=Pk∣k−1HkTSk−1x^k∣k=x^k∣k−1+Kky^
-
第三个公式在更新估计协方差方程，计算得到后验概率。
所以简化后的更新部分的公式有5个：
2.5 推导
以上部分已经能够知道卡尔曼滤波在干嘛了，但要想知其然且知其所以然，那还得进行公式推导，这样也更有利于记忆。
已知:
Q k = cov ( w k ) = E ( w k w k T ) R k = cov ( v k ) = E ( v k v k T ) P k ∣ k − 1 = cov ( x k − x ^ k ∣ k − 1 ) = E ( ( x k − x ^ k ∣ k − 1 ) ( x k − x ^ k ∣ k − 1 ) T ) P k ∣ k = cov ( x k − x ^ k ∣ k ) = E ( ( x k − x ^ k ∣ k ) ( x k − x ^ k ∣ k ) T ) S k = cov ( y ^ ) = cov ( z k − H k x ^ k ∣ k − 1 ) (1) \begin{aligned} \tag{1} &Q_{k}=\operatorname{cov}\left(w_{k}\right)=E\left(w_{k} w_{k}^{T}\right) \quad \\ &R_{k}=\operatorname{cov}\left(v_{k}\right)=E\left(v_{k} v_{k}^{T}\right) \\ &P_{k \mid k-1}=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}\right)=E\left(\left(x_{k}-\hat{x}_{k \mid k-1}\right)\left(x_{k}-\hat{x}_{k \mid k-1}\right)^{T}\right) \\ &P_{k \mid k}=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k}\right)=E\left(\left(x_{k}-\hat{x}_{k \mid k}\right)\left(x_{k}-\hat{x}_{k \mid k}\right)^{T}\right) \\ &S_{k}=\operatorname{cov}(\hat{y})=\operatorname{cov}\left(z_{k}-H_{k} \hat{x}_{k \mid k-1}\right) \end{aligned} Qk=cov(wk)=E(wkwkT)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)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)(1)
协方差矩阵是对称半正定矩阵。
-
从状态估计方程推 P k ∣ k − 1 P_{k \mid k-1} Pk∣k−1 :
P k ∣ k − 1 = cov ( x k − x ^ k ∣ k − 1 ) = cov ( F k x k − 1 + w k − F k x ^ k − 1 ∣ k − 1 ) = cov ( F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) ) + cov ( w k ) = F k cov ( x k − 1 − x ^ k − 1 ∣ k − 1 ) F k T + Q k = F k P k − 1 ∣ k − 1 F k T + Q k (2) \begin{aligned} \tag{2} P_{k \mid k-1} &=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}\right) \quad\\ &=\operatorname{cov}\left(F_{k} x_{k-1}+w_{k}-F_{k} \hat{x}_{k-1 \mid k-1}\right) \\ &=\operatorname{cov}\left(F_{k}\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)\right)+\operatorname{cov}\left(w_{k}\right) \\ &=F_{k} \operatorname{cov}\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right) F_{k}^{T}+Q_{k} \quad\\ &=F_{k} P_{k-1 \mid k-1} F_{k}^{T}+Q_{k} \end{aligned} Pk∣k−1=cov(xk−x^k∣k−1)=cov(Fkxk−1+wk−Fkx^k−1∣k−1)=cov(Fk(xk−1−x^k−1∣k−1))+cov(wk)=Fkcov(xk−1−x^k−1∣k−1)FkT+Qk=FkPk−1∣k−1FkT+Qk(2) -
从状态估计更新方程推 P k ∣ k P_{k \mid k} Pk∣k :
P k ∣ k = cov ( x k − x ^ k ∣ k ) = cov ( x k − x ^ k ∣ k − 1 − K k ( z k − H k x ^ k ∣ k − 1 ) ) = cov ( x k − x ^ k ∣ k − 1 − K k ( H k x k + v k − H k x ^ k ∣ k − 1 ) ) = cov ( x k − x ^ k ∣ k − 1 − K k H k ( x k − x ^ k ∣ k − 1 ) − K k v k ) = cov ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k ) = cov ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) ) + cov ( K k v k ) = ( I − K k H k ) cov ( x k − x ^ k ∣ k − 1 ) ( I − K k H k ) T + K k cov ( v k ) K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T (3) \begin{aligned} \tag{3} P_{k \mid k} &=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k}\right) \quad\\ &=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}-K_{k}\left(z_{k}-H_{k} \hat{x}_{k \mid k-1}\right)\right) \\ &=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}-K_{k}\left(H_{k} x_{k}+v_{k}-H_{k} \hat{x}_{k \mid k-1}\right)\right) \\ &=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}-K_{k} H_{k}\left(x_{k}-\hat{x}_{k \mid k-1}\right)-K_{k} v_{k}\right) \\ &=\operatorname{cov}\left(\left(I-K_{k} H_{k}\right)\left(x_{k}-\hat{x}_{k \mid k-1}\right)-K_{k} v_{k}\right) \\ &=\operatorname{cov}\left(\left(I-K_{k} H_{k}\right)\left(x_{k}-\hat{x}_{k \mid k-1}\right)\right)+\operatorname{cov}\left(K_{k} v_{k}\right) \\ &=\left(I-K_{k} H_{k}\right) \operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k-1}\right)\left(I-K_{k} H_{k}\right)^{T}+K_{k} \operatorname{cov}\left(v_{k}\right) K_{k}^{T} \\ &=\left(I-K_{k} H_{k}\right) P_{k \mid k-1}\left(I-K_{k} H_{k}\right)^{T}+K_{k} R_{k} K_{k}^{T} \end{aligned} Pk∣k=cov(xk−x^k∣k)=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))=cov(xk−x^k∣k−1−KkHk(xk−x^k∣k−1)−Kkvk)=cov((I−KkHk)(xk−x^k∣k−1)−Kkvk)=cov((I−KkHk)(xk−x^k∣k−1))+cov(Kkvk)=(I−KkHk)cov(xk−x^k∣k−1)(I−KkHk)T+Kkcov(vk)KkT=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT(3)
这一公式对于任何卡尔曼增益 K k K_{k} Kk 都成立。如果 K k K_{k} Kk 是最优卡尔曼增益，则可以进一步简化 -
基于上式推导最优卡尔曼增益 K k K_{k} Kk :
卡尔曼滤波器是最小均方误差估计器，后验状态误差估计是指 P k ∣ k = cov ( x k − x ^ k ∣ k ) P_{k \mid k}=\operatorname{cov}\left(x_{k}-\hat{x}_{k \mid k}\right) Pk∣k=cov(xk−x^k∣k) ，当 P k ∣ k P_{k \mid k} Pk∣k 矩阵的均方误差为最小时，即可求出最优卡尔曼增益。
矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差，即是求矩阵的迹的最小值，对矩阵的迹求导，导数为 0 时，迹最小。协方差 P k ∣ k P_{k \mid 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 − 1 − K k H k P k ∣ k − 1 ) ( I − ( K k H k ) T ) + K k R k K k T = 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 = 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 (4) \begin{aligned} \tag{4} P_{k \mid k} &=\left(I-K_{k} H_{k}\right) P_{k \mid k-1}\left(I-K_{k} H_{k}\right)^{T}+K_{k} R_{k} K_{k}^{T} \\ &=\left(P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}\right)\left(I-\left(K_{k} H_{k}\right)^{T}\right)+K_{k} R_{k} K_{k}^{T} \\ &=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k} H_{k} P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k} R_{k} K_{k}^{T} \\ &=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T} \\ \end{aligned} Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT=(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=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT(4)
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 k T + R k ) K k T ) = 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 k T + R k ) K k T ) = tr ( P k ∣ k − 1 ) − 2 tr ( K k H k P k ∣ k − 1 ) + tr ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) (5) \begin{aligned} \operatorname{tr}\left(P_{k \mid k}\right)&=\operatorname{tr}\left(P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T}\right) \\ &=\operatorname{tr}\left(P_{k \mid k-1}\right)-\operatorname{tr}\left(K_{k} H_{k} P_{k \mid k-1}\right)-\operatorname{tr}\left(P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}\right)+\operatorname{tr}\left(K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T}\right) \\ &=\operatorname{tr}\left(P_{k \mid k-1}\right)-2 \operatorname{tr}\left(K_{k} H_{k} P_{k \mid k-1}\right)+\operatorname{tr}\left(K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T}\right) \\ \end{aligned} \tag{5} 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)=tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)(5)d tr ( P k ∣ k ) d K k = d [ tr ( P k ∣ k − 1 ) − 2 tr ( K k H k P k ∣ k − 1 ) + tr ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) ] 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 ) (6) \begin{aligned} \tag{6} \frac{d \operatorname{tr}\left(P_{k \mid k}\right)}{d K_{k}}&=\frac{d\left[\operatorname{tr}\left(P_{k \mid k-1}\right)-2 \operatorname{tr}\left(K_{k} H_{k} P_{k \mid k-1}\right)+\operatorname{tr}\left(K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T}\right)\right]}{d K_{k}} \\ &=-2\left(H_{k} P_{k \mid k-1}\right)^{T}+2 K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) \end{aligned} dKkdtr(Pk∣k)=dKkd[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)(6)
当矩阵导数是 0 的时候得到 P k ∣ k P_{k \mid k} Pk∣k 的迹 (trace) 的最小值:
tr ( 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{\operatorname{tr}\left(P_{k \mid k}\right)}{d K_{k}}=-2\left(H_{k} P_{k \mid k-1}\right)^{T}+2 K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right)=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 (7) \tag{7} K_{k}=P_{k \mid k-1} H_{k}^{T}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right)^{-1}=P_{k \mid k-1} H_{k}^{T} S_{k}^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=Pk∣k−1HkTSk−1(7)
其中
S k = H k P k ∣ k − 1 H k T + R k S_{k} = H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k} Sk=HkPk∣k−1HkT+Rk -
使用最优卡尔曼增益化简 P k ∣ k P_{k \mid k} Pk∣k
由公式(7)
K k S k = ( H k P k ∣ k − 1 ) T K_{k} S_{k}=\left(H_{k} P_{k \mid k-1}\right)^{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_{k}^{T}=\left(H_{k} P_{k \mid k-1}\right)^{T} K_{k}^{T}=P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T} KkSkKkT=(HkPk∣k−1)TKkT=Pk∣k−1(KkHk)T故公式(4)可化为
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 − 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 = 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 = P k ∣ k − 1 − K k H k P k ∣ k − 1 = ( I − K k H k ) P k ∣ k − 1 \begin{aligned} &\\ P_{k \mid k}&\quad=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k}\left(H_{k} P_{k \mid k-1} H_{k}^{T}+R_{k}\right) K_{k}^{T} \\ &\quad=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+K_{k} S_{k} K_{k}^{T} \\ &\quad=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1}-P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T}+P_{k \mid k-1}\left(K_{k} H_{k}\right)^{T} \\ &\quad=P_{k \mid k-1}-K_{k} H_{k} P_{k \mid k-1} \quad\\ &\quad=\left(I-K_{k} H_{k}\right) P_{k \mid k-1} \end{aligned} Pk∣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=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Pk∣k−1(KkHk)T=Pk∣k−1−KkHkPk∣k−1=(I−KkHk)Pk∣k−1
该简化式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k \mid k}=\left(I-K_{k} H_{k}\right) P_{k \mid 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 \mid k}=\left(I-K_{k} H_{k}\right) P_{k \mid k-1}\left(I-K_{k} H_{k}\right)^{T}+K_{k} R_{k} K_{k}^{T} Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT，即上文的公式(4) 。
3. 卡尔曼滤波在无人驾驶汽车感知模块的应用
该部分主要来自博客。
3.1 无人车感知模块的传感器
无人驾驶汽车上的传感器能够多达几十个，而且是不同种类的，比如：
-
立体摄像机
立体摄像机往往用于获取图像和距离信息；
-
交通标志摄像机
交通标志摄像机用于交通标志的识别；
-
雷达（RADAR）
雷达一般安装在车辆的前保险杆里面，用于测量相对于车辆坐标系下的物体，可以用来定位，测距，测速等等，容易受强反射物体的干扰，通常不用于静止物体的检测；
-
激光雷达（LIDAR）
激光雷达往往安装在车顶，使用红外激光束来获得物体的距离和位置，空间分辨率高，但是笨重，容易被天气影响。
由此可知，各种传感器都有其优点和缺点，在实际的无人驾驶汽车里，我们往往结合多种传感器的数据去感知我们的车辆周边的环境。这种结合各种传感器的测量数据的过程为传感器融合（Sensor Fusion）。
3.2 基于卡尔曼滤波的行人位置估算
当我们要估计一个行人的状态时，首先就是建立要估计的状态的表示。这里，行人的状态大致可以表示为 x = ( p , v ) x=(p, v) x=(p,v)， 其中 p p p 为行人的位置，而 v v v 则是行人此时的速度。我们用一个向量来表示一个状态：
x = ( p x , p y , v x , v y ) T x = (p_x, p_y, v_x, v_y)^T x=(px,py,vx,vy)T
在确定了我们要估计的状态以后，接下来确定过程模型（即状态方程），过程模型用于在预测阶段来产生一个对当前估计的先验。我们先以一个最简单的过程模型来描述行人的运动——恒定速度模型：
x k + 1 = F x k + ω k x_{k+1} = Fx_k+\omega_k xk+1=Fxk+ωk
即
x k + 1 = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] ⋅ [ p x p y v x v y ] k + ω k x_{k+1}=\left[\begin{array}{cccc} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{c} p_{x} \\ p_{y} \\ v_{x} \\ v_{y} \end{array}\right]_{k}+\omega_k xk+1=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⋅⎣⎢⎢⎡pxpyvxvy⎦⎥⎥⎤k+ωk
恒定速度过程模型假定预测的目标的运动规律是恒定的速度的，在行人状态预测这个问题中，很显然行人并不一定会以恒定的速度运动，所以我们的过程模型包含了一定的过程噪声，在这个问题中过程噪声也被考虑了进来，其中的 ω k \omega_k ωk是这个过程模型的过程噪声。在行人状态估计中的过程噪声其实就是行人的加减速，那么我们原来的处理过程就变成了：
x k + 1 = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] ⋅ [ p x p y v x v y ] k + [ 1 2 a x Δ t 2 1 2 a y Δ t 2 a x Δ t a y Δ t ] k x_{k+1}=\left[\begin{array}{cccc} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{c} p_{x} \\ p_{y} \\ v_{x} \\ v_{y} \end{array}\right]_{k}+\left[\begin{array}{c} \frac{1}{2} a_{x} \Delta t^{2} \\ \frac{1}{2} a_{y} \Delta t^{2} \\ a_{x} \Delta t \\ a_{y} \Delta t \end{array}\right]_{k} xk+1=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⋅⎣⎢⎢⎡pxpyvxvy⎦⎥⎥⎤k+⎣⎢⎢⎡21axΔt221ayΔt2axΔtayΔt⎦⎥⎥⎤k
我们预测的第二步就变成了:
P k + 1 = F P k F T + Q k P_{k+1}=F P_k F^{T}+Q_k Pk+1=FPkFT+Qk
P P P是我们的估计状态概率分布的协方差矩阵。 Q k Q_k Qk 是过程噪声的协方差矩阵，由于过程噪声是随机带入的，所以 ω k \omega_k ωk 本质是一个高斯分布： ω k ∼ N ( 0 , Q k ) \omega_k \thicksim N(0, Q_k) ωk∼N(0,Qk)， Q k Q_k Qk的形式为：
Q = [ σ p x 2 σ p x p y σ p x v x σ p x v y σ p y p x σ p y 2 σ p y v x σ p y v y σ v x p x σ v x p y σ v x 2 σ v x v y σ v y p x σ v y p y σ v y v x σ v y 2 ] Q=\left[\begin{array}{cccc} \sigma_{p_{x}}^{2} & \sigma_{p_{x} p_{y}} & \sigma_{p_{x} v_{x}} & \sigma_{p_{x} v_{y}} \\ \sigma_{p_{y} p_{x}} & \sigma_{p_{y}}^{2} & \sigma_{p_{y} v_{x}} & \sigma_{p_{y} v_{y}} \\ \sigma_{v_{x} p_{x}} & \sigma_{v_{x} p_{y}} & \sigma_{v_{x}}^{2} & \sigma_{v_{x} v_{y}} \\ \sigma_{v_{y} p_{x}} & \sigma_{v_{y} p_{y}} & \sigma_{v_{y} v_{x}} & \sigma_{v_{y}}^{2} \end{array}\right] Q=⎣⎢⎢⎡σpx2σpypxσvxpxσvypxσpxpyσpy2σvxpyσvypyσpxvxσpyvxσvx2σvyvxσpxvyσpyvyσvxvyσvy2⎦⎥⎥⎤
进而表示为:
Q = G ⋅ G T ⋅ σ v 2 Q=G \cdot G^{T} \cdot \sigma_{v}^{2} Q=G⋅GT⋅σv2
其中 G = [ 0.5 Δ t 2 , 0.5 Δ t 2 , Δ t , Δ t ] T , σ v 2 G=\left[0.5 \Delta t^{2}, 0.5 \Delta t^{2}, \Delta t, \Delta t\right]^{T}, \sigma_{v}^{2} G=[0.5Δt2,0.5Δt2,Δt,Δt]T,σv2 对于行人我们可以设置为 0.5 m / s 2 0.5 \mathrm{~m} / \mathrm{s}^{2} 0.5 m/s2
测量步骤中，我们直接可以测量到速度 v x v_{x} vx 和 v y v_{y} vy ，所以我们的测量矩阵 H H H 可以表示为:
H k = [ 0 0 1 0 0 0 0 1 ] H_k=\left[\begin{array}{llll} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] Hk=[00001001]
测量噪声的协方差矩阵 R R R 为:
R k = [ σ v x 2 0 0 σ v y 2 ] R_k=\left[\begin{array}{cc} \sigma_{v_{x}}^{2} & 0 \\ 0 & \sigma_{v_{y}}^{2} \end{array}\right] Rk=[σvx200σvy2]
其中的 σ v x 2 \sigma^2_{v_x} σvx2和 σ v x 2 \sigma_{v_{x}}^{2} σvx2 描述了我们的传感器的测量能够有多“差”，这是传感器固有的性质，所以往往是传感器厂商提供。
最后，在测量的最后一步:
P k ← ( I − K k H k ) P k P_{k} \leftarrow\left(I-K_{k} H_k\right) P_{k} Pk←(I−KkHk)Pk
相应的python代码实现见于GitHub仓库。