自动驾驶中的传感器融合算法：卡尔曼滤波器和扩展卡尔曼滤波器
附赠自动驾驶学习资料和量产经验：链接
介绍：
追踪静止和移动的目标是自动驾驶技术领域最为需要的核心技术之一。来源于多种传感器的信号，包括摄像头，雷达，以及激光雷达（基于脉冲激光的测距设备）等传感器组合的组合体来估计位置，速度，轨迹以及目标的种类，例如其他车辆和行人。详情请见：++Link++（原文中的链接是无效的因此我将原作者的文章连接替换了）
你可能会问——为什么我们需要这么多的传感器？
这是因为每种传感器提供了追踪物体所需要的不同精度和类型的信息，尤其是在不同天气条件下。比如，以激光雷达为基础的传感器能很好地解决位置的问题，但是在糟糕的天气条件下其精度和性能都会有很大程度的下降。另一方面，雷达的空间解决方案也相对便宜很多，与此同时，在恶劣的天气条件下也能提供更准确的数据。
同样的，不像激光雷达传感器，雷达可以提供目标的速度和方位。雷达数据也是计算密集型的，因为一束激光发射非常多包含每个独立的激光点的范围的数据，它使得你必须理解你的算法。
组合来自不同传感器信息的技术称之为传感器融合技术。之所以较早的讨论这个，是因为应用在传感器融合之上的算法必须处理短暂的，充满杂讯的输入，生成可靠的运动状态估计的概率。
该文章展示了在位置的追踪和估计中最通用的算法，卡尔曼滤波器的变种——‘扩展卡尔曼滤波器’。在进一步的文章中，我们会兼顾到其他技术比如无损卡尔曼滤波器和粒子滤波器。
1.使用激光雷达数据的基础卡尔曼滤波器:
卡尔曼滤波器的历史已经超过半个世纪,但是对于输入数据的噪声信息和状态估计的平滑来说仍然是最有效的传感器融合算法之一.它假定位置参数是符合高斯分布的,即完全可以被均值和协方差参数化:X∼N(μ, σ²)
当传感器的信息流开始的时候,卡尔曼滤波器使用一系列的状态信息来预测和衡量更新步骤去更新被追踪目标的信心值(概率).预测和更细心的步骤如下图所示:
状态预测:
我们会使用一个简化的线性状态空间模型(++Model Link++)去阐述滤波器的工作方式. 一个系统在t时刻的线性状态可以从t-1时刻根据以下等式被估计：
衡量更新:
卡尔曼滤波器的下一部分则是去使用实测参数z去更新预测状态’x’,通过缩放因子(通常称之为卡尔曼增益)成比例的计算估计值和测量值之间的误差.
你可以在以下链接中找到衡量更新等式的求导:++Derivation Link++
举个例子
理论部分结束！让我们尝试用一些代码去表示一些卡尔曼滤波器的基础过程。
对此，我们模拟一个目标，它的状态被四维向量x=[px py vx vy]所描述。
在这个例子中测量传感器是返回位置数据但是没有速度信息的激光传感器。为了观测到速度信息我们需要使用雷达传感器数据。在接下来的章节中，当我们讨论扩展卡尔曼滤波的时候我们将会涉及到这些。
让我们以一些假设开始：
伪代码：
基础版本的卡尔曼滤波器代码步骤列在了下面。你可以找到一个基础的例子：++code link++
"""prediction step"""
def predict(x, P):
x = (F * x) + u
P = F * P * F.transpose() #Acceleration noise Q is assumed to be zero
return x, P
"""measurement update step"""
def update(x, P,z):
# measurement update
Z = matrix([z])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P
最后的迭代步骤通过测量和应用预测以及滤波器的更新步骤如下所示：
plot_position_variance(x,P,edgecolor='r') #plot initial position and covariance in red
for z in measurements:
x,P = predict(x, P)
x,P = update(x, P,z)
plot_position_variance(x,P,edgecolor='b') #plot updates in blue
print(x)
print(P)
卡尔曼滤波器迭代：滤波器在迭代之后向真实值收敛
下方的图阐述了滤波器在每次迭代中状态向量的px,py维度和位置的协方差发生了哪些变化。红圈表示初始过程不确定性。随着预测和测量更新，我们开始有了较小的误差（半径），状态估计开始越来越精确。
如同你看到的，最终估计的状态向量x[11.99,2.05]非常接近最终的观测值，误差最小值也缩小到了0.05。
2.扩展卡尔曼滤波器——使用雷达数据
雷达数据带来了一些更困难的挑战。雷达返回数据值的是基于极坐标系，其由三个部分构成:
- ρ /Range(从原点到此的距离)
- ϕ / bearing ( ρ 和 x的夹角),
- ρ˙：接近率/距离变化率
由于没有H矩阵将状态向量映射到雷达的测量空间，我们需要一个函数h（x）来将状态空间银蛇到测量空间以测量更新步骤。这个方法是映射极坐标到笛卡尔坐标的方法的衍生方法，定义如下：
这个映射阐述了一个非线性函数，它将使得卡尔曼滤波器的过程和测量符合高斯分布的假设无效。扩展卡尔曼滤波器使用局部线性模型来逼近非线性模型，然后使用卡尔曼滤波应用到逼近值上。局部线性逼近是通过计算当前状态估计的一阶泰勒展开得出的。一阶的逼近也叫雅克比矩阵。我们不会在这里过多介绍雅克比矩阵的推到过程。相关内容在网络中可以找到优秀的介绍，但是如果你想要直接使用这些东西，你可以在以下的github引用中找到代码实例：++Github Link++
实现参考
你可以在github存储库中找到关于Kalman过滤器的c++编写代码:
代码示例：++code link++
总结
到此我们涉及到了一些目标追踪领域传感器融合的基本算法。在下一个阶段，将着眼于无损卡尔曼滤波，它将解决投影逼近值的问题。我们也会看看日益流行的基于蒙特卡洛数值积分的粒子滤波器。
