自动驾驶感知融合-卡尔曼及扩展卡尔曼滤波(Lidar&Radar)

Kalman Filter and Extended Kalman Filter

卡尔曼滤波是自动驾驶领域最常用的数据最优估计算法。人们对它的第一印象往往是它那复杂的线性代数表达式。本文将介绍卡尔曼滤波及其变种扩展卡尔曼滤波的数学原理及代码实现，帮助初学者熟悉和掌握卡尔曼滤波。在实际项目中我们会融合激光雷达和毫米波雷达的测量数据，从而精确地追踪目标的位置和速度。

Github: https://github.com/williamhyin/CarND-Extended-Kalman-Filter

Email: williamhyin@outlook.com

知乎专栏: 自动驾驶全栈工程师

Kalman Filter

1. 首先我们需要了解什么是卡尔曼滤波？

根据维基百科的官方解释，卡尔曼滤波(Kalman Filter)使用随时间推移观察到的一系列测量值（包含统计噪声和其他误差），生成未知变量的估计值，该估计值往往更多通过估计每个时间范围内变量的联合概率分布，结果因此比仅基于单个测量的结果更准确。

听起来稍微有点复杂，也就是说，卡尔曼滤波器是一种最优估计算法，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。

2. 其次我们要了解为什么需要卡尔曼滤波?

使用卡尔曼滤波的根本原因其实是来自传感器的测量结果不是100％可靠，如果毫无误差，或者误差远低于期望阈值，则完全不需要卡尔曼滤波，直接使用测量数据更新下一个状态即可。

在自动驾驶领域，卡尔曼滤波器广泛被用于通过特征级融合激光雷达和毫米波雷达的数据，估计汽车的位置和速度。 不同的传感器提取得到的测量结果如距离，速度，角度，因为受到信号漂移或噪声的影响，往是不准确的。而卡尔曼滤波为我们提供了一种将来自不同传感器的测量结果与预测位置的数学模型相结合的方法。它根据我们对一个特定传感器或模型的信任程度，来更新测量值和预估值之间的可信权重，从而获得对准确位置的最佳估计。

3. 卡尔曼滤波的步骤？

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计结果，做出对当前状态的估计。在更新阶段，滤波器利用当前状态的观测值优化在预测阶段获得的预估值，以获得一个当前阶段更精确的新估计值。

每当我们从传感器获取新数据时，我们都会重复这两个步骤，不断更新位置状态估计值和误差协方差矩阵P的准确性。

值得注意的是测量更新基于的依据是贝叶斯法则。当我们有了上一时刻的状态向量的先验概率P(x), 又有了测量值的似然P(z|x)，似然是指在当前位置下，我们能得到的测量值
，在P(z)为一个常数的情况下，我们求目标位置的概率分布其实就是在求后验概率P(x|z)。
$$P(\mathbf{x}|\mathbf{z})=\frac{P(\mathbf{z}|\mathbf{x})P(\mathbf{x})}{P(\mathbf{z})}$$
而预测更新是基于全概率公式。
$$P(B)=\sum _{i=1}^{n}P\left(A_i\right)P\left(B|A_i\right)$$

在下面的例子中，我们可以用在概率密度函数的帮助下解释卡尔曼滤波的工作原理。

在最初的时间步骤 k-1，目标的位置是平均位置为 x_k-1的高斯分布。 之后基于运动模型进行预测，得到时刻 k 的 目标位置x_k ，由于预测本身具有较大的不确定性，因此预测的位置结果具有较大的方差。与此同时我们获得来自传感器的测量结果 y ，测量值的不确定性较小，因此测量的位置结果具有较小的方差。卡尔曼滤波器通过将预测和测量两个概率函数相乘，来估计最佳位置。值得注意的是通过数学推导可以发现相乘的结果是一个分布紧密具有更小方差的高斯函数，意味着最终位置估计准确性得到了提高。

4. 卡尔曼滤波与隐马尔科夫模型的关系？

卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示。它是是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。

卡尔曼向量与矩阵

卡尔曼滤波器的在预测和测量步骤中相互传递的数值有以下两个：

• $$\hat{\mathbf{x}}:状态的估计$$

• $$\mathbf{P}：状态误差协方差矩阵，度量估计值的精确程度。$$

以下是维基百科中对于卡尔曼滤波各个向量与矩阵的数学描述，初看非常复杂，但其实熟悉之后就会发现，你只需要套公式。我会在后续章节中，详细讲解各个向量和矩阵的意义。在下文中Xk-1|k-1用x表示，Xk|k用x’ 表示，其他变量相似的表示有同样的含义。

预测

假设我们知道一个物体的当前位置和速度，我们把它保持在 x 变量中。 现在一秒钟已经过去了。因为我们一秒钟前就知道了物体的位置和速度，以及匀加速运动的加速度 a，那么我们可以在一秒钟后预测物体的位置。如果我们假设物体以同样的速度运动，则我们可以用等式计算预测值。

但也许物体并没有保持完全相同的速度。 也许物体改变了方向，加速或减速。 所以当我们一秒钟后预测位置时，我们的不确定性就增加了。

代表不确定性增加了。

我们来分别讨论各个变量的含义：

• x是平均状态向量，包含被t-1时刻被跟踪对象综合预测和测量得到的估计值，该估计值包括四个值：(px, py, vx, vy)，均基于笛卡尔坐标。x之所以被称为“mean state vector”，是因为位置和速度由均值为x的高斯分布表示。

• F是预测下一个状态值的状态变换矩阵，为此我们需要一个现实中的线性运动模型。

$$\{\begin{array}{l}{p}_x^{\prime }=p_x+v_x\Delta t+\frac{a_x\Delta t^2}{2}\\ p_y^{\prime }=p_y+v_y\Delta t+\frac{a_y\Delta t^2}{2}\\ v_x^{\prime }=v_x+a_x\Delta t\\ v_y^{\prime }=v_y+a_y\Delta t\end{array}$$

根据上述线性速度模型，我们得到下列状态转移方程：
$$\begin{gathered} \left(\begin{array}{c}{p}_x^{\prime }\\ p_y^{\prime }\\ v_x^{\prime }\\ v_y^{\prime }\end{array}\right)=\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)\left(\begin{array}{c}{p}_x\\ p_y\\ v_x\\ v_y\end{array}\right)+\left(\begin{array}{c}\frac{\Delta t^2}{2}\\ \frac{\Delta t^2}{2}\\ \Delta t\\ \Delta t\end{array}\right)\left(\begin{array}{c}{a}_x\\ a_y\\ a_x\\ a_y\end{array}\right) \\ {x}^{\prime }=Fx+Bu+w \end{gathered}$$
这里F是状态变换矩阵，x‘ 是基于t-1时刻估计状态x下理论预测的t时刻的状态，B被称为控制矩阵，u被称为控制变量，由于汽车的运动并不严格遵循匀加速运动，因此需要在等式后面加上w噪声项。
x ′