扩展卡尔曼滤波EKF

百度首页

Udacity的自动驾驶汽车

我们可以在任何有关某些动态系统的不确定信息的地方使用卡尔曼滤波器，并且可以对系统下一步要做什么做出有根据的猜测。

在这里，另一个有趣的事实：

在20世纪60年代，卡尔曼滤波器被应用于阿波罗计划的导航，该计划需要估计载人航天器到月球和返回的路径。今天它也帮助商业航空公司飞行员从纽约到达西雅图，而不是在圣地亚哥。

本文详细介绍卡尔曼滤波器和扩展卡尔曼滤波器算法。

什么是卡尔曼滤波器？

卡尔曼滤波器r是用于从间接和不确定测量估计系统状态的最佳估计算法。卡尔曼滤波器仅针对线性系统定义。如果有非线性系统并且想要估计系统状态，则需要使用非线性状态估计器。

卡尔曼滤波器广泛用于使用激光雷达和雷达传感器估算汽车位置。这些传感器的测量不准确，因为它们会漂移或产生噪音。卡尔曼滤波器可用于融合这些传感器的测量值，以找到精确位置的最佳估计。

卡尔曼滤波周期

卡尔曼滤波器代表我们的高斯分布，并迭代两个主要周期：预测和测量更新。

在深入了解卡尔曼滤波器算法流程的细节之前，考虑以下基于使用卡尔曼滤波器的预测结果和测量结果来估计汽车位置的示例。我们可以在概率密度函数的帮助下解释卡尔曼滤波器的工作原理，如下所示。

在初始时间步骤k-1，汽车位置可以处于x（k- 1）位置处的高斯分布的平均位置。在下一个时间步骤k，x（k）位置的不确定性增加，如下所示，方差更大。

下一个是来自传感器的测量结果y和在第三高斯分布的方差中表示的噪声，如下所示。

卡尔曼滤波器表示估计位置的最佳方法是将这两个结果结合起来。这是通过将这两个概率函数相乘而得到的，结果也将是另一个高斯函数。

让我们进入卡尔曼滤波器的两个周期：预测和测量更新。

预测

假设我们知道对象的当前位置和速度，我们将其保存在x变量中。现在一秒过去了。我们可以预测物体在一秒钟之后的位置，因为我们知道一秒钟前的物体位置和速度; 我们只是假设物体保持相同的速度。

但也许对象没有保持完全相同的速度。也许物体改变方向，加速或减速。因此，当我们在一秒钟后预测位置时，我们的不确定性会增加。

x是平均状态向量。对于扩展卡尔曼滤波器，平均状态向量包含有关正在跟踪的对象位置和速度的信息。它被称为"平均"状态向量，因为位置和速度由具有平均值x的高斯分布表示。

P是状态协方差矩阵，其包含关于对象的位置和速度的不确定性的信息。您可以将其视为包含标准偏差。

Q是过程协方差矩阵。它是与状态中的噪声相关联的协方差矩阵。我没有在这里指定计算矩阵的公式。

F是转换矩阵（处理时间步长和恒定速度的矩阵）

测量更新

我们将从两种测量（传感器）中获取数据：

激光测量雷达测量激光测量允许我们获取物体的当前位置而不是其速度。另一方面，激光测量具有更高的分辨率。

如上所示，这是用于找到x和P的等式。测量和状态向量表示如下所示。

z是测量矢量。对于激光雷达传感器，该载体包含位置 - X和位置 - 测量。

H是将您对物体当前状态的信念投射到传感器测量空间的矩阵。对于激光雷达，这是一种奇特的说法，我们从状态变量中丢弃速度信息，因为激光雷达传感器只测量位置：状态向量x包含有关[ px，py，vx，vy ]的信息，而zvector只包含[ px，py ]。乘以Hx允许我们将x（我们的信念）与z（传感器测量）进行比较。

R只是测量噪声的协方差矩阵。我没有在这里指定计算矩阵的公式。

雷达测量更进一步，我们也可以获得速度信息。雷达的测量更新部分将在扩展卡尔曼滤波器部分进行说明。

什么是扩展卡尔曼滤波器？

扩展卡尔曼滤波器（EKF）线性化围绕当前估计的均值的分布，然后在卡尔曼滤波器算法的预测和更新状态中使用该线性化。

我们的预测状态x由高斯分布表示，其被映射到非线性函数（雷达测量），然后得到的函数将不是高斯函数。