扩展卡尔曼滤波EKF EKF

发布时间:2019-07-22 12:08:32 来源:AI火箭营 关键词:EKF
EKF
原文标题:扩展卡尔曼滤波EKF
原文发布时间:2018-12-04 08:05:41
原文作者:AI火箭营。
如果您喜欢本文,请关注头条号【AI火箭营】阅读更多相关文章。
如果您是本文作者,不希望我们转载此文,请联系我们删除。
EKF

扩展卡尔曼滤波EKF

Udacity的自动驾驶汽车

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

在这里,另一个有趣的事实:

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

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

什么是卡尔曼滤波器?

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

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

卡尔曼滤波周期

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

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

扩展卡尔曼滤波EKF

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

扩展卡尔曼滤波EKF

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

扩展卡尔曼滤波EKF

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

扩展卡尔曼滤波EKF

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

预测

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

扩展卡尔曼滤波EKF

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

扩展卡尔曼滤波EKF

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

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

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

F转换矩阵(处理时间步长和恒定速度的矩阵)

测量更新

我们将从两种测量(传感器)中获取数据:

  • 激光测量
  • 雷达测量

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

扩展卡尔曼滤波EKF

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

扩展卡尔曼滤波EKF

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

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

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

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

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

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

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

扩展卡尔曼滤波EKF

因此卡尔曼滤波器不再适用,因为它仅适用于高斯函数。

扩展卡尔曼滤波EKF

其中一个解决方案是线性化h(x)函数,这是EKF的关键。

扩展卡尔曼滤波EKF

EKF 使用称为First Order Taylor Expansion的方法来线性化该函数。

虽然数学证明有些复杂,但事实证明卡尔曼滤波方程和EKF 方程非常相似。

扩展卡尔曼滤波EKF

主要区别是:

  • 在计算P ' 时,F矩阵将被Fj代替。
  • 当计算S,K和P时,卡尔曼滤波器中的H矩阵将被雅可比矩阵Hj代替。
  • 为了计算x ',使用预测更新函数f代替F矩阵。
  • 为了计算y,使用h函数代替H矩阵。

EKF有一些缺点:

  • 如果需要通过分析方法找到雅可比人,则很难计算雅各比。
  • 如果雅各比人在数字上发现,则计算成本很高。
  • EKF仅适用于具有可区分模型的系统。

回到我们之前解释过的测量更新部分。由于雷达测量是非线性的,我们需要在测量更新周期中使用EKF ,但我们可以在预测周期中使用相同的公式。

来自激光测量的H矩阵和来自雷达测量的h(x)方程实际上完成了相同的事情; 它们都需要在更新步骤中解决y = z - Hx '。

但对于雷达,没有H矩阵将状态向量x映射到极坐标; 相反,您需要手动计算映射以从笛卡尔坐标转换为极坐标。

这是h函数,它指定预测位置和速度如何映射到范围,方位和范围速率的极坐标。

扩展卡尔曼滤波EKF

因此,对于雷达y = z - Hx '变为y = z - h(x ')。

扩展卡尔曼滤波EKF

范围(ρ)是到行人的距离。该范围基本上是位置矢量ρ的大小,其可以定义为ρ= sqrt(px2 + py2)。

φ= ATAN(PY /像素)。请注意,φ是从x轴逆时针方向参考的,因此在该情况下,来自上方视频剪辑的φ实际上是负的。

距离率ρ˙是速度v在线上的投影。

这里是的自动驾驶汽车纳米级的EKF 项目的高级架构。

扩展卡尔曼滤波EKF


正文完,原文标题:扩展卡尔曼滤波EKF
原文发布时间:2018-12-04 08:05:41
原文作者:AI火箭营。

EKF EKF




本文关键词:EKF
猜你喜欢