自动驾驶中扩展卡尔曼滤波器的应用（Extended Kalman Filter）

卡尔曼滤波（Kalman
filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

自动驾驶中的卡尔曼滤波

例如使用卡尔曼滤波器来预测前方的车辆将采取的下一组动作，预测汽车周围的其他汽车或者自行车的位置。是一个迭代的过程，通过对于前一个状态的预测值，加上实际的测试值，来预测下一个状态的状态值。
在这里边的先验值，测试值，预测值均符合正态分布。使用两个正态分布的相关量可以预测出第三个相关量，也是符合正态分布。


使用一个状态的值预测下一个状态的值

卡尔曼滤波器的原理

卡尔曼滤波器是一个迭代的过程，主要是两个阶段，第一个阶段是预测，第二个阶段是更新。

预测

在该步骤中，卡尔曼滤波器根据初始值 预测新值，然后根据系统中存在的各种过程噪声预测我们预测中的不确定性/误差/方差。

在自动驾驶过程中，可以通过采用我们车辆前方移动的汽车的简单示例来理解噪音。我们的模型将假设汽车由于零加速而以恒定速度移动，但实际上它将具有加速度，即速度将不时波动。这种汽车加速度的这种变化是不确定性/误差/方差，我们使用过程噪声将其引入我们的系统。

更新

在此步骤中，我们从系统的设备中获取实际测量值。在自动驾驶车辆的情况下，这些设备可以是雷达或激光雷达。然后我们计算预测值和测量值之间的差值，然后通过计算卡尔曼增益来决定保持哪个值，即预测值或测量值。根据卡尔曼增益做出的决定，我们计算新值和新的不确定性/误差/方差。

卡尔曼滤波的公式

F–状态转移矩阵
H–状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
P–描述不确定性的协方差矩阵
u–运动向量
Z–测量值
R–测量噪声
I–单位矩阵

传感器融合

雷达可以探测到物体的速度信（矢量），激光雷达可以探测到物体的位置信息，即距离，因此我们想要将两种传感器融合，来准确的探测物体，从而预测物体的位置。
我们想要将两种数据融合起来，一起放入卡尔曼滤波器中。
以下是卡尔曼滤波器的流程;

• first measurement 滤波器在第一次运行的时候接受雷达和激光雷达的数据初始化自行车起始位置。
• initialize state and covariance matrices 该过滤器将基于所述第一次测量初始化自行车的位置。
• 然后，该车将一个时间段Δt之后接收另一组传感器数据。
• predict 该算法将预测时间Δt之后该自行车的位置。一种预测的方案是假设自行车在Δt内是匀速行驶的，所以自行车的速度V是一个常数，因此我们认为自行车走过的距离为V*Δt。在扩展卡尔曼滤波器中，我们认为速度是恒定的，但是在无迹卡尔曼滤波器中，我们会定义更加复杂的模型。
• update 滤波器将会比较自行车预测的位置和实际上测量到的位置，然后将两个数据综合起来，给出新的预测的位置。滤波器将会更具每个值的不确定度给予相应的权重。
• 然后再经过Δt的时间，传感器又会给出新的值，于是现在的预测值跟传感器的测量值结合，就又可以开始新一轮的预测。

卡尔曼滤波器工作流程：

x是平均状态向量。对于一个扩展卡尔曼滤波器来说，平均状态向量包含了你在追踪的物体的位置信息和速度。之所以被命名为“平均”状态向量，是因为位置和速度可以表示为均值为x的高斯分布。
p是状态协方差矩阵，它包含了物体位置和速度的不确定性信息。你可以理解为标准差。
k是时间间隔。因此，x​k表示物体在k时刻的位置和速度。
k+1∣k表示预测步骤，在k+1时刻，可获得传感器的采集数据。考虑传感器数据是为了获知新的物体位置和速度信息，在此之前，你需要预测k+1时刻物体的位置和速度。你可以根据k时刻物体的位置和速度信息来预测其k+1时刻的位置。因此，x​k+1∣k表示我们已经预测到k+1时刻物体的位置信息，但还没有用传感器进行测试。
xk+1表示我们已经预测到k+1时刻物体的位置信息，以及考虑到传感器信息后更新的物体位置和速度信息。

​​ 如果雷达和激光雷达的传感器数据在k+3时刻同时到达：

先预测xk+3，然后用任意一个传感器信息进行更新。得到新的新的xk+3之后，再使用另一个传感器信息进行更新。