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

卡尔曼滤波器

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

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

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

来源：

原创 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）提出，并在统计学中得到广泛应用。它不仅用于描述单个数据集的变异性，还可以用于衡量不同数据集之间的差异。