当前位置：首页 > news >正文

高翔【自动驾驶与机器人中的SLAM技术】学习笔记（五）卡尔曼滤波器一：认知卡尔曼滤波器；协方差矩阵与方差；

news 2025/9/17 0:46:38

卡尔曼滤波器

为了研究卡尔曼，我阅读了大量博文。不敢说完全吃透，但是在做一件什么事，可以通过下面这文章来理解，我读了不下五遍。并整理标准重点，添加自己的一些见解。

自动驾驶传感器融合算法 - 自动驾驶汽车中的激光雷达和雷达

自动驾驶传感器融合算法-自动驾驶汽车中的激光雷达和雷达

来源：

原创 HITJACKJU [机器人规划与控制研究所](javascript:void(0);) 2024-06-26 17:32 美国

在本文中，我们将深入探讨 LiDAR 和 RADAR 之间的传感器融合。这两种传感器在自动驾驶和许多其他机器人应用中被广泛使用。我们将首先简要介绍这两种传感器，然后介绍我们使用的融合算法及其特性。

顺便问一下，什么是传感器融合？在感知模块中，即观察环境（道路线、交通标志、建筑物、行人等），我们需要使用多个传感器。这可以增加冗余度、确定性，或者利用多个传感器的优势并创建多个用例。这创建了一个我们称之为传感器融合的领域。

例如，使用相机可以让我们看到交通信号灯的颜色。它是分类、车道线检测等的完美工具……使用LiDAR非常适合 SLAM（同步定位和地图构建）和深度估计——即估计任何物体的精确距离。最后，RADAR具有一种可以测量物体速度并给你开超速罚单的特定技术！

在本文中，我们将学习融合 LiDAR 和 RADAR，从而利用LiDAR 技术估计距离并以 3D 方式观察世界，并利用 RADAR估计速度的能力。

一、用于 LiDAR 雷达融合的传感器数据

在考虑任何传感器融合任务之前，我们必须做两件事：

选择多个传感器进行合并——并定义一个明确的目标。
研究两个传感器来确定“如何”将它们融合。

让我们花一点时间来回顾一下不同的传感器...

1、雷达（无线电探测和测距）

雷达发射无线电波来探测几米（约 150 米）范围内的物体。多年来，雷达一直安装在我们的汽车上，用于探测盲区中的车辆并避免碰撞。

与其他传感器计算两次测量之间的位置差异不同，雷达利用多普勒效应，通过测量车辆向我们移动或远离时下一波频率的变化。这称为径向速度。

雷达可以直接估算速度。

因此，它们在移动物体上的表现比在静态物体上的表现更好。

它的分辨率较低 ，可以知道被检测物体的位置和速度。另一方面，它很难确定被检测的物体是什么。

2、LiDAR（光检测和测距）

LiDAR 使用红外传感器来确定与物体的距离。旋转系统可以发送波并测量波返回所需的时间。这使得可以生成传感器周围环境的点云。它每秒可以生成大约 200 万个点。点云具有不同的 3D 形状，因此可以借助 Lidar 对物体进行分类。

它具有良好的范围（100 至 300 米），并且可以准确估计周围物体的位置。但是它的尺寸很笨重，有些人可能会把它看作拐杖。更不用说，它的价格（1,000-50,000 美元）多年来一直很高，而且仍然远远高于相机（500 美元）或雷达（300 美元）的平均价格。

现在我们对雷达和激光雷达有了很好的了解。让我们看看如何将它们融合！

二、使用哪种类型的数据融合？

正如我在有关传感器融合的文章中所解释的那样，我们有 3 种类型的传感器融合分类：

低级传感器融合- 融合 RAW 数据
中级传感器融合- 融合物体
高级传感器融合——融合物体及其时间位置（跟踪）

在本文中，我们将重点介绍中级传感器融合（后期融合），并了解如何将 RADAR 的输出与 LiDAR的输出相结合。也可以将 RADAR的原始数据进行组合，但RADAR的噪声非常大。因此，所涉及的后期处理级别使我们使用后期融合。

三、用于LiDAR雷达融合的传感器融合算法

到目前为止，我们了解到：

我们将雷达Radar和激光雷达Lidar合作
我们将融合结果，而不是原始数据。

那我们该如何处理呢？

1、使用卡尔曼滤波器进行传感器融合

用于合并数据的算法称为卡尔曼滤波器。

卡尔曼滤波器是数据融合中最流行的算法之一。它由鲁道夫·卡尔曼于 1960 年发明，现在用于我们的手机或卫星的导航和跟踪。该滤波器最著名的用途是在阿波罗11号任务中，该任务将宇航人员送至月球。

在本文中，您将了解卡尔曼滤波器的工作原理，以及如何使用卡尔曼滤波器合并来自两个传感器的数据。使用的示例是LiDAR和RADAR，但实际上我们可以用任何传感器来实现这一点，正如您将看到的。

2、何时使用卡尔曼滤波器？

卡尔曼滤波器可用于数据融合，以估计动态系统（随时间发展）的现在（过滤）、过去（平滑）或未来（预测）的状态。

自动驾驶汽车中嵌入的传感器发出的测量结果有时不完整且有噪声。传感器的不准确性（也称为噪声）是一个非常重要的问题，可以通过卡尔曼滤波器来处理。让我告诉你如何解决。

卡尔曼滤波器用于估计系统的状态，表示为 x。该状态向量由位置 p 和速度 v 组成。

$x=\begin{pmatrix} p\\ v \end{pmatrix}$

在每个估计中，我们都关联一个不确定性度量 P。使用不确定性度量非常好，因为我们可以考虑到LiDAR在我们的测量中比RADAR更准确的事实！（前置信息、预测评估提示）（就理解为误差）

通过进行数据融合，我们考虑传感器噪声和输出。

举个例子，看一下这两个LiDAR融合在一起观察行人的情况。

两个传感器都同样确定，但我们可以看到，如果它们不确定的话，情况会发生怎样的变化。

与只有一个传感器的情况相比，考虑多个传感器及其不确定性可以帮助我们更好地了解行人的位置。

现在，让我们看看如何在雷达/激光雷达融合中实际使用卡尔曼滤波器。首先，我们需要了解传感器融合算法（如卡尔曼滤波器）表示估计值的方式；使用高斯函数。

3、高斯

状态和不确定性用高斯表示。

高斯是一个连续函数，其面积为 1。正如您在这里看到的，高斯是一个以我们所谓的平均值为中心的公式，协方差有助于理解我们有多“确定”。

我们用均值 μ 表示状态，用方差 σ² 表示不确定性。

方差越大，不确定性越大。

高斯可以估计系统状态和不确定性的概率。

我们采用的是正态分布的概率。卡尔曼滤波器是单峰的：这意味着我们每次都有一个峰值来估计系统的状态。

换句话说：障碍物并不是在 90% 距离 10 米处和 70% 距离 8 米处；而是在 98% 距离 9.7 米处或没有障碍物处。

卡尔曼滤波器是连续的单峰函数。

到目前为止，我们已经了解了卡尔曼滤波器如何在表示过程中发挥作用，但在传感器融合过程中却没有发挥作用。现在，让我们通过理解一个关键概念来考虑传感器融合：贝叶斯滤波。

4、贝叶斯过滤

卡尔曼滤波器是贝叶斯滤波器的一种实现，即反复在预测和更新之间交替。

预测：我们利用估计的状态来预测未来的状态和不确定性。
更新：我们使用传感器的观测来修正预测并获得更准确的估计。

传感器融合的具体实现如下：传感器数据到达；我们更新正在跟踪的行人的估计位置，并预测下一个行人的位置。接下来，新的传感器数据到达，我们更新行人的位置，评估预测的准确程度，并据此预测下一个行人的位置。

⚠️要对卡尔曼滤波器进行估计，只需要当前观测值和先前预测值。不需要测量历史。这不是机器学习，而是人工智能。因此，此工具很轻便，并且会随着时间的推移而改进。

让我们回顾一下，分析一下迄今为止所学到的一切：

1. 我们正在进行LiDAR和RADAR之间的传感器融合

2. 但只有它们各自的输出（后期融合）

3. 为此，我们使用卡尔曼滤波器

4. 哪些是单峰的，并使用高斯来表示状态和不确定性

5. 并使用预测/更新循环（也称为贝叶斯过滤）进行工作。

现在，让我们看看它的内部工作原理。

个人总结一波：这个预测和更新是重点，得明白这个过程。

1、初始化：读取第一帧数据，估计当前时刻的状态，并预测下一时刻也就是第二帧数据时的状态。

2、读取第二帧数据：基于此数据，我更新当前时刻的状态（这个状态是上一时刻预测的），生成新的当前状态的估计值，并预测第三帧数据的状态。换言之：数据来之后，我先更新上一时刻对此时刻的预测，生成对此刻状态新的估计值，并对下一时刻进行预测。（或者说基于上一帧的预测，结合传感器的读数，估计出当前的最优估计状态值。并生成下一时刻的预测）。

3、不断循环第2步。

PS：后文称：读取第n帧数据为：测量measurement。

四、卡尔曼滤波器背后的数学

卡尔曼滤波器背后的数学原理是由矩阵的加法和乘法组成的。我们有两个阶段：预测和更新。

在本节中，我将概述卡尔曼滤波器背后的公式。如果您想进一步了解并真正掌握这些内容，请参加我的课程学习卡尔曼滤波器：默默为未来提供动力的隐藏算法。在那里，您将了解卡尔曼滤波器的工作原理，了解数学原理，并从头开始为不同的用例编写自己的卡尔曼滤波器。

1、预测公式

我们的预测包括根据时间 t 时的先前状态 x 和 P 估计时间 t+1 时的状态 x' 和不确定性 P'。

${x}' = Fx + u \\ \\ {P}' = FPF^{T} + Q$

因此，我们只有两个公式来估计 x 和 P。其他值是：

F：从 t 到 t+1 的转移矩阵
u：噪声
Q：包含噪声的协方差矩阵

👉如果我们简化事情——我们可以说我们的新位置 x' 等于前一个位置 x，乘以矩阵 F。这个矩阵 F 称为我们的运动模型。

这是一个关于它的外观和使用方法的简短示例。

${x}' = Fx \\ \\ {x}' = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}\begin{bmatrix} p\\ v \end{bmatrix} \\ \\ \\ {x}' = \begin{bmatrix} p + v\Delta t\\ v \end{bmatrix}$

如您所见，F 只是一个矩阵，描述我们如何从步骤 t 移动到步骤 t+1。

这里，我们有：

位置（t+1）=位置（t）+速度（t）*时间
速度（t+1）=速度（t）

换句话说，我们考虑恒定速度。

我们的预测方程旨在转换两个时间帧之间的运动。我们可以让 F 矩阵实现不同的运动模型，例如恒定转弯速率、恒定速度、恒定加速度……

这就是我们预测下一个位置的方法 - F 是用于平移运动的矩阵。现在，让我们看看更新公式。

2、更新公式

在更新步骤中，我们要调整我们的位置，并纠正我们下一步的预测方式。

$y = z - H{x}' \\ \\ S = H{P}'H^{T} + R \\ \\ K = {P}'H^{T}S^{-1}$

$x = {x}' + Ky \\ \\ P = (\mathit{I - KH}){P}'$

让我们看一看它是如何工作的——我们有时间 t+1 的 x'（预测状态）和 P'（预测协方差）。

现在，我们收到了新的传感器数据并可以确认我们是否接近。

y 是实际测量值与我们的预测之间的差异——这是预测误差！（这个差值，结合卡尔曼增益更新最优状态估计）
其他矩阵用于考虑传感器噪声（R）、估计系统误差（S）和卡尔曼增益（K）
- 最后一个值卡尔曼增益K介于 0 和 1 之间，有助于决定我们是否应该更信任预测或测量。
所有这些都导致计算一个新的 x 和一个新的 P。

更新阶段可以估计出比测量和预测更接近现实的 x 和 P。

经过几个循环后，卡尔曼滤波器将会收敛，做出越来越准确的预测。

在回顾并了解全局之前，让我们先考虑以下有关贝叶斯概率的内容。

========================说大事专用起始分割符===========================

上面提到卡尔曼增益K是一个介于0和1之间的数值。

$x = {x}' + Ky$

其中这个y：是运动方程对当前状态的预测值，与观测方程对当前的观测值之间的差，上面称为预测误差。

换句话说：对当前状态量：

运动方程有个预测值

观测方程有个测量值

现在这俩值摆在这里，我们该信谁？（误差/偏差/不确定性/协方差）。我们要来确定当前状态的最优估计。

真相往往可能存在于这两者之间。中庸之道。到底倾向于多少呢？这个预测误差y，（下一篇卡尔曼滤波器二中提到的温度差）把这两者之间的差异做了一个划分，而卡尔曼增益K来决定，最优估计到底应该倾向于两者中的哪一个。

这个差异操作，相当于更新，我们下一篇文章，讲卡尔曼滤波器二中，还要提及。

========================说大事专用终止分割符===========================

3、先验/后验和贝叶斯过滤

该图显示了贝叶斯过滤器内部的最终数学。条件概率

我们想要估计的就是后验概率。从数学上讲，它等于先验概率（预测）乘以似然概率（测量）。

个人总结一波：

P(A)：先验概率：预测。基于A对B发生的条件概率

P(B|A)：似然概率：测量。是在A为真的情况下观测到B的概率，这里可以理解为观测的准确性。

P(B)：参照标准：B发生的概率。

P(A|B)：已知B发生的条件下，反推A在B发生之后的条件概率。成为后验概率。

我们原来是拿A预测B的，此时AB都发生了，即得到了P(AB)，也即：既有预测数据又有测量数据。那么依托测量数据的条件下，更新A发生的概率。

再言之：我本来想法是依托T1时刻预测T2时刻的状态。这个预测就是先验概率。等我在T2时刻拿到测量数据之后，也就是这个测量读数成为似然概率的事实，此时对于T2时刻来说，预测和测量都有了。事件都发生了。基于此事实上，我可以更新T1时刻的状态，使我的T1时刻的状态估计的更加准确。

人话就是：线性数学模型算出预测值+传感测量值=更准确的测量值。

将其翻译成高斯如下：

如你所见，更新（后验）始终是最佳估计。其次是测量和预测，这自然是三者中最不确定的。

此时，您可能会想“好吧......这很酷，但是 Jeremy，您并没有谈论激光雷达和雷达......您只是以非常笼统的方式谈论了传感器融合。”

你说得对，那我们就进入正题吧。

五、雷达/激光雷达传感器融合流程

流程如下：每次传感器到来时，我们都会运行一个新的预测+更新循环。

每次预测都会降低确定性，
每次更新都会增加确定性。
但最终，两者结合/融合可以提供更多数据，并获得更好的总体结果。

需要理解的是：我们始终使用相同的状态——我们的状态（位置、速度）估计。

1、在雷达 LiDAR 融合中使用卡尔曼滤波器

使用LiDAR时，我们有“笛卡尔”线性值。我们的数学公式全部用 y = ax + b 类型的线性函数实现。

另一方面，当我们使用雷达Radar时，数据不是线性的。该传感器通过三个指标来观察世界：

𝞺（rho）：到被跟踪物体的距离。
φ（phi）：x 轴和物体之间的角度。
𝞺 ̇ (rhodot)：𝞺 的变化，导致径向速度。

由于包含了角度 φ，这三个值使得我们的测量结果呈非线性。

意思是说，当使用雷达Radar时，我刚才谈到的一切都是错误的，无法起作用。

2、非线性卡尔曼滤波器

这里的一切都将我们引向这一点——世界是非线性的。并非所有事物都完美地沿直线移动。传感器的工作方式并不相同。因此，我们必须使用两种特定类型的卡尔曼滤波器：

扩展卡尔曼滤波器：EKF
无迹卡尔曼滤波器

这些过滤器将首先“线性化”非线性数据，以便它可以在经典过滤器中工作。

在扩展版本中，我们使用雅可比矩阵和泰勒级数对数据进行线性化。
在无迹版本中，我们使用 Sigma 点预测。

3、最终传感器融合流程

考虑到这一点，这是我们最终的传感器融合流程。

LiDAR会定期更新，而RADAR则需要进行线性化处理。这个循环不断重复。我们永远不会将数据融合在一起，而总是一个接一个地进行。

RMSE 值是均方根误差测量，即我们的预测与现实之间的误差。（原文有图）

👉无迹卡尔曼过滤器的误差比扩展过滤器的误差更低，因为这种技术更有效。

因此，我们有两种技术——对于这个特定的用例，一种似乎比另一种更好。

六、结论

传感器融合是自动驾驶汽车中最重要的主题之一。传感器融合算法可让车辆准确了解障碍物的数量，并估计它们的位置和行驶速度。这至关重要。

它们总是依赖于我们拥有的传感器：卡尔曼滤波器不是一个系统的答案。

现在每天都会使用多个传感器。它用于物体检测、定位、计算机视觉和跟踪。

自动驾驶算法——使用扩展卡尔曼滤波算法实现传感器融合和物体跟踪。（有代码）
关于卡尔曼及卡尔曼增益的理解【精】
自动驾驶-序惯卡尔曼滤波【附Github源代码】

七、补充阅读1：variance与协方差

User: variance

Kimi: 方差（Variance）是统计学中衡量数据离散程度的一个重要指标，它描述了数据点与均值之间的差异。具体来说，方差是每个数据点与均值差的平方的平均值。

在概率论中，方差用来衡量随机变量与其期望值（均值）之间的偏离程度。方差通常用希腊字母σ²（sigma squared）表示。方差的计算公式在离散型随机变量和连续型随机变量中有所不同，但基本原理是相同的。方差不会是负数，因为它是平方的结果。如果一个数据集的方差为零，这意味着所有数据点都相同，没有变异性。

在实际应用中，方差有时因为单位平方而难以直观理解，因此经常使用方差的平方根——标准差（Standard Deviation），来描述数据的离散程度。标准差与原始数据具有相同的单位，更易于解释和比较。

方差的概念最早由罗纳德·费雪（Ronald Fisher）提出，并在统计学中得到广泛应用。它不仅用于描述单个数据集的变异性，还可以用于衡量不同数据集之间的差异。

例如，在经济学、金融学和其他领域，方差和标准差常用于衡量风险和不确定性。()

方差的计算方法在总体数据和样本数据中有所不同。

总体方差的计算公式是所有数据点与总体均值差的平方和除以总体大小。
而样本方差的计算则是样本中每个数据点与样本均值差的平方和除以样本大小减一（(N-1)），这是一种无偏估计，用于估计总体方差。

方差在数学上也有其扩展形式，例如对于随机向量，方差可以表示为协方差矩阵，这在多维数据分析中非常重要。此外，方差在概率论中也有其特定的定义和性质，例如在连续分布中，方差是通过积分来计算的。

总的来说，方差是描述数据分布特性的关键统计量，它在数据分析、风险评估和科学研究中都有着广泛的应用。

对于这个协方差，我必须推荐一篇文章：概率论—协方差/相关系数/协方差矩阵

总结一句：协方差矩阵，可以表述变换的旋转方向，以及变换的尺度，或者说包含了变换的权重信息。

八、协方差其意义：

度量各个维度偏离其均值的程度（均值期望预测误差有多大）。协方差的值如果为正值，则说明两者是正相关的(从协方差可以引出“相关系数”的定义)，结果为负值就说明负相关的，如果为0，也是就是统计上说的“相互独立”。

终于明白协方差的意义了-CSDN博客

协方差的意义_协方差网络-CSDN博客

在概率论中，两个随机变量 X 与 Y 之间相互关系，大致有下列3种情况：

当 X, Y 的联合分布像上图那样时，我们可以看出，大致上有： X 越大 Y 也越大， X 越小 Y 也越小，这种情况，我们称为“正相关”。

当X, Y 的联合分布像上图那样时，我们可以看出，大致上有：X 越大Y 反而越小，X 越小 Y 反而越大，这种情况，我们称为“负相关”。

当X, Y 的联合分布像上图那样时，我们可以看出：既不是X 越大Y 也越大，也不是 X 越大 Y 反而越小，这种情况我们称为“ 不相关”。

怎样将这3种相关情况，用一个简单的数字表达出来呢？

在图中的区域（1）中，有 X>EX ，Y-EY>0 ，所以(X-EX)(Y-EY)>0；

在图中的区域（2）中，有 X<EX ，Y-EY>0 ，所以(X-EX)(Y-EY)<0；

在图中的区域（3）中，有 X<EX ，Y-EY<0 ，所以(X-EX)(Y-EY)>0；

在图中的区域（4）中，有 X>EX ，Y-EY<0 ，所以(X-EX)(Y-EY)<0。

当X 与Y 正相关时，它们的分布大部分在区域（1）和（3）中，小部分在区域（2）和（4）中，所以平均来说，有E(X-EX)(Y-EY)>0 。

当 X与 Y负相关时，它们的分布大部分在区域（2）和（4）中，小部分在区域（1）和（3）中，所以平均来说，有(X-EX)(Y-EY)<0 。

当 X与 Y不相关时，它们在区域（1）和（3）中的分布，与在区域（2）和（4）中的分布几乎一样多，所以平均来说，有(X-EX)(Y-EY)=0 。

所以，我们可以定义一个表示X, Y 相互关系的数字特征，也就是 协方差

cov(X, Y) = E(X-EX)(Y-EY) 。

当 cov(X, Y)>0时，表明 X与Y 正相关；

当 cov(X, Y)<0时，表明X与Y负相关；

当 cov(X, Y)=0时，表明X与Y不相关。

这就是协方差的意义。