热文推荐: MIT雾天识别系统 卡尔曼滤波器是传感器融合工程师用于自动驾驶汽车的工具。想象一下,你有一个雷达传感器,告诉你另一辆车距离15米,一个激光传感器说车辆距离20米。你如何协调这些传感器测量?这就是卡尔曼滤波器的功能。卡尔曼滤波在自动驾驶汽车上的应用十分广泛,笔者搜集并整理了几篇卡尔曼滤波的文章,陆续带给大家,希望能给大家的研究一些帮助。本文为一段关于卡尔曼滤波有趣的对话,清晰把卡尔曼滤波的知识讲明白了。 卡尔曼滤波器讨论(作者:Harveen Singh)最近我遇到了一位同事拉里,他是一位年轻的开发人员,对能够成为自动驾驶汽车行业的一员感到非常兴奋。他问我关于卡尔曼滤波器的理解。所以这里就有了下面的谈话。 拉里:什么是卡尔曼滤波器? 我:卡尔曼滤波器是一种有助于预测值的工具。 拉里:那太酷了!意味着它是某种占卜功能? 拉里:我在数学上并不擅长!也许这听起来有点荒唐。你能继续吗? 拉里:正态分布? 图1.左倾斜和右倾斜分布的示例 但是有很多情况下,数据趋向于左右中间值,没有左偏差或右偏差,所得分布称为正态分布或高斯或钟形曲线。 图2.正态分布的例子,看起来像一个钟,因此钟形曲线。 拉里:明白了!正如你指出的那样,这是一条连续的曲线,而不是离散值。我怎么知道掷骰子掷到5点的概率? 拉里:哦!那么完整图的面积将为1,因为概率可以有最大值1? 拉里:但你没有用数学术语表示高斯? 拉里:那么与卡尔曼滤波器有关的高斯如何? 拉里:哦!你在这里玩了一些多态游戏。你刚才提到这是一个迭代过程? 在预测中,我们只是根据初始值预测称为预测值的新值,然后根据系统中存在的各种过程噪声预测我们预测中的不确定性/误差/方差。 在更新中,我们考虑来自设备的实际测量结果,我们称之为测量值。这里我们计算我们的预测值和测量值之间的差异,然后通过计算卡尔曼增益来决定保留哪个值。然后,我们根据Kalman Gain做出的决定计算新值和新的不确定性/误差/方差。这些计算值将最终成为我们的卡尔曼滤波器在迭代1中完成的预测。 更新步骤的输出再次馈入预测状态,并且循环继续,直到我们的预测值和实际值之间的误差/不确定性倾向于收敛到零。 拉里:那很快。你能解释任何例子或流程图? x - >平均 图3.卡尔曼滤波器的粗略流程图 拉里:看起来很直观!但Kalman Gain到底是什么? 拉里:但它如何知道如何相信预测值或实际值? K =预测中的误差/(预测误差+测量误差) K的取值范围为0到1。如果我们的测量误差很大,K接近0,这意味着我们的预测值接近实际值。如果我们预测的误差很大,K更接近1,这意味着我们的测量值更接近实际值。 拉里:好的,明白了!我现在变得很兴奋。你现在可以详细阐述方程吗? 图4.预测步骤的等式 图5.更新步骤的等式 拉里:在这一步,可能我们应该结束我们的讨论。什么是F,B,H?有各种各样的变量搞乱了。你说这就是为了卡尔曼过滤器,我了解功能,但为什么现在如何解? 假设我们想要根据来自不同传感器的测量结果来预测汽车的位置和速度。 x - >包含位置和速度的平均状态向量。 x向量 预测步骤公式1-: x'= Fx +B.μ+ν x' - >预测值 F矩阵F是将矩阵从一种形式转换为另一种形式所需的状态转移矩阵或自适应矩阵。例如,假设我们有一个模型来预测未加速的对象的位置和速度。所以在这种情况下,时间增量t之后的新p和v被赋予为: p'= p +vΔt v'= v 所以在这种情况下,F矩阵将是: F矩阵 B矩阵B是控制输入矩阵,表示由于内部或任何外力而引起的物体状态变化。例如:重力或对物体的摩擦力。 为什么B.μ= 0?大多数情况下,在自动驾驶汽车中,控制乘积矢量的值等于零,因为我们无法模拟作用于汽车物体的外力。 ν这是过程中的噪声。我们添加可能存在于频道中的随机噪声,以使我们的预测有点正确。 公式2-: P'= FPF + Q P' - >预测协方差 Q矩阵我们假设物体改变方向或者可能加速或减速。所以在一段时间Δt之后,我们的不确定度增加了Q因子,这又是噪声。所以我们在噪音技术上添加噪音。 所以在预测步骤中,我们得到两个预测值x'和P'。 更新步骤公式1: y = z-Hx' z - >实际测量值 Z这是来自传感器的实际测量值。 H这又是一个状态转换矩阵。使用H,我们可以丢弃我们不需要的状态变量的信息。从技术上来说,H在做与预测步骤中F相同的工作。 等式2: S =HP'Hᵀ+ R R->测量噪声 RR表示测量中的噪声。什么?那么这些设备不是100%准确的?是的,在这个世界上没有什么是完美的,甚至没有衡量价值的设备。所有设备都有一个由制造商给出的R参数的预定义值,该值在整个周期内始终保持不变。 K 我们在这里有一个复杂的等式,但它非常简单。我们正在计算卡尔曼增益K,之前给出的公式。 S这是系统中的总错误。我们的预测误差加上测量误差。 那么为什么如此复杂的K方程与公式中的早期相比简单呢? 等式3: x = x'+ Ky 最后一步这是我们根据卡尔曼增益所做的计算更新x和P的最后一步。注意:对于LHS,我们有x和P,而不是x'和P',因为我们现在将x和P设置为下一个预测步骤,因此我们需要找到它们的值。 拉里:好的,这需要一些时间来消化!我必须通过什么额外的资源才能完全理解它? (https://www./channel/UCiGxYawhEp4QyFcX0R60YdQ) 总结所有方程式: 我的同事拉里对于了解卡尔曼滤波器的工作非常兴奋,但他真的能理解非线性和扩展卡尔曼滤波器的概念吗? 拉里:我知道卡尔曼滤波器,现在我可以预测和更新,我基本知道一个重要的预测工具。 我:你能告诉我什么是我们在阅读卡尔曼滤波器时做出的假设吗? 拉里:你什么意思?你只是说卡尔曼滤波器只能使用高斯滤波器。而已?不是吗? 拉里:天啊!线性函数从哪里出现? 线性函数看起来像这样: 另一方面,非线性函数如下所示: 因此,从这些图中观察,直线方程是线性函数,而cos函数是非线性函数。 拉里:是的,没关系。我们的方程没有任何角度,所以它们看起来只是线性的。那么KF现在的问题是什么? 我:大多数现实世界问题都涉及非线性函数。在大多数情况下,系统正在研究某个方向并在另一个方向进行测量。这涉及角度和正弦,余弦函数,它们是非线性函数,然后导致问题。 拉里:嗯,但是非线性函数又是如何产生问题的呢? 如果喂入具有非线性函数的高斯,则输出不是高斯。非线性函数导致非高斯分布。 因此,如果我们应用非线性函数,它不会最终成为高斯分布,我们无法再应用卡尔曼滤波器。非线性破坏了高斯分布,计算均值和方差是没有意义的。 拉里:哦,不,在这种情况下,我们的卡尔曼滤波器现在已经损坏。那么解决方案是什么? 拉里:我?嗯。我会说只使用线性函数:D 拉里:什么?你是说我是对的?怎么来的? 拉里:那么你说的那些因素,罪恶功能呢?他们仍然是非线性的,对吧? 拉里:新工具不断涌现!泰勒是如何工作的? 假设我们想在x = 0时近似sin(x)。 在x = 0时,sin(x)= 0,P(x)= c_0 + 0 + 0 如果我们的逼近必须比sin(x)更接近一点,那么sin(x)的值必须相等到在x = 0时P(x)的值。所以,c_0 = 0 如果我们的近似在x = 0 如果我们的近似必须精确,则P(x)的导数值必须等于x X = 0。所以c_1 = 1 继续...我们可以发现sin(x)= x - x³的近似值sin(x) = x − x³. 3! + x⁵. 5! − x⁷. 7! + x⁹. 9! … 拉里:很酷!这真的很酷。但是这又会给出一条非线性的曲线,我们是不是只对线性化感兴趣? 拉里:嗯。好。KF只适用于线性函数,但在现实生活中,我们有非线性函数来破坏我们的高斯函数,所以我们尝试用泰勒级数线性逼近这些函数,并将其纳入扩展卡尔曼滤波器。对? 拉里:我想知道这是如何影响我们为卡尔曼滤波器编写的方程,但在此之前,所有提供数据的传感器都是如此? Lidar => {px, py} px,py - >笛卡尔坐标系中物体的坐标 拉里:我们可以从两个传感器获取数据吗? 拉里:好的,据我所知,来自雷达的测量结果是非线性的,因为它们涉及角度。现在我有兴趣了解扩展卡尔曼滤波器的方程! 预测步骤x′ = F.x + B.μ + ν P′ = FPFᵀ + Q 预测步骤与卡尔曼滤波完全相同。数据是来自激光雷达还是雷达并不重要,预测步骤完全相同。 更新步骤(仅在EKF情况下,即来自雷达的非线性测量)公式1: y = z - h(x') z - >极坐标中的实际测量 H(X')这是一个函数,它指定了笛卡尔坐标系中的预测值与极坐标系之间的映射关系。这种映射是必需的,因为我们在笛卡尔坐标系中预测,但是我们的来自传感器的测量值(z)是在极坐标系中。 图7.笛卡尔和极坐标之间的映射 等式2: S= HⱼP′Hⱼᵀ + R R - >测量噪声 HⱼHⱼ是雅可比矩阵。雅可比矩阵是我们刚刚在泰勒级数中讨论的一阶导数。既然我们在这里处理矩阵,我们需要以矩阵的形式找到差分。 J_kl = d F_k / dX_l J_kl是雅可比矩阵的k,l元素,F_k是向量函数F的第k个元素,并且X_1是向量变量X的第l个元素。 这里F_k = { ρ, Φ , ρ_dot},X_1 = {px,py,vx,vy} 因为在RADAR的情况下,我们有4个测量值,2个距离和2个速度。 图8.雅可比矩阵 等式3: x = x′ + K.y 拉里:哦,明白了!因此,在LIDAR的情况下,我们将应用卡尔曼滤波器,因为来自传感器的测量结果是线性的。但是在雷达的情况下,我们需要应用扩展卡尔曼滤波器,因为它包含非线性的角度,因此我们使用称为雅可比矩阵(Hⱼ)的泰勒级数的一阶导数来近似非线性函数。然后我们使用h(x')将我们的笛卡尔空间转换为极坐标空间,最后在KF的所有其他方程中Hj替换H。 我:10/10。谢谢! 当然,雅可比矩阵有点魔幻,因为它将非线性空间转换为线性空间。但相信我,这不是什么魔术,而是数学。 |
|
来自: 昵称60593673 > 《卡尔曼滤波》