多伦多大学自动驾驶状态估计和定位笔记-全-
多伦多大学自动驾驶状态估计和定位笔记（全）
001：欢迎来到自动驾驶汽车专项课程
在本节课中，我们将介绍多伦多大学自动驾驶汽车专项课程的概览、课程结构以及学习本课程所需的基础知识。
大家好，欢迎来到多伦多大学的自动驾驶汽车专项课程。我是Steven。我是Jonathan，我们将作为您在整个专项课程中的讲师。自动驾驶汽车是沉睡的巨人，它有能力改变一切：道路安全、每个人的出行便利性，同时大幅降低驾驶成本。本专项课程将教授您在自动驾驶行业开启职业生涯所需的知识。
无论您来自学术界还是工业界，这些课程都将为您提供基础知识和实践技能，帮助您与自动驾驶汽车共同构建新的未来。在整个课程中，我们将向您展示来自我们研究以及我们自己的自动驾驶汽车（我们称之为“Autonoose”）的真实世界数据和场景。
经过数十年的应用研究和获奖研究，我们多伦多大学很高兴向您展示自动驾驶的细节。本专项课程共有四门课程。
课程一：自动驾驶汽车简介
在第一节课程中，我将向您介绍自动驾驶汽车的软件和硬件架构。到本课程结束时，您将能够为自动驾驶汽车设计一个基本的硬件系统，识别自动驾驶软件栈的主要组件，并为自动驾驶汽车项目创建安全评估策略。
接下来，您将学习开发完整的车辆运动模型，为纵向控制定义PI控制器，为横向车辆控制定义路径跟踪控制器，并在CARLA模拟器中测试您的控制设计。
课程二：状态估计与定位
上一节我们介绍了自动驾驶汽车的基础，本节中我们来看看状态估计与定位。在专项课程的第二门课程中，我将教授您关于自动驾驶汽车的状态估计、传感和定位知识。到本课程结束时，您将理解参数和状态估计的关键方法，开发和使用典型车辆定位传感器的模型，将扩展卡尔曼滤波器和无迹卡尔曼滤波器应用于车辆状态估计问题，并将LiDAR扫描创建的点云注册到静态环境的3D地图中。
课程三：视觉感知
在第三门课程中，我将教授您自动驾驶汽车的视觉感知。完成本课程后，您将能够将3D点投影到相机图像平面上，校准针孔相机模型，应用特征检测、描述和匹配算法进行定位与建图，开发和训练用于目标检测和语义分割的神经网络。您将把这些方法应用于车辆跟踪和可行驶路面估计。
课程四：运动规划
在第四门也是最后一门课程中，我将教授您自动驾驶汽车的运动规划。到本课程结束时，您将能够设计轨迹展开运动规划方法，计算与静态和匀速运动物体的碰撞时间，在复杂的道路网络上规划路线，为车辆定义高级行为和在通过交叉口、绕过停放车辆以及并线时的状态转换，在存在静态障碍物的环境中开发运动学上可行的路径，计算满足速度、曲率和移动物体运动规划约束的速度曲线，规划行为并执行操作以安全地通过世界，并在CARLA模拟器中获得调试和测试自动驾驶算法的宝贵经验。
学习前提
因此，到专项课程结束时，您将对自动驾驶软件栈的架构和组件有详细的了解，并将能够编程您自己的自动驾驶汽车。由于我们教授的是如何编程您自己的自动驾驶汽车，本专项课程有几项先决条件。
以下是您需要掌握的基础知识：
- 线性代数：您应精通线性代数，熟悉矩阵、向量、矩阵乘法、秩、特征向量和逆矩阵。这些背景知识将帮助您理解整个课程中的控制、状态估计、感知和规划算法。
- 统计学：您还应熟悉统计学，特别是高斯概率分布。这些知识对于状态估计和感知非常重要，例如当我们从GPS和惯性测量中估计车辆速度和航向时。
- 微积分与物理：您应熟悉基础微积分和物理学，例如力、力矩、惯性和牛顿定律。
当然，了解如何驾驶汽车是有帮助的，但由于汽车是自动驾驶的，这并不是本专项课程的硬性要求。如果您不具备这些必要的先决条件，Coursera上有优秀的机器人学、人工智能、深度学习和其他课程，可以帮助您为本专项课程做好准备。
总结与展望
自动驾驶是一个不断发展和变化的领域，因此不仅要跟上自动驾驶知识，还要跟上机器人学、人工智能和深度学习的发展，这将有助于您保持技术技能的敏锐。我们还有很长的路要走，我们需要先驱者来帮助我们到达那里。
那么，您准备好开始这段旅程了吗？
🎼是的。
本节课中我们一起学习了多伦多大学自动驾驶汽车专项课程的整体框架，包括四门核心课程（简介、状态估计与定位、视觉感知、运动规划）的主要内容与学习目标，并明确了成功学习本课程所需的数学与物理基础。
002：课程介绍与概述 🚗
在本节课中，我们将学习多伦多大学自动驾驶汽车专项课程的第二门课程。这门课程将聚焦于自动驾驶汽车的状态估计与定位问题，这是实现安全、可靠自动驾驶的核心技术之一。
欢迎来到多伦多大学自动驾驶汽车专项课程的第二门课程。
我是Jonathan，我将担任本课程的讲师。
如果你是第一次加入我们，我建议你先学习专项课程的第一门课程，以便熟悉自动驾驶汽车的硬件和软件架构，以及车辆模型与控制的基础知识。
如果你已经完成了第一门课程的学习，那么你已经熟悉了自动驾驶架构、车辆动力学建模、纵向与横向控制。
你也学习了自动驾驶汽车的安全保障知识，并使用CARLA模拟器设计了纵向和横向控制器。
在本课程中，我们将深入探讨自动驾驶问题的另一面，学习不同类型的传感器，以及如何利用它们进行自动驾驶汽车的状态估计与定位。
状态估计是根据通常带有噪声的测量值，确定某个物理量最佳值的过程。
我们说测量值带有噪声，是因为传感器并非完美无缺。
如果你反复测量同一个量，每次得到的结果都不会完全相同。
例如，如果你将温度计插入一杯40摄氏度的茶中，温度计第一次可能显示39.8度，第二次显示40.1度，依此类推。
状态估计为我们提供了一种方法，可以综合所有这些带有噪声的温度测量值，来估计茶的真实温度。
因此，状态估计对于任何带有传感器的设备都至关重要，自动驾驶汽车也不例外。
对于自动驾驶而言，最重要的一类状态估计就是定位，即确定汽车在世界中的位置及其运动状态的过程。
定位极其重要，因为即使你拥有世界上最好的控制器设计，如果不知道汽车的状态，你也无法使用它。
因此，在本课程结束时，你将学到以下内容：
以下是本课程的核心学习目标：
- 用于参数和状态估计的最小二乘法。
- 将线性卡尔曼滤波器及其非线性变体——扩展卡尔曼滤波器和无迹卡尔曼滤波器应用于状态估计问题。
- 为典型的定位传感器（如GPS、惯性传感器和激光雷达单元）开发模型。
- 学习激光雷达扫描匹配和迭代最近点算法。
- 最终，利用这些工具将多个传感器数据流融合成一个用于自动驾驶汽车的单一状态估计。
作为本课程的最终项目，你将应用所学知识，在CARLA模拟器中为自动驾驶汽车开发一个完整的车辆状态估计器。
你将开发的估计器与当今自动驾驶汽车中使用的非常相似，这将是一个非常有效的方式，帮助你建立对这些系统在实践中如何工作的直觉和理解。
你准备好了吗？让我们开始吧。
本节课中，我们一起学习了本课程的核心目标与内容框架。我们了解到状态估计是处理传感器噪声、获取准确信息的关键，而定位是自动驾驶汽车实现精确控制的基础。在接下来的课程中，我们将逐一深入这些激动人心的主题。
003：认识讲师乔纳森·凯利
在本节课中，我们将认识本课程的讲师乔纳森·凯利教授，了解他的背景、职业动机以及他对自动驾驶技术领域的看法。
我叫乔纳森·凯利，是加拿大多伦多大学航空航天研究所的助理教授。
童年时期，我每个月都喜欢去杂志店，总是阅读《大众机械》或《大众科学》。这些杂志上有许多有趣的文章，内容涵盖从太空旅行到新型交通工具，再到新的计算机设计，内容非常吸引人。
因此，这最终促使我开始将科学技术视为一个可能的职业选择。
在我还很小的时候，发生了一件大事，即第一次航天飞机灾难——挑战者号事故。我至今仍清楚记得那一刻，当时我想，天哪，一定是出了什么严重的问题，后果是多么可怕。那场巨大的爆炸发生时，我记得自己震惊地呆坐着，心想技术总是可靠的。那是我当时的看法，我认为技术永远不会失败。汽车不会无缘无故爆炸。但在电视上看到这一幕，让我深刻认识到，扎实的工程确实至关重要。
在那些危及生命或可能改变人生的情境中，可靠的工程是极其重要的。
自动驾驶汽车在许多方面与此非常相似。作为工程师，我们有责任将这些系统设计得尽可能安全可靠。这是近期最令人兴奋的技术革命之一。
每天都有新的想法、新的技术和新的设计涌现。我常常坐在办公桌前，看到人们提出的创新性新想法和技术时，都会感到眼前一亮。
这种兴奋感也来自于它正在改变技术格局，并有可能让很多人受益。
本节课中，我们一起认识了乔纳森·凯利教授，了解了他如何因童年兴趣而投身科技领域，以及他从挑战者号事故中领悟到的工程责任。这些背景奠定了他对自动驾驶技术安全性与可靠性的高度重视，也让我们看到了这一领域充满活力的创新前景。
004：认识讲师史蒂文·瓦斯拉恩德
概述
在本节课中，我们将通过讲师史蒂文·瓦斯拉恩德的个人经历与见解，了解他如何从一名广泛的兴趣爱好者成长为自动驾驶与机器人领域的研究者。他的故事揭示了将基础学科兴趣应用于前沿科技项目的路径，并为我们理解自动驾驶系统的构建与挑战提供了背景。
从广泛兴趣到专业聚焦
我童年时兴趣广泛，热爱各种事物。我曾认为自己可以成为一名科学家，或许是物理学家，也可能是化学家，但当时其实并没有明确的方向。成长过程中，我并未过多担忧未来，而是度过了一个田园诗般的童年。
随着年龄增长，大约在十六、七或十八岁时，我开始思考需要做一些有趣的事情来充实人生。正是在这个阶段，我开始主动迎接挑战，并逐渐认清是什么在持续驱动我前进。
发现解决问题的乐趣
我热爱解决数学问题，也喜欢在放学后与朋友们一起钻研化学或微积分挑战。这些活动都是我课余时间自发进行的，并没有特别明确的目标。
直到我开始认真尝试选择一项事业时，才发现那些我喜欢的活动，在特定的应用领域中会频繁出现。
职业生涯的起点：航空工业
大学毕业后，我实际上在普拉特·惠特尼加拿大公司工作，这是一家小型喷气发动机制造商。在普拉特·惠特尼工作期间，经历非常迷人。
我们有机会做一些诸如测试喷气发动机控制系统的事情，这让我充分接触了航空学的乐趣。然而，航空学领域的挑战在于其极其严格的安全要求。
你所做的每一件事都必须严谨可靠，并且每次执行都必须精确无误。因此，该领域的发展实际上受制于经过反复验证的、已有三十年历史的技术。这对于一名试图推动变革、改变世界的年轻工程师来说，是相当令人沮丧的。
转向学术研究
于是，我带着从航空航天工业中获得的宝贵经验以及对安全的专注，进入了研究生生涯，投身于航空与航天学领域，并开始建造无人机。
投身机器人学与自动驾驶
是的，这就是解决问题，是算法开发。具体来说，是处理机器人任务。
例如，让无人机降落在行驶的汽车上，或者让自动驾驶汽车自行泊车、穿越障碍区域。所有这些问题都非常实际、非常具有应用性，并且你在构建过程中就能看到成果。你可以看到它的运行效果——如果无人机错过了汽车，它就会坠毁失败。这其中有一种令人兴奋的、非常有趣的成分。
构建自动驾驶汽车的基本步骤
要制造一辆自动驾驶汽车，首先必须将一辆普通汽车改造成线控驾驶车辆。这意味着你可以将计算机接入汽车，并向油门、转向和刹车发送指令，汽车将实际执行这些操作。
一旦具备了这种能力，你就可以真正开始编程让它做任何你想做的事情。
接下来，你必须在后备箱中添加一整套传感器和计算设备。拥有了所有这些部件，你就可以开始构建自己的自动驾驶车辆了。
研究重点与学术角色
我的研究实验室围绕这样一个理念建立：如果我们拥有一个真实的测试平台，并将其置于真实情境中，我们就能更快地了解当前方法的局限性。
我们真正关注的是寻找特别适合学术界的研究问题。例如，处理恶劣天气条件、处理一些边缘案例，或者试图从理论上证明我们所使用的方法具有可量化的安全性。因此，在学术界确实有很多具体问题可供我们研究。
我们可以尝试许多大胆的想法，可以比工业界可能的方式更快地、或者更少约束地探索可能的解决方案空间。
自动驾驶的未来与挑战
我们无疑将看到这些车辆的性能不断提升。我们将看到像特斯拉自动驾驶仪这样的系统逐年变得更好。我们还将看到真正自主的车队在受限领域内运行，这些领域被认为是安全可控的。
但我仍然认为，要实现最终的、在任何你想驾驶的地方都能实现的完全自动驾驶，我们还有很多工作要做。这仅仅是因为汽车可能失败的方式太多，它们必须理解的情境太多，有太多无法提前预测的奇怪状况。
给初学者的建议
如果你刚接触自动驾驶领域，并认为这是一个美妙的领域且想尝试，我认为你首先应该做的显然是学习这个Coursera专项课程。
除此之外，还有数百种方法可以丰富你的知识，并更好地了解如何进入这个领域。要知道，整个领域都依赖于机器人学、计算机视觉、神经网络与深度学习的基础。这些都是你可以深入研究的领域，而真正有趣的是，这些知识也适用于其他美妙的应用。
想想机器人操纵器，想想手机摄像头系统及其改进的视频拍摄功能，想想无人机飞行和物体跟踪——你可以用这些知识做无数的事情。
对于那些正在思考自动驾驶是否是一个值得投入、值得探索的方向的人，有一点是绝对明确的：这个市场将持续增长。对我们项目中毕业生的需求简直是无法满足的，这非常惊人。
总结
回顾过去，从放学后钻研随机数学问题到现在从事自动驾驶汽车研究，并多年来一直与无人机打交道，我只是一个幸运的人。
本节课中，我们一起跟随史蒂文·瓦斯拉恩德教授的讲述，了解了他从广泛兴趣者到专业研究者的历程，认识了自动驾驶系统构建的基本步骤与核心挑战，并获得了针对初学者的宝贵学习与发展建议。他的经历表明，将基础学科的热情与解决实际问题的实践相结合，能够引领我们走向令人兴奋的技术前沿。
005：认识固件工程师戴安娜
概述
在本节课中，我们将通过固件工程师戴安娜的分享，了解自动驾驶行业中固件工程师的角色、日常工作内容以及进入该领域的可能路径。你将了解到硬件在环测试等核心概念。
戴安娜的背景介绍
我是戴安娜·伯吉斯，目前在一家名为Zs的公司担任固件工程师，隶属于固件团队。
我拥有滑铁卢大学的机电一体化工程学士学位。在校期间，我完成了多次实习，并在许多不同的岗位工作过。在加入Zs之前，我曾在Peloton Technology工作，该公司专注于半自动驾驶卡车技术。我之所以能得到那份工作，是因为我曾在本迪克斯公司实习，该公司生产卡车制动系统。因此，我是通过卡车行业，以一种迂回的方式进入了自动驾驶领域，最终在Zs实现了完全自动驾驶。
对机器人技术的兴趣起源
我从高中早期就对机器人技术产生了兴趣。我一直对工程学抱有普遍的兴趣，并曾以为自己在大学会选择机械工程。但在高中时，我选修了一些计算机课程，发现自己也喜欢编程。因此，我认为机电一体化是一个不错的选择。
我还加入了一个机器人团队，即FIRST机器人团队。在这个团队中，你需要构建一个复杂的机器人，参加每年规则都会变化的比赛。这意味着每年都需要为下一年的比赛构建一个全新的机器人。每场比赛都包含自主控制部分和驾驶员操作部分，因此需要完成更复杂的任务。通过这些经历，我对机器人技术产生了浓厚的兴趣，并在大学期间进一步巩固了这份热情。
当前工作职责
目前，我在固件团队工作。我的大部分工作都围绕着一个主要的运动控制单元展开。这个单元在车辆中帮助与其他部件协同工作，以控制车辆，例如执行制动或控制机器人向任意方向移动、转向等。
因此，我的很多工作是确保我们能为该单元编写驱动程序，并能与其他系统进行安全通信。通过这项工作，我还进行了大量测试，并使用我们称为Labo的装置。Labo本质上是一个硬件在环测试器，它允许你在代码部署到即将上路的车辆之前，在真实的硬件上测试你的代码。
总结
本节课中，我们一起认识了固件工程师戴安娜。我们了解了她的教育背景、如何从卡车行业进入自动驾驶领域，以及她对机器人技术的兴趣起源。更重要的是，我们学习了她作为固件工程师的核心工作，包括开发运动控制单元的驱动程序、确保系统间安全通信，以及使用硬件在环测试在真实硬件上验证代码。这些工作对于确保自动驾驶汽车的安全可靠运行至关重要。
006：认识软件工程师温斯顿
在本节课程中，我们将认识一位在自动驾驶领域工作的软件工程师——温斯顿，并了解他如何进入这个行业，以及他目前从事的工作内容。
我的名字是温斯顿·维尤。我目前在Zoox公司担任定位与地图构建方向的软件工程师。
我进入自动驾驶领域的契机是在大学最后一年。当时，通用汽车公司与汽车工程师学会（SAE）联合发起了一项名为“Autodrive”的学生竞赛。这项竞赛为期三年，每年年底都会有一次比赛。最终目标是让各学生团队在三年后能够打造出一辆SAE自动驾驶等级4的车辆。
我负责领导轨迹规划团队，并设计了我们在第一年比赛中使用的轨迹规划和变道算法。
最终，我们团队赢得了第一名。我们提交了一篇详细阐述我们设计的论文，这项工作至今仍在推进。可以说，那一刻我明确了自己对自动驾驶汽车的兴趣。
这背后还有一个小故事。在我大学生涯初期，我曾是“无人飞行器”学生团队的成员。我们当时在建造一种半自主的固定翼无人机。它们由遥控器控制，搭载了工业摄像头和机载计算设备。我负责领导该项目的计算机视觉部分，具体任务是检测绘制在木质标牌上的字母。这些标牌被放置在地面上，而我们的飞机则在200英尺的高空飞行。
有一天，当我开车行驶在街道上时，我想到：我能否将用于检测标牌的这些计算机视觉工具，应用到检测道路标志上呢？我产生了一个想法：在我父母的车上安装网络摄像头，然后开车在社区里转，检测所有的街道标志。不过，我的父母对此并不太支持，所以这个项目最终没有进行下去。
后来，当学校的Autodrive团队开始招募成员时，我觉得这很酷。而当我真正开始参与这个项目后，我意识到这就是我想从事的事业。于是，我走到了今天这一步。
在Zoox公司，我隶属于CLAMS团队。CLAMS这个词像是一种贝类，它代表Calibration, Localization And Mapping Simultaneously（同步校准、定位与地图构建）。我主要专注于地图构建方面的工作，这主要涉及生成3D地图。我无法透露更多细节，但可以说，目前通过构建我们的地图，我对旧金山地理的了解，可能比亲自到访那里还要多。
真正让我每天早上充满动力去工作的原因是，我正在处理这些非常前沿且极其酷炫的问题。这些问题之所以前沿，是因为它们仍然是当前非常活跃的开发领域，许多公司或个人此前并未深入涉足。
在本节课中，我们一起认识了软件工程师温斯顿，了解了他通过学生竞赛进入自动驾驶行业的经历，以及他目前在同步定位与地图构建领域所从事的具体工作。他的故事展示了实践项目和解决前沿问题在职业发展中的重要性。
自动驾驶汽车的状态估计和定位：P07：认识自动驾驶系统架构师安迪
在本节课中，我们将通过系统架构师安迪的视角，了解自动驾驶行业的起源、发展以及一名系统工程师在其中的角色与工作内容。
大家好，我是安迪·王，一名自动驾驶系统架构师。我的机器人学生涯始于高中。我是加拿大最早参与机器人竞赛的学生之一。进入大学后，我协助共同创立了一个机器人团队。当时的指导老师刚从美国回来，向我们介绍了那里正在举行的一项名为DARPA挑战赛的比赛。这项赛事是推动整个自动驾驶行业兴起的关键活动之一。受此启发，我们开展了一些自动驾驶赛车活动，我也参与其中。从那时起，我便深深着迷于此。
工作几年后，我重返研究生院深造，期间主修车辆控制与动力学。现在，我从事下一代自动驾驶汽车平台的研发工作。作为一名系统级工程师，我有幸能与多个不同的团队广泛合作，共同构建一个完整的系统。这意味着我既要与负责感知、运动控制软件等的软件工程师协作，也要与设计传感器和车辆平台的硬件工程师合作。
我认为机器人技术非常有趣，而自动驾驶这个特定问题尤其令我热爱，因为它能直接影响人类的生活质量。我们不仅致力于提升安全性、挽救生命，这项技术还有可能帮助我们重塑对交通的思考方式，甚至改变城市的使用方式，让生活更高效、更幸福。
本节课中，我们一起了解了自动驾驶系统架构师安迪的个人经历与职业背景。他分享了从学生时代的机器人竞赛，到参与行业起源的DARPA挑战赛，再到目前负责整合软硬件的系统架构工作的历程。这帮助我们理解了一名系统工程师在复杂的自动驾驶项目中所扮演的跨领域协调与整合的关键角色。
008：认识奥博提卡创始人、牛津大学教授保罗·纽曼
在本节课中，我们将通过牛津大学教授、奥博提卡公司创始人保罗·纽曼的分享，了解自动驾驶领域的发展历程、核心挑战以及未来愿景。我们将探讨为何自动驾驶技术至关重要，以及智能车辆与智能基础设施之间的关系。
我是保罗·纽曼，我是奥博提卡公司的首席技术官兼创始人，同时也是牛津大学的信息工程学教授。
我记得自己开始对自动驾驶车辆和自主系统产生兴趣的那一天。那是在悉尼，当时我刚开始攻读博士学位，正在和一位博士生交谈。他向我介绍了一个名为“SLAM”的问题。这个问题是：一辆车能否在没有人提供地图的情况下，自己弄清楚它在世界中的位置，并且能否自己构建那张地图？我当时就知道，这是我想要研究的所有问题中最核心的一个。我记得那天是1996年3月19日，我真正发现这里存在一个重大问题。从那以后，这个问题从未过时。虽然我们现在已经知道如何解决这些问题。
但我一直是一名工程师，致力于构建事物。当我发现将软件和机械事物结合起来是最令人兴奋的构建方式时，我就投身其中了，而这几乎就是机器人的定义。
因此，我从未真正失去这份热情，大概从九岁开始尝试编程非常基础的计算机，直到现在每天所做的工作，这份激动从未离开过我。
那么，我们在奥博提卡做什么呢？我们构建使自动驾驶车辆实现自主的软件。我们从最底层的传感器开始，一路向上，贯穿整个软件栈，包括规划、定位、控制、感知，直至车队管理系统。我们构建的是使整个车队实现自主运行的完整软件环境。
这就是我们构建的东西。同时，我在大学里也进行开发工作。
我是牛津机器人研究所的所长，在那里我也拥有很棒的生活。在那里，我研究的是自主系统和机器人技术中那些非常、非常困难的问题，这些问题可能是未来十年才会遇到的。
以下是那些处于萌芽状态的研究问题，关于机器、智能以及机器学习，我们希望在未来五到十年内将其应用于我们的车辆中。
因此，我拥有非常优越的生活。那么，我为什么如此关心自动驾驶汽车呢？
因为目前的交通系统已经过时，并且存在缺陷。
道路上80%的事故都是由注意力不集中引起的。机器绝不会做的一件事就是感到无聊。我们目前完全没有解决这个问题。
我们目前的驾驶方式是低效的。随着我们年龄增长，身体变得虚弱，我们最需要的是保持与社会的联系。而如果我们说：“哦，很抱歉，你视力下降了，不能开车了。你癫痫发作了，不能开车了。”那么，整个我们如何驾驶自己、如何移动我们自身（无论是人类本身还是货物）的体系就显得非常不合理。
这套体系源自马车时代。如果我们拥有现在的技术，我们绝不会那样做。所以这是不合理的，而软件和算法将改变这一切。
我绝对相信，自动驾驶技术将使更多的物体（人和货物）移动得更高效、更安全。
这里存在一个问题：车辆本身应该是独立智能的，还是应该利用智能基础设施？
那么，什么是智能基础设施？例如，一个交通灯可以告诉你：“我即将变红，这是我的位置。”或者一个环岛可以告诉你：“这是车辆的位置，你的空位即将到来。”我们可以设想无数种方法来为我们的城市配备智能设备。
所以我的观点是，车辆本身需要足够智能，能够在不依赖基础设施的情况下运行。如果提供了智能基础设施，当然可以在信任它的情况下使用它，但这涉及到网络安全等一系列问题。
因此，我的观点是，车辆本身必须足够智能，就像你作为司机一样。你可以看交通灯，可以看车道线，你不需要前面的车告诉你前面有车，因为你可以看到它。因为如果基础设施被移除，你仍然希望拥有能够自动驾驶的车辆。它可以被基础设施增强，但我认为不应该依赖它。因为如果你解决了这个问题，那么你就真正掌握了在任何地方实现全交通自主的可能性。这可能需要一些时间，但你正朝着这个方向前进，并且不依赖于昂贵、定制的专用基础设施。在我看来，为自动驾驶汽车打造的昂贵、定制的专用基础设施，其名称就是“城市”。
在本节课中，我们一起学习了保罗·纽曼教授进入自动驾驶领域的契机，了解了SLAM（同步定位与建图）这一核心问题的提出。我们探讨了奥博提卡公司的业务范围，即构建从底层传感器到高层车队管理的完整自动驾驶软件栈。更重要的是，我们分析了自动驾驶技术对于解决当前交通系统缺陷（如事故率高、效率低下、社会隔离）的重大意义，并明确了智能车辆应具备独立运行能力，而非过度依赖智能基础设施的核心观点。这为理解自动驾驶系统的自主性和鲁棒性奠定了重要基础。
009：状态估计的重要性
在本节课中，我们将探讨状态估计对于自动驾驶汽车为何至关重要。我们将了解状态估计要解决的核心问题，以及为什么这个问题在实际中充满挑战。
状态估计的核心作用 🎯
状态估计之所以对自动驾驶汽车至关重要，一个直接的原因是：你需要知道自己的位置，而你所能获得的全部输入就是传感器数据。因此，状态估计问题可以简单地表述为：给定所有这些输入的传感器数据，我在哪里？ 你的状态就是“你在哪里”这个问题的答案，而你必须估计它，因为传感器（例如摄像头）并不会直接告诉你答案。如果我有一张世界的图片，我必须从那张图片中推断出我的位置、我的运动方式、我的速度有多快、我正在以多快的速度转弯。这些都是状态估计问题。
因此，你需要利用那些传感器数据。你可能还有一些先验信息，例如关于10毫秒前你的运动速度的信念，你可以将这些信息整合进去。但如果你不能进行状态估计，你就无法处理传感器数据。可以说，这是最根本的问题之一。
状态估计的挑战性 ⚠️
我们已经达成共识，状态估计非常重要。如果它同时也是一个简单的问题，那岂不是很好？不幸的是，它相当困难。为什么？因为我们需要处理、并用来回答“我在哪里”、“我如何运动”、“那边那个个体将要做什么（因为我对它的速度有个估计）”等问题的传感器数据，常常是有缺陷的。
本节课总结
本节课中，我们一起学习了状态估计在自动驾驶中的核心地位。我们明确了状态估计要解决的根本问题：根据传感器数据推断自身的位置、速度和运动状态。同时，我们也认识到这个任务充满挑战，主要因为输入的传感器数据本身可能存在缺陷和噪声。理解这些是深入学习后续具体状态估计算法的基础。
自动驾驶汽车的状态估计和定位：1.1：平方误差准则与最小二乘法 🚗
在本节课中，我们将学习状态估计的基础方法——最小二乘法。我们将了解其历史背景，学习如何通过最小化平方误差来估计未知参数，并推导出求解所需的关键方程。
最小二乘法可以追溯到18世纪末，远早于汽车的概念出现。1801年1月1日，意大利牧师兼天文学家朱塞佩·皮亚齐在夜空中发现了一个新的天体——即现在被称为谷神星的小行星。
皮亚齐在40天内对该天体进行了24次望远镜观测，之后它便消失在太阳的光芒中。由于谷神星直径仅约900公里，再次找到它极具挑战性。为了帮助重新定位谷神星，被誉为“数学王子”的卡尔·弗里德里希·高斯，基于皮亚齐发表的观测数据，使用最小二乘法精确估计了谷神星的轨道参数。
高斯在1809年发表了该方法，但他声称早在1795年就已提出，早于勒让德的工作。高斯将这种方法总结为：一个未知参数的最可能值，是使观测值与期望值之间的误差平方和最小的那个值。
为了说明其工作原理，我们来看一个简单的例子。
假设你正在尝试测量一辆自动驾驶汽车驱动系统中一个简单电阻的阻值（单位：欧姆）。为此，你使用实验室里一个相对廉价的万用表。现在，假设你按顺序收集了以下四个独立的测量值。
如果你以前学习过电路，可能会记得图中所示的这种碳膜电阻的阻值是通过色环编码的。这个电阻的标称阻值是1千欧。然而，由于多种因素，其真实阻值可能与标称值存在差异。在本例中，电阻有一个金色环，表示其阻值最多可偏差5%。此外，我们假设我们的万用表质量特别差，并且测量人员也不够仔细。
目前，我们可以将这种噪声视为等同于一般误差。但在本模块后续课程中，我们将从概率角度重新探讨如何解释这种噪声。
对于这四个测量值中的每一个，我们定义一个标量噪声项，该噪声项独立于其他噪声项。从统计学角度，在这种情况下我们会说噪声是独立同分布的，我们稍后会详细讨论这一点。
接下来，我们定义每个测量值与我们电阻真实值 x 之间的误差。但请记住，我们还不知道 x 是多少。为了找到 x，我们计算这些误差的平方，从而得到一个关于我们的测量值和未知电阻值的方程。
定义了这些误差后，最小二乘法指出，我们要寻找的电阻值——即 x 的最佳估计值——是使平方误差准则（有时也称为平方误差成本函数或损失函数）最小的那个值。
为了最小化平方误差准则，我们将用矩阵符号重写我们的误差。当我们需要处理成百上千个测量值时，这将特别有用。
我们将定义一个误差向量 E，它是我们的观测值（堆叠成向量 Y）、一个称为雅可比矩阵的矩阵 H 以及我们的真实电阻值 x 的函数。H 的维度是 M × N，其中 M 是测量值的数量，N 是我们希望估计的未知数或参数的数量。通常，这是一个矩形矩阵，在线性情况下很容易写出。在后续课程讨论非线性估计时，计算它将需要更多的数学推导。
请注意，这里的 x 是一个标量。稍后我们会看到，它也可以是一个包含多个未知数的向量。
有了这些定义，我们可以将平方误差准则转换为向量符号表示如下。
展开括号，我们得到了这个看起来有些复杂的表达式。那么现在该怎么办呢？请记住，我们需要相对于真实电阻 x 来最小化平方误差。根据微积分知识，我们可以通过对误差函数关于未知数 x 求导，并将导数设为零来实现这一点。
让我们照此操作。我们需要使用一些标准的矩阵表达式。重新整理后，我们得到了所谓的正规方程，它可以写成一个单一的矩阵公式。
我们可以求解这些方程来找到 x̂，即最小化我们平方误差准则的电阻值。
这个表达式有唯一解，当且仅当 HᵀH 非奇异，换句话说，如果该矩阵有逆矩阵。如果我们有 M 个测量值和 n 个未知参数，HᵀH 将是 n × n 矩阵。该矩阵可逆的充要条件是 M ≥ N，因此我们至少需要与未知数一样多的测量值才能推导出最小二乘解。这通常不是问题。事实上，我们常常面临处理过多测量值的挑战。尽管如此，在使用该公式时，你应该记住这个限制。
回到我们特定的电阻问题，让我们填入变量。
一旦我们有了这些量，剩下的就是代入计算了。这个表达式很容易编码实现，你将在模块作业中看到。
请注意，这个表达式简化为我们四个测量值的算术平均值。也许你一直认为应该这样做。现在，我们有了使用算术平均值的另一个理由：它最小化了简单的最小二乘准则。
最后，我们应该注意我们做出的两个重要假设。首先，我们假设了我们的测量模型是线性的。这是一个非常重要的假设，在复杂系统中经常被打破。我们将在后续课程中讨论非线性测量模型。其次，我们假设了所有测量值在我们的误差方程中具有相等的权重。换句话说，我们假设我们同等关心每一个测量值。
在本节课中，我们一起学习了最小二乘法由卡尔·弗里德里希·高斯首创，并被他用于精确预测名为谷神星的新行星状天体的轨道。我们看到了如何通过最小化最小二乘准则来求解感兴趣的参数值。我们还注意到，普通最小二乘法假设了一个线性测量模型，并且无法处理重要性不等的测量值。
在下一个视频中，我们将把最小二乘法扩展到加权最小二乘法，该方法能够考虑重要性不同的测量值。
011：平方误差准则与最小二乘法
在本节课中，我们将要学习如何处理质量不同的测量数据。我们将从上一节讨论的普通最小二乘法出发，引入加权最小二乘法，并推导其核心公式。通过本课，你将能够理解如何为不同质量的测量值分配不同的权重，从而获得更准确的参数估计。
概述
在上一节视频中，我们学习了如何使用最小二乘法，根据一组含有噪声的测量值来求解电阻的更准确值。本节视频中，我们将探讨一个问题：如果我们怀疑某些测量值的质量优于其他测量值，我们应该怎么做？
通过本节的学习，你将能够推导并最小化加权最小二乘准则，这种方法允许我们处理具有不同质量的测量值，并将其与上一节讨论的普通最小二乘法进行比较。
引入加权的原因
我们可能希望更信任某些测量值的一个原因是，它们可能来自更好的传感器。例如，我们的一部分电阻测量值可能来自比其它测量仪昂贵得多的万用表。
此外，从现在开始，我们也将放弃只估计一个参数的假设，转而推导更通用的正规方程。这将使我们能够制定一种同时估计多个参数的方法。例如，如果我们想同时估计多个电阻值。
通用模型与假设
让我们从使用以下通用符号开始。
我们将有一组 M 个测量值，它们通过一个线性模型与一组 N 个未知参数相关联：
z = Hx + v
这里，H 是雅可比矩阵，其形式和元素将取决于具体问题。
解释普通最小二乘法的一种方式是，我们隐含地假设每个噪声项 v_i 在测量之间是独立的随机变量，并且具有相等的方差（或标准差，如果你更喜欢的话），即上一节提到的独立同分布。
加权最小二乘准则
如果我们假设每个噪声项具有不同的方差，我们可以如下定义我们的噪声协方差矩阵 R：
R = E[vv^T] = diag(σ₁², σ₂², ..., σ_M²)
基于这个定义，我们可以定义加权最小二乘准则 J_wls：
J_wls(x) = (z - Hx)^T R^{-1} (z - Hx)
展开这个表达式，我们可以看到为什么称之为加权最小二乘：
J_wls(x) = Σ_{i=1}^{M} ( (z_i - h_i^T x)² / σ_i² )
现在，每个平方误差项都由相应测量值关联的方差的倒数进行加权。换句话说，噪声的方差越低，其关联的误差项在损失函数中的权重就越大。我们更关心来自低噪声测量的误差，因为这些测量应该更能告诉我们未知参数的真实值。
与普通最小二乘法的关系
在我们了解如何最小化这个新的加权准则之前，让我们看看如果将所有噪声标准差设置为相同的值 σ 会发生什么。
在这种情况下，我们可以将分母中的方差因子提取出来：
J_wls(x) = (1/σ²) * Σ_{i=1}^{M} (z_i - h_i^T x)² = (1/σ²) * J_ols(x)
由于 σ² 项是常数，它不会影响最小化过程。这意味着在方差相等的情况下，最小化我们加权最小二乘准则的参数也将最小化我们的普通最小二乘准则，正如我们所预期的那样。
最小化加权准则
回到我们的加权最小二乘准则，我们采用与之前相同的方法来最小化它：求导。在一般情况下，我们的粗体向量 x 中有 n 个未知参数，这个导数实际上是一个梯度：
∇_x J_wls = -2 H^T R^{-1} (z - Hx)
将梯度设为零向量，然后求解我们的最佳或最优参数向量 x̂：
H^T R^{-1} H x̂ = H^T R^{-1} z
这导出了另一组正规方程，这次称为加权正规方程。
应用示例
让我们看一个加权最小二乘法如何工作的例子。
我们将采用之前收集的相同数据，但现在假设最后两个测量值实际上是用噪声方差小得多的万用表测量的。
请注意，这里列出的数字是标准差，这就是它们以欧姆为单位的原因。为了在我们的公式中使用它们，我们需要对它们进行平方以获得方差。
定义我们的变量，然后评估我们的加权最小二乘解，我们可以看到最终得到的电阻值更接近更精确的万用表测量的值，正如预期的那样。
方法总结
以下是普通最小二乘法和加权最小二乘法的快速总结。
通过使用加权最小二乘法，我们可以改变每个测量值对最终估计的重要性。重要的是要能够熟练处理不同的测量方差，以及有时相关的测量值。自动驾驶汽车将搭载许多不同且复杂的传感器，我们需要确保正确建模我们的误差源。
本节总结
本节课中我们一起学习了加权最小二乘法。我们讨论了某些测量值可能来自具有更好噪声特性的传感器，因此应在我们的最小二乘准则中赋予更大的权重。基于这种直觉，我们推导了加权最小二乘准则以及相关的加权正规方程，求解该方程可以得到一组常数参数的加权最小二乘估计。
在下一节视频中，我们将研究如何修改最小二乘法以递归方式工作。即基于测量流计算最优估计，而无需事先获取整个数据集。当我们研究状态估计或估计随时间连续变化的量的问题时，这将非常重要。
012：递归最小二乘法
在本节课中，我们将讨论递归最小二乘法。这是一种在线计算最小二乘解的技术。课程结束时，你将能够：
- 将前两节视频中讨论的批量最小二乘解，扩展为可递归工作的形式。
- 使用递归最小二乘法，在新测量数据不断流入时，持续更新最小二乘解的估计值。
让我们开始吧。
从批量处理到递归处理
在前面的课程中，我们已经探讨了如何从一组测量值中计算某个未知但恒定参数的值。我们的一个假设是，我们手头拥有所有数据。也就是说，我们假设收集了一批测量值，并希望使用这些测量值来计算我们感兴趣的估计量。
这个假设有时是完全合理的。但是，如果我们面对的是一个数据流呢？每次收到新测量值时，我们是否需要重新计算最小二乘解？
例如，假设我们有一个每秒可以测量10次电阻的万用表。理想情况下，我们希望使用尽可能多的测量值来获得电阻的准确估计。然而，如果我们使用最小二乘法，求解正规方程所需的计算资源量将随着测量向量的大小而增长。
或者，我们可以尝试使用递归方法。这种方法会保留到上一个时间步为止收集的所有测量的最优参数估计值，然后根据当前时间步的测量值来更新该估计值。为此，我们使用递归算法，随着数据流入逐步更新我们的估计。
递归估计算法框架
假设我们在时间 K-1 拥有最优估计值。在时间 K，我们收到一个新的测量值，我们假设它遵循带有加性高斯噪声的线性测量模型。
我们的目标是，在给定新测量值和先前估计值的情况下，计算时间 K 的更新后最优估计。线性递归估计由以下表达式给出：
x̂_K = x̂_{K-1} + K_K (y_K - H_K x̂_{K-1})
这里，K_K 称为估计器增益矩阵。括号内的项称为新息，它量化了我们当前测量值与先前最佳估计值的匹配程度。
即使不知道 K 的表达式，我们也能看到这个递归结构是如何工作的。我们的新估计值就是旧估计值加上一个修正项，该修正项基于我们预期的测量值与实际测量值之间的差异。事实上，如果新息等于0，我们根本不会改变旧的估计值。
如何计算增益矩阵 K？
那么，我们如何计算 K 呢？为此，我们需要像以前一样使用递归最小二乘准则和一些矩阵微积分。这一次，数学推导要复杂得多，因此我们只介绍几个关键步骤，让更有好奇心的学习者参考教科书以获取更多信息。
在这种情况下，我们的最小二乘准则将是时间 K 估计值的误差平方的期望值。对于像电阻这样的单个标量参数，这相当于最小化估计器状态方差 σ²_K。对于多个未知参数，这相当于最小化时间 K 的状态协方差矩阵 P_K 的迹。
这与我们之前的最小二乘准则完全相同，只是现在我们必须讨论期望值。我们不直接最小化误差，而是最小化其期望值，这实际上就是估计器方差。方差越低，我们对估计值的把握就越大。
事实证明，我们可以为这个状态协方差矩阵 P_K 制定一个递归定义。通过使用矩阵微积分并求导，我们可以证明当 K 取以下值时，该准则被最小化：
K_K = P_{K-1} H_K^T (H_K P_{K-1} H_K^T + R_K)^{-1}
完整的推导有点超出本课程的范围，可以在任何标准的估计理论教材中找到。
协方差矩阵的递归更新
最后，利用这个公式，我们还可以将 P_K 的递归定义重写为更简单的形式：
P_K = (I - K_K H_K) P_{K-1}
花点时间思考一下这个方程。我们的增益矩阵 K 越大，新的估计器协方差 P_K 就越小。直观地说，你可以把这个增益矩阵看作是在平衡我们从先验估计中获得的信息和从新测量中获得的信息。
算法总结
将所有内容放在一起，我们的递归最小二乘算法如下所示：
以下是算法步骤：
- 初始化：用未知参数的初始估计值
x̂_0和相应的协方差矩阵P_0初始化算法。这个初始猜测可以来自我们进行的第一次测量，协方差可以来自技术规格。 - 建立模型：设置我们的测量模型
y = Hx + v，并为测量噪声协方差R选取值。 - 递归更新：每次记录到测量值
y_K时，我们计算测量增益K_K，然后使用它来更新我们的参数估计x̂_K和估计器协方差（不确定性）P_K。
每次我们获得新的测量值时，我们的参数不确定性就会缩小。
递归最小二乘法的重要性
为什么递归最小二乘法是一个重要的算法？正如我们所看到的，它使我们能够最小化估计过程中的计算量，这总是一件好事。
更重要的是，递归最小二乘法构成了线性卡尔曼滤波器的更新步骤。我们将在下一个模块中更详细地讨论这一点。
在接下来的分级评估中，你将获得一些实践经验，使用递归最小二乘法从一系列测量中确定电压值。
本节总结
本节课中我们一起学习了递归最小二乘法。该算法使我们能够在无需掌握整批测量数据的情况下，生成参数的实时估计值。递归最小二乘法是一种递归线性估计器，它最小化当前时刻参数的方差。
在本模块的最后一个视频中，我们将通过将最小二乘法与统计学中的另一种技术——最大似然估计——联系起来，讨论为什么最小化平方误差是合理的做法。
自动驾驶汽车的状态估计和定位：1.3：最小二乘法与最大似然估计法的联系
欢迎来到第一模块的最后一课。我们将通过讨论一个重要的联系来结束本模块，这将有助于加深对最小二乘法的理解。
具体来说，在本课结束时，你将能够阐述最小二乘法与高斯随机变量最大似然估计之间的联系。
让我们首先回顾本模块第一个视频中提到的最小二乘准则。我们通过确定能使测量误差平方和最小化的值，来找到一些未知但恒定参数的最佳估计。
但我们可以问，为什么是误差的平方？为什么不是立方误差、平方根误差或其他形式？这实际上是一个相当深刻的问题，有一个完整的稳健统计学领域专门研究它。确实可以使用不同的误差函数，但我们将讨论平方误差具有吸引力和相关性的两个原因。
第一个原因很简单。平方误差允许我们通过相对直接的代数运算来求解最优参数。如果测量模型是线性的，最小化平方误差准则就相当于求解一个线性方程组。
第二个原因与概率有关，并且在高斯噪声的假设下，最小二乘法与最大似然估计器之间存在深刻的联系。正如你可能猜到的，这种联系最初是由高斯以一种特定形式推导出来的，因此它涉及高斯随机变量（或等价的高斯噪声）也就不足为奇了。
为了理解这一基本联系，让我们首先讨论最大似然估计。
我们可以不写损失函数，而是通过提问“哪些参数最可能产生我们记录到的测量值？”来探讨最优参数估计问题。为了简单起见，我们将坚持使用单个标量参数来建立直觉。
例如，让我们再次看看测量电阻的例子。根据我们对概率的了解，如果我们有四个可能的未知电阻参数值 (X_A) 到 (X_D)，并且每个值都会产生以下关于我们测量值 (y) 的条件概率。那么，给定测量值 (y_{meas})，哪个值能使条件似然最大化？没错，是 (X_A)。在测量位置处，最高的概率密度由绿色曲线给出，这意味着 (X_A) 是给定此测量值的最可能参数值。
现在，如果我们采用简单的测量模型，并通过假设加性噪声的密度，可以将其转换为概率密度。未知参数 (X) 成为该密度的均值，方差就是我们的噪声方差。回想一下，高斯随机变量的概率密度由以下公式给出：
[
p(y) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(y - \mu)2}{2\sigma2}\right)
]
这意味着我们可以将单次测量的测量似然表示如下：
[
p(y|X) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(y - X)2}{2\sigma2}\right)
]
如果我们有多个独立测量，每个测量具有相同的噪声方差，我们可以简单地将多个高斯分布相乘，结果得到另一个高斯分布。
我们尝试关于均值参数 (\hat{X}) 最大化这个似然。为此，我们将使用优化中常用的一种技术：不直接最大化似然，而是取其对数并最大化该对数。由于似然总是一个正数，并且因为对数是一个单调递增函数，这不会影响最终结果，这非常方便。
将对数应用于此似然后，我们看到出现了一些看起来很像误差平方和的东西：
[
\log p(y_1, ..., y_N|X) = C - \frac{1}{2\sigma^2} \sum_{i=1}^{N} (y_i - X)^2
]
表达式中的常数 (C) 指的是不依赖于 (X) 的项，我们可以安全地忽略它们。
最后一步是认识到，函数 (f) 的 (\arg\max) 与函数 (-f) 的 (\arg\min) 相同。利用这个事实，我们可以将似然最大化问题转化为误差平方和的最小化问题。
看，最小化误差平方和等价于最大化一组测量的似然，前提是假设测量值被具有相同方差的加性独立高斯噪声所破坏。
此外，如果我们保持相同的假设，但改变每次测量的高斯噪声方差，我们可以得到与加权最小二乘法视频中看到的相同准则。
因此，在这两种情况下，对于加性高斯噪声，最大似然估计给出的结果等同于我们之前推导的最小二乘或加权最小二乘解。
那么，为什么这个结果如此重要？我们的自动驾驶汽车将不得不处理许多许多误差源，其中一些非常难以建模。然而，中心极限定理告诉我们，当所有这些误差结合在一起时，它们可以合理地用一个单一的高斯误差分布来建模。我们希望以概率方式对系统进行建模，同时保持计算的简洁性。如果我们的误差是高斯分布的，那么感兴趣参数的最佳最大似然估计正是我们已经熟悉的最小二乘解，非常简单。
最后，让我们讨论一下这种方法的一个重要注意事项。当我们使用最小二乘法时，测量异常值会对我们的最终估计产生显著影响。
要理解原因，请考虑在高斯分布下，一个距离均值两个标准差的样本出现的概率小于5%。因此，如果我们的测量数据中存在一些异常值，最大似然法以及等价的最小二乘法会赋予这些测量值很大的重要性。因此，我们参数的估计值会被这些异常值强烈地拉偏。我们的最优方法会发生偏斜，使得异常测量值看起来更可能发生。
这种情况可能发生在使用自动驾驶车辆传感器数据的许多估计器中。例如，异常值可能来自走在激光雷达扫描中间的人，或者来自不良的GPS信号。考虑我们的电阻示例，一个可能来自简单事故的异常测量值，会将最终估计值显著拉离无异常值情况下的结果。
我们始终需要警惕异常值，并尽可能在盲目应用最大似然法或最小二乘法之前量化误差分布。
既然我们已经推导出最大似然估计与最小二乘法之间的联系，我们现在就可以将递归最小二乘估计器扩展到完整的卡尔曼滤波器——20世纪最著名的算法之一。
总结一下，我们了解到，在高斯噪声的假设下，最小二乘法和加权最小二乘法产生的估计与最大似然估计相同。这一点尤其重要，因为许多复杂的噪声源在相加时往往会趋向于高斯分布。然而，始终需要警惕可能显著影响最终估计值的异常测量。
在下一个模块中，我们将看看如何将我们学到的关于最小二乘法和参数估计的知识，扩展到可能具有更复杂非线性模型的持续变化的状态。我们下次见。
014：线性卡尔曼滤波
欢迎回来。在本模块中，我们将学习工程领域最著名的算法之一：卡尔曼滤波。在当今先进的机器学习世界中，卡尔曼滤波仍然是一个重要工具，用于融合来自多个传感器的测量数据，以实时估计机器人系统（如自动驾驶汽车）的状态。在本模块中，我们将了解卡尔曼滤波的一些历史，并介绍其基本的线性公式。我们将阐述为何卡尔曼滤波是最佳线性无偏估计器，然后通过线性化将线性公式扩展到非线性系统。讨论这种线性化方法的局限性。最后，通过无迹变换介绍一种现代替代线性化的方法。
在本节课结束时，你将能够：将卡尔曼滤波描述为一个分两个阶段（预测和校正）工作的状态估计器。理解运动模型和测量模型之间的区别。然后在一个简单的一维定位示例中使用卡尔曼滤波公式。
让我们从一点历史开始。卡尔曼滤波算法由匈牙利裔教授兼工程师鲁道夫·E·卡尔曼于1960年发表，当时他在马里兰州巴尔的摩的高级研究所工作。多年后，在2009年，美国总统巴拉克·奥巴马授予卡尔曼国家科学奖章，以表彰他在卡尔曼滤波及其他控制工程领域的贡献。1960年发表后，卡尔曼滤波被美国国家航空航天局采用，用于阿波罗制导计算机。正是这项开创性的创新，在成功将阿波罗飞船送上月球以及我们在另一个世界迈出第一步的过程中发挥了关键作用。该滤波器帮助阿波罗飞船精确地绕月轨道飞行。美国国家航空航天局艾姆斯研究中心的工程师们采用了卡尔曼的线性理论，并将其扩展到非线性模型。我们将在接下来的模块中探讨这个具体的扩展。但首先，让我们谈谈基本的线性卡尔曼滤波。卡尔曼滤波与我们之前讨论的线性递归最小二乘滤波器非常相似。递归最小二乘法更新静态参数的估计，而卡尔曼滤波能够更新一个演变状态的估计。
卡尔曼滤波的目标是获取状态的概率估计，并使用两个步骤实时更新它：预测和校正。为了使这些概念更具体，让我们考虑一个估计车辆一维位置的问题。从时间 K-1 的初始概率估计开始。我们的目标是使用一个运动模型（可以来自车轮里程计或惯性传感器测量）来预测我们的新状态。然后，我们将使用一个观测模型（例如来自GPS）来校正时间 K 的车辆位置预测。这些组成部分——初始估计、预测状态和最终校正状态——都是随机变量，我们将通过它们的均值和协方差来指定。这样，我们可以将卡尔曼滤波视为一种融合来自不同传感器信息的技术，以产生某个未知状态的最终估计，同时考虑到运动和测量中的不确定性。
对于卡尔曼滤波算法，我们能够将运动模型写成以下形式：
公式：
x_k = A * x_{k-1} + B * u_k + w_k
时间步 K 的估计是时间步 K-1 的估计、一个控制输入和一些零均值噪声的线性组合。输入是影响我们系统状态演变的外部信号，在自动驾驶汽车的背景下，这可能是用于加速和变道的车轮扭矩。接下来，我们还需要定义一个线性测量模型。最后，我们将像以前一样需要一个测量噪声，以及一个过程噪声，它决定了我们对线性动态系统实际正确的确定程度，或者等效地，我们对控制输入效果的不确定程度。
一旦我们掌握了系统，我们可以使用一种与我们之前在递归最小二乘视频中讨论的方法非常相似的方法，只是这次我们将分两步进行。首先，我们将使用过程模型来预测我们的状态（请注意，我们现在通常讨论的是演变状态，而不是状态参数）自上一个时间步以来的演变，并传播我们的不确定性。其次，我们将使用我们的测量值，基于我们的测量残差（或新息）和我们的最优增益来校正该预测。最后，我们将使用增益将状态协方差从我们的预测传播到我们的校正估计。
在我们的符号中，帽子（^）表示特定时间步的校正后预测，而勾号（ˇ）表示在纳入测量之前的预测。如果你以前使用过卡尔曼滤波，你可能也见过分别用加号和减号表示校正量和预测量的写法。
让我们回顾一下。我们从时间步 k-1 的状态（可能还有参数）的概率密度开始，我们将其表示为多元高斯分布。然后，我们使用线性预测模型预测时间步 K 的状态，并将均值和不确定性（协方差）向前传播。最后，使用我们的概率测量模型，我们通过最优增益矩阵 K 最优地融合来自测量的信息与先前的预测，来校正我们的初始预测。我们的最终结果是时间步 K 状态的更新概率估计。
熟悉卡尔曼滤波的最佳方法是使用它。让我们看一个简单的例子。再次考虑自动驾驶汽车估计自身位置的情况。我们的状态向量将包括车辆位置及其一阶导数速度。我们的输入将是一个标量加速度，可能来自命令汽车向前或向后加速的控制系统。对于我们的测量，我们假设能够使用类似GPS接收器的东西直接确定车辆位置。最后，我们将如下定义我们的噪声方差。给定这个初始估计和我们的数据，当我们使用卡尔曼滤波执行一次预测步骤和一次校正步骤后，我们校正后的位置估计是多少？
以下是我们如何使用这些定义来求解校正后的位置和速度估计。请注意，我们最终校正后的状态协方差更小。也就是说，在纳入位置测量后，我们对汽车的位置更加确定。这种不确定性的减少是因为我们的测量模型相当准确。也就是说，测量噪声方差非常小。尝试增加测量方差，观察最终状态估计会发生什么变化。
总而言之，卡尔曼滤波类似于递归最小二乘法，但还增加了一个定义状态如何随时间演变的运动模型。卡尔曼滤波分两个阶段工作。首先，使用运动模型预测下一个状态。其次，使用测量值校正这个预测。
但我们如何确定卡尔曼滤波为我们提供了准确的状态估计呢？在下一个视频中，我们将讨论卡尔曼滤波的一些吸引人的理论特性，这些特性使其成为工程领域的支柱。
015：卡尔曼滤波与偏差问题
在本节课中，我们将探讨卡尔曼滤波器的核心特性，理解它为何是一种强大的状态估计方法。我们将学习两个关键概念：偏差和一致性，并最终解释为何卡尔曼滤波器被称为最佳线性无偏估计器。
上一节我们介绍了卡尔曼滤波器，本节中我们来看看是什么特性使其成为一种极具吸引力的估计方法。
偏差
首先，让我们讨论偏差的概念。考虑使用上一课的卡尔曼滤波器来估计自动驾驶汽车的位置。
如果我们有办法知道车辆的真实位置，例如通过一个“先知”告知，我们就可以记录滤波器在每个时间步K的位置误差。由于我们处理的是随机噪声，仅进行一次实验是不够的。我们需要多次重复这个过程，并记录每个时间步的位置误差。
收集这些误差后，如果它们在特定时间步K的平均值趋近于0，那么我们称卡尔曼滤波器在该时间步的估计是无偏的。
从图形上看，情况可能如下所示。在特定时间步，我们知道真实位置。我们构建一个直方图，显示滤波器在多次试验中报告的位置，然后计算这些估计值的平均值与真实位置之间的差值。如果这个差值不趋近于0，那么我们的估计就是有偏的。然而，如果我们重复实验很多次，这个差值确实趋近于0，并且这种情况发生在所有时间间隔，那么我们称滤波器是无偏的。
虽然我们可以通过实验验证这种无偏性，但我们更希望有一些理论上的保证。考虑卡尔曼滤波器的动力学方程，定义预测和校正的状态误差，我们可以使用卡尔曼滤波器方程写出以下关系。
对于卡尔曼滤波器，我们可以证明这些误差的期望值恰好等于0。要使此结论成立，我们需要确保初始状态估计是无偏的，并且噪声是白噪声、不相关且零均值的。
虽然这对于线性系统是一个很好的结果，但请记住，这并不能保证在单次试验中我们的估计没有误差，只能保证误差的期望值为0。
一致性
卡尔曼滤波器还具有所谓的一致性。一致性意味着，对于所有时间步K，滤波器协方差矩阵 P_k 与我们的误差平方的期望值相匹配。
对于标量参数，这意味着我们估计的经验方差应与滤波器报告的方差相匹配。实际上，这意味着滤波器对其产生的估计既不过度自信，也不缺乏自信。
一个过度自信（因而不一致）的滤波器会报告一个过于乐观的协方差。也就是说，滤波器本质上会过分强调自身的估计，而对可能提供关键信息的未来测量更新不那么敏感。很容易看出，一个过度自信的滤波器可能会对自动驾驶汽车的性能产生负面或危险的影响。
正式证明一致性特性超出了本课程的范围。但可以确信，对于卡尔曼滤波器，只要我们的初始估计是一致的，并且噪声是白噪声、零均值的，那么所有估计都将是一致的。
最佳线性无偏估计器
综合以上内容，我们已经证明，在给定白噪声、不相关、零均值噪声的条件下，卡尔曼滤波器是无偏且一致的。基于这两个事实，我们说卡尔曼滤波器是BLUE，即最佳线性无偏估计器。它产生无偏估计，并具有可能的最小方差。
总结与展望
本节课中我们一起学习了偏差和一致性的定义，并证明了卡尔曼滤波器是无偏的、一致的，并且是最佳线性无偏估计器。这里的“最佳”指的是卡尔曼滤波器最小化了状态方差。
尽管这是一个非常好的结果，但大多数真实系统，对于自动驾驶汽车，我们通常需要估计非线性量，例如车辆在2D和3D中的位姿。为此，我们需要将线性卡尔曼滤波器扩展到非线性领域。我们将在下一课中完成这个任务。
自动驾驶汽车的状态估计和定位：第3课：迈向非线性世界：扩展卡尔曼滤波：03_02_01
在本节课中，我们将学习如何将卡尔曼滤波应用于非线性系统。我们将介绍扩展卡尔曼滤波，这是一种通过线性化来处理非线性问题的强大工具。我们将了解其核心概念、数学原理，并通过一个具体示例来演示其应用。
到目前为止，在本模块中，你已经学习了如何使用线性卡尔曼滤波器进行状态估计。你也了解到卡尔曼滤波器是最佳线性无偏估计器。然而，线性卡尔曼滤波器不能直接用于估计那些是测量值或控制输入的非线性函数的状态。例如，汽车的姿态包含其方向，而方向不是一个线性量。实际上，三维空间中的方向存在于一个球面上。因此，我们需要寻找其他方法。在本视频中，我们将学习卡尔曼滤波器的一个重要且广泛应用的变体，称为扩展卡尔曼滤波器。
扩展卡尔曼滤波器专为处理非线性系统而设计，通常被认为是状态估计的“主力军”，因为它被用于包括自动驾驶汽车在内的各种应用。通过本视频的学习，你将能够：描述扩展卡尔曼滤波器如何使用一阶线性化将非线性问题转化为线性问题；理解雅可比矩阵在扩展卡尔曼滤波器中的作用以及如何计算它们；并将扩展卡尔曼滤波器应用于一个简单的非线性跟踪问题。
滤波器的工作原理是，首先基于先前的状态和我们给予系统的任何输入，预测在某个时间步K的更新状态估计的均值和协方差。然后，滤波器使用测量模型来预测基于状态估计应该接收到什么测量值，并将这些预测与实际从传感器接收到的测量值进行比较。卡尔曼增益告诉我们如何权衡所有这些信息，以便将它们最优地组合成一个修正后的估计，即一个新的状态和一个更新的协方差。这有时被称为预测-校正架构。
正如我们在上一个视频中看到的，对于线性系统，卡尔曼滤波器实际上是所有可能估计器中的最佳选择。不幸的是，这里有一个问题：现实中并不存在线性系统。即使是一个非常简单的系统，比如施加电压的电阻，也并非真正线性，至少并非总是如此。在一定的电压范围内，电流是电压的线性函数并遵循欧姆定律。但随着电压升高，电阻发热，会以非线性方式改变电阻值。由于我们在实践中遇到的系统都是非线性的，这就提出了一个重要问题：我们还能将卡尔曼滤波器用于非线性系统吗？如果能，该如何使用？
扩展卡尔曼滤波器的核心概念是线性化非线性系统的思想。因此，扩展卡尔曼滤波器有时也被称为线性化卡尔曼滤波器。线性化一个系统意味着选择一个工作点，并找到该非线性函数在该点邻域内的线性近似。在二维空间中，这意味着找到函数在x等于a处的切线。从数学上讲，我们通过对函数进行泰勒级数展开来实现这一点。回想一下你的微积分课程，你可能记得泰勒级数展开是一种将函数表示为无穷级数的方法，其项由函数在单点处的导数计算得出。对于线性化，我们只对泰勒级数展开中的一阶项感兴趣。
让我们回到一般的非线性运动和测量模型，并尝试对它们进行线性化。我们应该选择什么作为泰勒展开的工作点呢？理想情况下，我们希望围绕状态的真实值对模型进行线性化，但我们无法做到这一点，因为如果我们已经知道状态的真实值，就不需要估计它了。因此，让我们选择次优方案：对于我们的运动模型，我们将围绕先前状态的后验估计进行线性化；对于测量模型，我们将围绕基于运动模型预测的当前状态进行线性化。这样，我们就得到了一个状态空间中的线性系统，而矩阵被称为系统的雅可比矩阵。正确计算这些矩阵是扩展卡尔曼滤波器算法中最重要和最困难的一步，也是最容易出错的地方。
但这些雅可比矩阵到底是什么呢？在向量微积分中，雅可比矩阵是一个向量值函数的所有一阶偏导数组成的矩阵。雅可比矩阵的每一列包含函数输出相对于某个给定输入的导数。直观地说，雅可比矩阵告诉你函数的每个输出沿着每个输入维度变化的速度有多快，就像标量函数的导数告诉你随着输入变化，输出变化的速度一样。雅可比矩阵实际上就是一阶导数在多维情况下的推广。这里有一个二维函数的雅可比矩阵的简单例子。雅可比矩阵捕获了两个输出变量分别相对于两个输入变量的一阶导数。熟悉推导雅可比矩阵的最佳方法就是练习。尝试推导向量值函数的雅可比矩阵。
现在，我们知道了如何计算扩展卡尔曼滤波器所需的雅可比矩阵。剩下的就是将它们代入我们标准的卡尔曼滤波器方程中。与我们在模块2中看到的卡尔曼滤波器方程相比，扩展卡尔曼滤波器方程有几个需要注意的不同之处。首先，在预测和校正步骤中，我们仍然使用非线性模型来传播状态估计的均值，并计算测量残差。这是因为我们围绕先前的状态估计对运动模型进行了线性化，并围绕预测的状态对测量模型进行了线性化。根据定义，线性化模型在工作点处与非线性模型完全重合。第二个不同之处是出现了与过程和测量噪声相关的雅可比矩阵。在许多情况下，这两个矩阵都是单位矩阵，因为噪声通常被假定为加性的，但情况并非总是如此。
到目前为止，这一切都非常抽象。因此，让我们通过一个实际使用扩展卡尔曼滤波器的具体示例来逐步讲解。我们将使用模块2中的同一个例子，但稍作改动。我们将跟踪沿轨道移动的汽车的位置和速度。但现在，我们不再接收周期性的GPS测量值来告知我们的位置，而是使用像相机这样的车载传感器来测量远处地标相对于地平线的方位角。我们将保持与原始示例中相同的线性运动模型，并假设我们知道地标的高度及其在全局参考系中的位置。由于我们的传感器测量的是一个角度，我们的测量模型对汽车的位置具有非线性依赖关系。我们将需要对测量模型进行线性化，并在我们的扩展卡尔曼滤波器中使用它。这个问题的雅可比矩阵看起来像这样。注意，这个问题中的矩阵与原始问题中的矩阵完全相同，因为我们的运动模型在状态上已经是线性的。同时，注意噪声雅可比矩阵都是单位矩阵，因为运动和测量模型都具有加性噪声。
尝试使用这里给出的数据，通过扩展卡尔曼滤波器估计时间1时车辆的位置。这是状态均值和协方差的预测步骤结果。注意，结果与线性卡尔曼滤波器的情况相同，因为运动模型实际上是线性的。对于校正步骤，这是你应该得到的结果。请记住，你应该使用非线性测量模型来计算测量残差，而不是线性化模型。另外，请注意，在这种情况下，即使状态估计的校正后均值与预测均值不同，校正后的协方差与预测协方差相比变化不大。这是因为在这个距离上，方位角变化缓慢，与GPS测量相比，它没有提供太多关于车辆状态的信息。
总结一下，我们看到扩展卡尔曼滤波器使用线性化来使卡尔曼滤波器适应非线性系统。在接下来的课程项目中，我们将遇到几种不同的非线性系统，可以对其应用扩展卡尔曼滤波器或其近亲无迹卡尔曼滤波器。线性化是通过围绕一个工作点进行一阶泰勒级数展开，计算非线性函数的局部线性近似来实现的。这需要几个雅可比矩阵，它们包含了一组一阶偏导数。
在下一个视频中，我们将讨论扩展卡尔曼滤波器的另一种形式，称为误差状态扩展卡尔曼滤波器。这将是本课程后面讨论在三维空间中估计车辆方向时的一个有用工具。
自动驾驶汽车的状态估计和定位：第4课：改进型扩展卡尔曼滤波 - 误差状态扩展卡尔曼滤波
在本节课中，我们将学习扩展卡尔曼滤波的一种变体——误差状态扩展卡尔曼滤波。学完本节，你将能够描述误差状态扩展卡尔曼滤波的公式，并阐述其相较于上一节介绍的经典扩展卡尔曼滤波的优势。
上一节我们介绍了扩展卡尔曼滤波，它通过局部线性化的方法，使得卡尔曼滤波方程能够应用于非线性系统。本节中，我们来看看一种名为误差状态扩展卡尔曼滤波的变体，它具备一些优良特性，将在后续课程中派上用场。
误差状态扩展卡尔曼滤波的核心思想非常简单。我们将车辆状态 X 视为由两部分组成：一个称为名义状态 X̂ 的主要部分，和一个称为误差状态 ΔX 的较小部分。
我们可以用一个简单的汽车位置跟踪例子来理解。绿色线代表汽车的真实位置，这是我们试图估计的量。红色线代表名义状态，即基于我们对汽车运动模型以及加减速输入的最佳猜测。当然，运动模型从不完美，总存在一些随机过程噪声。这些误差会随着我们积分运动模型而随时间累积。我们可以将误差状态视为所有这些建模误差和过程噪声随时间累积的地方。因此，误差状态就是名义状态与真实状态在任何给定时刻的差值。
如果我们能计算出误差状态，实际上就可以用它来修正名义状态，使其更接近真实状态。
在误差状态扩展卡尔曼滤波中，我们不再对可能具有复杂非线性行为的完整状态进行卡尔曼滤波，而是使用扩展卡尔曼滤波来估计误差状态，然后利用误差状态的估计值来修正名义状态。
从数学上讲，这意味着我们要重新排列线性化的运动模型，使其成为一个方程，能够告诉我们时刻K的真实状态与预测状态之间的差值，如何与时刻K-1的同一差值相关联。这些差值正是我们刚才讨论的误差状态 Δx_K 和 Δx_{K-1}，描述它们关系的方程称为误差状态运动学。
我们也可以直接用误差状态来重新表达线性化的测量模型。
我们可以用一种与经典扩展卡尔曼滤波非常相似的方式来使用这种误差状态公式。
以下是误差状态扩展卡尔曼滤波的步骤：
首先，我们使用非线性运动模型和当前对状态的最佳估计来更新名义状态。在获得测量值进行修正之前，我们可以多次执行此步骤。因此，当前最佳估计可能是 X̌ 或 X̂。
我们还需要跟踪状态协方差，它会随着我们从运动模型中积分越来越多的过程噪声而增长。同样，先前的协方差估计可能是 P̌ 或 P̂，这取决于我们是否使用了测量值进行修正。我们可以重复此循环，根据需要不断更新名义状态和误差状态协方差，直到收到测量值并希望进行修正。
当需要进行修正时，我们可以像往常一样计算卡尔曼增益，然后利用卡尔曼增益、测量值以及我们的非线性测量模型来计算误差状态的最佳估计。
现在，这里的情况略有不同。一旦我们有了误差状态均值的估计，我们希望用它来更新名义状态并修正误差。我们可以通过简单地将误差状态的估计值加到名义状态上来实现，从而得到完整状态的修正后状态估计。
最后，我们可以使用常规方程更新状态协方差。
这个过程会一直持续下去，至少持续到车辆耗尽燃料为止。
那么，在实践中我们为何要使用误差状态扩展卡尔曼滤波呢？主要有两个很好的理由。
一个原因是它通常比经典扩展卡尔曼滤波表现更好，因为较小的误差状态比较大的名义状态更适合线性滤波，而名义状态我们可以进行非线性积分。
另一个原因是，误差状态公式使得处理像旋转这样的受约束量变得更加容易，这在课程后面会很有用。原因在于，我们不一定必须使用简单的向量加法来分解状态。事实上，只要它为我们提供了一种将小扰动纳入名义状态的一致方法，我们可以使用任何广义的组合操作。
如果现在听起来有点抽象，请不要担心。在课程后面，我们将使用误差状态扩展卡尔曼滤波来帮助处理三维空间中的旋转，这是一种非常常见的受约束量类型。
让我们来总结一下。
扩展卡尔曼滤波的误差状态公式将车辆状态分为一个大的名义状态和一个小的误差状态。名义状态跟踪运动模型预测的状态应该是什么，而误差状态则捕获随时间累积的建模误差和过程噪声。在误差状态扩展卡尔曼滤波中，我们估计这个小误差状态，并将其用作对名义状态的修正。这是误差状态扩展卡尔曼滤波与估计完整状态的经典扩展卡尔曼滤波之间的主要区别。请记住，两种公式仍然依赖于局部线性化。
误差状态扩展卡尔曼滤波相较于经典扩展卡尔曼滤波有几个优势。首先，它通常表现更好，因为误差状态的演变往往更接近线性。其次，误差状态公式使得处理像三维旋转这样的特殊量变得更加容易，正如我们将在课程后面看到的那样。
在下一个视频中，我们将讨论扩展卡尔曼滤波的一些缺点，以及局部线性化如何可能失效，从而为自动驾驶汽车带来潜在的危险情况。
018：扩展卡尔曼滤波的局限性
在本节课中，我们将要学习扩展卡尔曼滤波（EKF）在实际应用中的主要局限性。我们将通过一个具体的非线性变换例子，来理解线性化误差如何影响状态估计的均值和协方差，并探讨这些局限性可能带来的后果。
在上一节中，我们介绍了扩展卡尔曼滤波（EKF）及其如何将线性卡尔曼滤波应用于非线性系统。本节中，我们来看看EKF方法存在的一些重要限制。
虽然EKF在许多实际问题中表现良好，但在决定是否采用EKF时，必须牢记其一些重要的局限性。
EKF的工作原理是通过线性化系统的非线性运动模型和观测模型，来更新状态估计的均值和协方差。线性化模型只是真实非线性模型的局部线性近似。我们将近似值与真实模型之间的差异称为线性化误差。
通常，任何函数的线性化误差取决于两个因素。
第一个因素是原始函数本身的非线性程度。如果我们的非线性函数变化非常缓慢或大部分时间相当平坦，线性近似将是一个相当好的拟合。反之，如果函数变化很快，线性近似在其大部分定义域内都无法很好地捕捉函数的真实形状。
第二个因素是距离线性化操作点的远近。距离操作点越远，线性近似与真实函数偏离的可能性就越大。
线性化误差的这些特性对EKF有重要影响。让我们通过一个例子，看看线性化误差如何影响经过一对常见非线性函数变换后的两个随机变量的均值和协方差。
具体来说，我们来看从极坐标（R, θ）到笛卡尔坐标（X, Y）的非线性变换。这种变换通常用于处理激光扫描仪或激光雷达（LiDAR）报告的距离和方位测量值，正如许多自动驾驶汽车上的扫描仪所做的那样。
我们从极坐标区间上的均匀分布中抽取若干样本，并考虑将这些样本变换到笛卡尔坐标时会发生什么。
为了说明这种情况，我们将均匀分布的随机样本绘制为左侧图中的蓝色点。当我们将这些样本点中的每一个变换为相应的笛卡尔坐标时，我们看到变换后的分布呈现出右侧图所示的香蕉形状。
显然，仅使用均值和协方差无法完全捕捉香蕉形状分布的特征。但让我们看看原始概率密度函数的均值和协方差会发生什么变化。
在左侧图中，均匀分布的真实均值显示为中间的绿点，真实协方差由绿色椭圆表示。
将均匀分布变换到笛卡尔坐标后，输出分布的均值落在这里，其协方差看起来像这样。
现在，考虑如果我们围绕原始分布的均值对极坐标到笛卡尔坐标的变换进行线性化，然后再次变换所有样本点会发生什么。线性化变换给出的输出分布看起来像是另一个均匀分布，与我们之前看到的香蕉形状分布几乎没有相似之处。
如果我们观察这个分布的均值（用红点表示），它落在上方某处。线性化的协方差看起来也大不相同。
让我们比较一下线性化和非线性化的输出分布。可以看到，线性化分布的均值与真实均值的位置非常不同。并且线性化协方差严重低估了真实输出分布在Y维度上的扩展范围。
因此，在这种情况下，我们可以看到线性化误差可能导致我们对输出分布的认知完全偏离目标。这会给我们的估计器带来大问题。
例如，从激光扫描仪估计出的车道标记位置可能与实际位置存在显著差异，导致我们的汽车越过中心线。那么，什么时候线性化会成为EKF的问题呢？
有两种情况需要注意。
第一种情况是当系统动力学高度非线性时。如果你的运动和测量模型接近线性，线性化误差不会是大问题。但如果它们不是线性的，你的线性化估计器将无法很好地捕捉系统的真实行为。
第二种情况与你试图使用线性化模型时距离操作点的远近有关。如果你的自动驾驶汽车相对于传感器采样时间移动得非常快，你将比它缓慢移动时获得更多的线性化误差。
从数学上讲，EKF中的线性化误差有两个重要后果，我们在前面的例子中都看到了。
- 估计的均值可能与真实状态大不相同。
- 估计的协方差可能无法准确捕捉状态中的真实不确定性。
换句话说，这意味着线性化误差可能导致我们的估计器对一个完全错误的答案过度自信。在最坏的情况下，估计器可能会发散，即完全迷失方向，无法提供有用的输出。
更糟糕的是，一旦像EKF这样的估计器发散，就无法将其拉回正轨。通常，如果可能，你不得不重新初始化。这对于处理自动驾驶汽车时的安全性来说是一个巨大的问题。如果你的汽车估计器在行驶过程中发散，它很容易驶离道路冲进沟里，或者导致与另一辆车相撞。
因此，线性化误差是EKF的主要理论局限性。但在我们的实现中还会遇到其他常见问题。
实践中最常见的问题可能是在计算雅可比矩阵时很容易出错，尤其是当我们处理复杂的非线性系统时。在推导雅可比矩阵时漏掉一个负号，可能导致无数小时的调试。有些人试图通过在运行时使用数值微分或在编译时使用自动微分来规避这个问题，但这些方法也有其自身的缺陷，有时可能表现不可预测。
除此之外，如果我们的一个或多个模型甚至不可微怎么办，比如步进电机的阶跃函数响应。所有这些缺点引出了一个问题：我们真的需要线性化来进行非线性卡尔曼滤波吗？
本节课中我们一起学习了扩展卡尔曼滤波（EKF）基于其解析局部线性化方法所带来的几个局限性。如果被建模系统的动力学高度非线性或线性化误差很大，滤波器可能会发散。EKF需要计算雅可比矩阵，这通常是一个繁琐且容易出错的过程。尽管如此，EKF是状态估计中一个经过验证的工具，作为自动驾驶汽车工程师，你无疑会经常遇到它。
在下一个视频中，我们将讨论另一种卡尔曼滤波变体，称为无迹卡尔曼滤波（UKF），它在很大程度上解决了EKF的这些局限性。
019：无迹卡尔曼滤波
在本节课中，我们将要学习一种处理非线性系统的卡尔曼滤波替代方案——无迹卡尔曼滤波。我们将了解其核心思想“无迹变换”的工作原理，并将其与扩展卡尔曼滤波进行比较，最后学习如何将其应用于一个简单的非线性跟踪问题。
无迹变换的动机
在上一节中，我们看到了线性化误差如何导致扩展卡尔曼滤波产生与真实状态值差异很大的状态估计，以及无法准确捕捉状态不确定性的协方差。在依赖扩展卡尔曼滤波的安全关键应用（如自动驾驶汽车）中，这可能是一个大问题。
无迹卡尔曼滤波是一种非线性卡尔曼滤波的替代方法，它依赖于一种称为“无迹变换”的技术，将概率分布通过非线性函数进行传递。我们将看到，对于相似的计算量，无迹变换比扩展卡尔曼滤波的分析线性化方法提供更高的精度，并且无需计算任何雅可比矩阵。
无迹变换的直观理解
无迹变换背后的直觉很简单：近似一个概率分布通常比近似一个任意的非线性函数要容易得多。
考虑一个简单的例子：一个一维高斯分布（如左侧所示）通过一个非线性函数变换成一个更复杂的一维分布（如右侧所示）。我们已经知道输入高斯分布的均值和标准差，我们希望利用这些信息和非线性函数来计算出输出分布的均值和标准差。无迹变换为我们提供了一种实现此目的的方法。
无迹变换的三个步骤
无迹变换的基本思想包含三个步骤。
以下是具体步骤：
- 选择Sigma点：首先，我们从输入分布中选择一组样本点。这些不是随机样本，而是确定性样本，被选择为距离均值特定数量的标准差。因此，这些样本被称为Sigma点，无迹变换有时也被称为Sigma点变换。
- 非线性变换：一旦我们拥有了一组精心选择的Sigma点，第二步也是最简单的一步，就是将每个Sigma点通过我们的非线性函数，产生一组属于输出分布的新Sigma点。
- 计算统计量：最后，我们可以使用一些精心选择的权重计算输出Sigma点的样本均值和协方差，这些将为我们提供真实输出分布均值和协方差的良好近似。
深入Sigma点
现在您已经了解了无迹变换的基本思想，让我们详细看看这些步骤。您可能首先会问，我们需要多少个Sigma点，以及哪些点才是Sigma点。
一般来说，对于一个n维概率分布，我们需要 2n + 1 个Sigma点：一个用于均值，其余对称地分布在均值周围。左侧的图表展示了一维和二维示例的Sigma点。在一维中，我们需要3个Sigma点；在二维中，我们需要5个。
确定Sigma点位置的第一步是取与输入分布相关的协方差矩阵的Cholesky分解。Cholesky分解本质上是对称正定矩阵（如协方差矩阵）的一种平方根运算。实际上，如果输入概率分布是一维的，Cholesky分解实际上就是方差的平方根，也就是标准差。我们不会深入讨论如何计算Cholesky分解，但您可以使用MATLAB中的 chol 函数或NumPy中的 cholesky 函数。
一旦我们分解了协方差矩阵，我们可以选择第一个Sigma点为分布的均值，其余的为均值加上或减去某个因子乘以我们从Cholesky分解得到的矩阵L的每一列。这里的n值再次是概率分布的维度数。参数Kappa是一个您可以自由设置的调优参数。对于我们处理的高斯分布，设置 n + Kappa = 3 是一个不错的选择。
重组变换后的Sigma点
现在我们有了Sigma点集合。下一步很简单，只需将每个Sigma点通过我们的非线性函数，得到一组新的变换后的Sigma点。
现在剩下的就是重新组合变换后的Sigma点，以找到我们的输出均值和输出协方差。我们使用您在基础统计学课程中见过的样本均值和协方差的标准公式来实现。诀窍在于，每个点在均值和协方差计算中都有一个特定的权重，该权重取决于参数Kappa和输入分布的维度n。
无迹变换实例
为了看无迹变换的实际效果，让我们回到上一节的例子，在那里我们将极坐标下的均匀分布非线性地变换到笛卡尔坐标。让我们看看无迹变换与分析线性化方法相比如何。
这里再次用绿色显示了两个分布的真实均值和协方差。现在，让我们应用无迹变换。我们输入分布的维度是2，所以我们需要5个Sigma点，在图中显示为橙色星号。将这些Sigma点通过我们的非线性函数变换后，它们位于此处。我们从变换后的Sigma点计算出的均值和协方差如图所示，显示为橙色。
请注意，我们使用无迹变换估计的均值几乎与真实的非线性均值完全相同，我们估计的协方差也几乎与真实协方差完全匹配。将其与分析线性化变换后的均值和协方差（红色）进行比较，两者都与真实均值和真实协方差有很大不同。从这个例子很容易看出，无迹变换为我们提供了输出概率分布的更好近似，而所需的工作量并不比分析线性化方法多多少。
无迹卡尔曼滤波
既然我们已经了解了无迹变换的工作原理，我们就可以轻松地将其用于我们的卡尔曼滤波框架中，以处理非线性模型。这种卡尔曼滤波的变体被称为无迹卡尔曼滤波，您也可能听到它被称为Sigma点卡尔曼滤波。
UKF的主要思想是，不像扩展卡尔曼滤波那样通过线性化来近似系统方程，我们使用无迹变换来直接近似概率分布。
让我们看看UKF的预测步骤。为了通过运动模型将状态和协方差从时间 K-1 传播到时间 K，我们使用当前对状态均值和协方差的最佳猜测来应用无迹变换。
以下是预测步骤：
- 首先，我们分解时间
K-1的估计状态协方差。 - 然后，我们计算以时间
K-1的估计状态均值为中心的Sigma点。 - 其次，我们通过非线性运动模型传播我们的Sigma点，得到一组新的用于时间
K预测状态的Sigma点。 - 最后，我们计算时间
K状态的预测均值和协方差。
在这一点上，通过将过程噪声的协方差加到变换后Sigma点的协方差中来得到最终的预测协方差是很重要的。如果过程噪声不是加性的，这个方程看起来会有点不同，但大多数时候，除非有充分的理由，否则您处理的模型中的噪声都是加性的。
无迹卡尔曼滤波的校正步骤
对于校正步骤，我们将遵循类似的程序，这次使用非线性测量模型。
以下是校正步骤：
- 首先，我们将从非线性运动模型得到的Sigma点集合代入我们的非线性测量模型，得到另一组用于预测测量的Sigma点。
- 然后，我们可以使用样本均值和协方差公式来估计预测测量的均值和协方差。
- 再次注意，我们加入了测量噪声协方差以获得预测测量的最终协方差，并且记住这个公式仅适用于加性噪声。
- 为了计算卡尔曼增益，我们还需要预测状态和预测测量之间的互协方差，它告诉我们测量值与状态的相关程度。您可以使用互协方差的标准公式并采用与之前相同的权重来计算它。
- 然后剩下的就是使用卡尔曼增益来最优地校正预测状态的均值和协方差，这样就完成了。
UKF遵循与EKF相同的预测-校正模式，但我们只是用无迹变换取代了分析线性化步骤。
应用示例
让我们尝试将UKF应用到我们之前用EKF处理过的相同驾驶场景示例中。我们再次尝试跟踪一辆通过踩油门或刹车控制的移动汽车的位置和速度。汽车上有一个传感器，可以测量远处地标与地平线之间的角度。运动模型是线性的，但测量模型是非线性的。
尝试使用此处给出的数据，利用UKF估计车辆在时间 K=1 时的位置。这是初始协方差矩阵的Cholesky分解，以及我们将用于表示状态及其协方差的五个Sigma点。这是状态均值的预测步骤结果。最后是预测协方差。请注意，预测的均值和协方差与我们使用线性卡尔曼滤波和扩展卡尔曼滤波得到的结果相同。这是因为运动模型实际上是线性的。
对于校正步骤，这些是预测测量的Sigma点以及该分布的均值和协方差。最后是互协方差、卡尔曼增益以及我们对车辆在时间 K=1 时位置的最终答案。
总结与回顾
总结本视频，我们研究了无迹卡尔曼滤波，它使用无迹变换使卡尔曼滤波适应非线性系统。正如我们所看到的，无迹变换通过将一小组精心选择的样本通过非线性系统并计算输出的均值和协方差来工作。对于相似的计算成本，它在近似输出分布方面通常比EKF使用的局部分析线性化技术做得更好。
让我们回顾一下在本模块中学到的内容。我们首先讨论了线性卡尔曼滤波，它是一种递归最小二乘估计形式，允许我们将运动模型的信息与传感器测量的信息结合起来估计车辆状态。卡尔曼滤波遵循预测-校正架构：运动模型用于预测状态，测量用于校正这些预测。我们还看到卡尔曼滤波是最佳线性无偏估计器，即卡尔曼滤波是仅使用测量值线性组合的最佳无偏估计器。
但是，当然，现实中并不真正存在线性系统，因此我们需要开发处理非线性系统的技术。在本模块中，我们研究了三种不同的非线性卡尔曼滤波方法：扩展卡尔曼滤波、EKF的误差状态公式以及无迹卡尔曼滤波。正如我们所讨论的，主要区别在于EKF依赖于局部分析线性化来通过非线性函数传播概率分布（无论是使用全状态还是误差状态公式）。相比之下，UKF依赖于无迹变换来处理非线性函数。
对于大多数仅轻度非线性的系统，EKF将给出准确的结果。但在线性化误差对EKF构成问题的情况下，UKF将更准确。误差状态卡尔曼滤波的性能介于两者之间。UKF相对于任何EKF公式的最大优势之一是，UKF不需要您计算非线性模型的任何导数，而这容易导致人为错误或数值不稳定。最后，在速度方面，对于典型的估计问题，EKF以微弱优势胜出。但总的来说，EKF和UKF需要非常相似的计算量。
由于其准确性和简单性，我们建议在您的项目中尽可能使用UKF而不是EKF。如果必须使用EKF，我们的建议是使用误差状态公式，警惕线性化误差，并格外小心以确保您的雅可比矩阵是正确的。
一个好的口诀是：警惕线性化误差。
现在您已经学习了状态估计所需的基本工具，我们可以开始思考在自动驾驶汽车上可能找到的传感器类型，以及如何使用它们来定位车辆。在下一个模块中，我们将讨论一对常见的传感器：惯性测量单元和全球导航卫星系统。
020：三维几何与参考坐标系
在本节课中，我们将学习三维几何与参考坐标系的基础知识。这是理解后续传感器工作原理和车辆定位算法的关键前提。
概述
本节课我们将回顾三维运动学中的核心概念。首先，我们会探讨参考坐标系如何影响向量的坐标表示。接着，我们将对比几种不同的旋转表示方法。最后，我们会介绍在自动驾驶定位中至关重要的几个参考坐标系。
向量与参考坐标系
一个向量是一个具有大小和方向的几何对象。我们常常将向量本身与它的坐标表示（即一组代表其方向和大小的数字）混为一谈。然而，这种理解并不完全正确。
想象一个在空间中固定的向量，当我们从不同角度观察它时，其坐标会发生变化。更准确地说，同一个向量量在不同的坐标系或参考系中，会拥有不同的坐标。
我们使用以下符号约定：在坐标系A中，向量R的坐标记为 R_A。同理，在坐标系B中，其坐标记为 R_B。
为了将一组坐标转换到另一组坐标，我们需要一个旋转矩阵，它精确地描述了一个坐标系相对于另一个坐标系的旋转关系。我们使用粗体大写字母 C 来表示旋转矩阵，其下标按从右到左的顺序表示初始和最终的参考坐标系。例如，C_AB 表示从坐标系B到坐标系A的旋转矩阵。
坐标变换与向量加法
在实际应用中，我们经常需要讨论点在不同参考系之间的坐标变化。例如，我们可能知道某个建筑物在一个固定坐标系中的位置，现在想求它在当前车辆坐标系中的位置。
计算这个位置需要使用向量加法，并确保所有坐标都在同一个参考系中表达。我们使用上标来表示三维向量的起点和终点（同样是从右到左），并使用下标来表示该向量所在的坐标系。例如，r_AB^V 表示在车辆坐标系V中表达的、从点B指向点A的向量。
我们可以通过操作这个表达式来求解车辆坐标系或某个惯性坐标系中的坐标。
旋转的表示方法
跟踪参考坐标系的一个关键部分是跟踪它们相对于某个基础参考系的方位或旋转。旋转是特别棘手的数学对象，如果处理不仔细，可能会成为重大错误的来源。
有多种方法可以表示旋转，以下是三种常见的方式。
1. 旋转矩阵
最常见的是使用一个3x3的旋转矩阵，正如我们之前所做的那样。这个矩阵通过点积定义了两个参考坐标系基向量之间的关系，因此它常被称为方向余弦矩阵。
旋转矩阵的一个重要性质是，它的逆矩阵就是它的转置矩阵：C^{-1} = C^T。
2. 单位四元数
四元数本身就是一个有趣的数学主题。对我们而言，只需知道一个单位四元数可以表示为一个单位长度的四维向量。它参数化了一个绕向量 u 定义的轴旋转角度 φ 的旋转。
我们可以通过一个略显繁琐但直接的代数表达式将四元数转换为旋转矩阵。
为什么我们要使用四元数？因为它们没有奇异性问题，并且只需要4个参数，而不是9个。在本课程的第5模块中，我们将需要将两个四元数相乘。重要的是，四元数乘法不同于普通的乘法，因为四元数是一种复数（实际上是超复数），因此适用不同的规则。四元数乘法运算符通常用一个带X的圆圈表示，并且可以用涉及每个四元数标量和向量部分的矩阵运算来表达。
与矩阵乘法类似，四元数乘法满足结合律，但通常不满足交换律，即乘法的顺序很重要。我们可以利用四元数乘法来表示连续的旋转。两个单位四元数 p 和 q 的乘积所对应的旋转矩阵，等同于分别由 p 和 q 定义的旋转矩阵的连续乘积。我们将在课程的最后模块中利用这一重要性质。
3. 欧拉角
另一种表示旋转的方法是使用三个称为欧拉角的数字。这些角度将一个任意旋转表示为绕不同主轴进行的三个独立旋转的组合。
欧拉角之所以有吸引力，部分原因在于它是一种简洁的表示法，只需要3个参数，而不是完整旋转矩阵所需的9个。然而，欧拉角表示法存在所谓的奇异性问题。奇异性使状态估计变得复杂，因为它代表了某些特定的旋转，从这些旋转出发，两个欧拉角是无法区分的。四元数和旋转矩阵没有这个问题，但代价是使用了更多的参数。
不同旋转表示法的比较
那么，自动驾驶工程师使用哪种旋转表示法呢？这取决于具体情况，每种方法都有其优缺点。
- 旋转矩阵：可以表示任何旋转，但需要9个参数并有6个约束条件。
- 单位四元数：也可以表示任何旋转，但它也有一个约束条件（单位长度）。要使用单位四元数实际旋转一个向量，除了简单的矩阵乘法外，还需要一些额外的代数运算。
- 欧拉角：无约束，直观易于可视化，且仅使用3个参数，但存在奇异性问题。
重要的参考坐标系
最后，让我们看看在定位车辆时将要用到的四个重要参考坐标系。
以下是这些坐标系的定义：
- 地心惯性坐标系：原点在地球中心。Z轴指向真北，X轴和Y轴相对于遥远的恒星固定。这意味着尽管地球绕Z轴旋转，但X轴和Y轴不随之移动。
- 地心地固坐标系：与ECIF类似，但其X轴与本初子午线对齐，并随地球一起旋转。Y轴由右手定则确定。
- 导航坐标系或当地切平面坐标系：这是一个固定在地面上的坐标系。一种非常常见的导航坐标系是附在某个已知起点上，其X轴指向北，Y轴指向东，Z轴指向下。这被称为NED坐标系。一个密切相关的坐标系是ENU坐标系，其X轴指向东，Y轴指向北，Z轴指向上。
- 传感器坐标系：这是一个刚性连接到传感器（如激光雷达、GPS接收器或惯性测量单元）上的坐标系。这个坐标系通常与通用的车辆坐标系不同，车辆坐标系可以放置在车辆上任何方便的位置，例如质心。对于定位任务，我们通常会忽略车辆坐标系和传感器坐标系之间的区别，并假设如果我们能跟踪传感器，那么在经过适当的校准后（这是我们后面会探讨的主题），我们就应该能够跟踪车辆上的任何点。
总结
本节课我们一起学习了三维几何与参考坐标系的基础知识。
我们了解到，向量量可以通过旋转和平移在不同的参考系中表达。旋转可以用旋转矩阵、四元数或欧拉角来参数化，每种方法都有其优缺点。此外，地心地固坐标系、地心惯性坐标系和导航坐标系是车辆定位中至关重要的参考坐标系。
在下一个视频中，我们将讨论如何利用本节课学到的工具，通过使用惯性测量单元来跟踪运动。
021：惯性测量单元（IMU）
在本节课中，我们将讨论惯性测量单元，即IMU。课程结束时，你将能够描述构成基本IMU的两种传感器的工作原理：加速度计和陀螺仪。你还将能够为每种传感器建立模型，并考虑传感器噪声和偏差等因素。当我们将IMU集成到完整的状态估计器中时，这将至关重要。
什么是IMU？🚀
惯性测量单元测量物体在惯性空间中的运动。如今，一种廉价的、大规模生产的IMU几乎出现在每一部智能手机中，例如图中所示的iPhone X。IMU通常用于步数统计、健康追踪等任务，最近也用于增强现实设备。
尽管IMU如今无处不在，但开发一种能够精确追踪运动物体运动的传感器是20世纪的一项重大成就。IMU在GPS出现之前就辅助了跨洋飞行，并且作为阿波罗任务中机载制导、导航和控制系统的一部分至关重要。阿波罗飞船依靠IMU在漫长的月球航行中精确追踪飞行器的位置和姿态。
在太空中，几乎没有地标可供制导参考。虽然可以追踪恒星，但这并不容易。机载IMU无需此类地标即可工作，实现了安全导航至月球表面。在现代自动驾驶汽车中，IMU扮演着非常相似的角色，在其他传感器的导航信息不可用或不可靠时进行补充。
IMU的组成与类型 📦
那么，什么是IMU？通常，惯性测量单元是一个复合传感器套件，它结合了三个陀螺仪和三个加速度计，以追踪刚体的六自由度运动。一些IMU还包含磁力计或罗盘来辅助追踪方向。
IMU有多种形状和形式。现代智能手机中的传感器相对便宜，批量购买时通常成本不到几美元。它们重量轻，功耗相对较低，但产生的测量值噪声较大。更昂贵的IMU使用更复杂的组件，并具有更精确的校准模型，例如可以消除温度波动的影响。
陀螺仪详解 🌀
让我们更详细地讨论IMU的组成部分：陀螺仪和加速度计。
陀螺仪有着悠久的历史。“陀螺仪”这个术语可能相当令人困惑，因为它指代几个与测量方向或方向变化相关的概念。这个词本身来源于希腊语，意为“看圆圈”。历史上，陀螺仪是一个旋转的圆盘，由于其角动量，能够抵抗方向的变化。
在19世纪末和20世纪初，工程师们意识到这种旋转轮可以用作航海和航空导航的方向参考。这需要精密的加工和一套使用高质量宝石或气动轴承的万向节。虽然这种旋转盘式陀螺仪可以非常精确，但它相当重、体积大，并且制造成本通常非常高昂。尽管如此，它仍然用于航空和弹道应用，转速可达24，000 RPM。
在现代陀螺仪中，旋转金属盘通常被微机电系统取代，该系统由一个小的硅音叉组成，其共振特性会根据施加的旋转或方向变化而改变。这些传感器便宜得多，可以封装在很小的空间里。然而，它们产生的测量值噪声较大，并且对基于温度的波动敏感。更重要的是，它们测量的是旋转速率，而不是直接测量方向，因此输出信号必须通过数值积分来确定方向变化。这个过程可能会给最终的方向估计引入额外的误差。
自动驾驶汽车工程师应该意识到，需要考虑漂移和性能随温度变化等问题，这给传感器建模过程带来了相当大的额外复杂性。人们通常认为旋转的机械装置不如硅组件，但情况并非总是如此。
加速度计详解 📈
加速度计测量沿单轴的线性加速度。基于MEMS的廉价加速度计使用一个带有检测质量的微型悬臂梁。当传感器被加速时，梁会发生偏转。这种偏转可以通过电容电路等方式测量，并转换为加速度值。更昂贵的传感器也可能使用压电材料。
需要注意的是，加速度计测量的是所谓的“固有加速度”或“比力”。这是单位质量所受的总非引力。固有加速度是相对于自由落体参考系的加速度。当你坐在椅子上，相对于地面静止时，你感受到的固有加速度将是你所在位置的重力加速度值，但方向向上。另一种说法是，作用在你身上的唯一非引力，即法向力，必须等于重力。
然而，对于导航目的，我们通常不关心固有加速度，我们关心的是相对于某个固定参考系的加速度。为了计算这个加速度，我们需要使用重力场中加速度计的基本方程：在固定坐标系中计算的位置二阶导数等于比力与重力加速度之和。
这有时可能是一个不太直观的概念。让我们在这里探讨一下。一辆静止汽车中的加速度计测量到向上的G值，因为坐标加速度为零（忽略地球自转）。由于重力向下作用，其负值就是向上的标量常数G。
让我们以国际空间站为例。低地球轨道上的G值更小吗？是的，但与地球表面相比，只小了大约10%。我们经常听到“零重力”这个术语，是因为整个国际空间站连同其中的宇航员都在自由落体。牢固连接到空间站的加速度计将具有等于G的坐标加速度。这意味着加速度计测量的比力将为0。另一种说法是，相对于自由落体的固有加速度为零。国际空间站处于自由落体状态。实际上，残余的大气阻力和结构振动会产生一些可测量的加速度，但它们通常低至10^-6 G。
传感器测量模型 📐
现在我们已经了解了陀螺仪和加速度计的基本原理，让我们讨论一下为了将它们集成到状态估计器中需要知道的测量模型。
使用上一课讨论的符号，让我们定义陀螺仪测量的表达式。来自三个陀螺仪的角旋转速率是体坐标系相对于惯性坐标系的角速度在体坐标系中的表示。我们为其添加一个缓慢变化的偏差项和一个白高斯加性噪声项来模拟传感器误差。虽然陀螺仪确实测量地球自转，但对于我们只关心短时间运动的应用，通常可以忽略这一点。
我们的加速度计测量模型将具有类似的噪声和偏差项。但现在，与我们可以直接测量旋转速率不同，我们需要使用重力场中加速度计的基本方程来明确消除重力的影响。由于加速度计在IMU体坐标系中测量加速度，我们需要始终跟踪方向，以便能够执行必要的减法。
模型的重要限制 ⚠️
最后，让我们讨论一下我们模型的一些重要限制。
首先，精确的方向估计对于精确的位置估计至关重要。当我们将测量的比力转换为加速度时，必须确保重力的方向是正确的。否则，即使是方向上的一个小误差，也可能导致我们认为自己在加速，而实际上并没有。
其次，我们推导的两个模型都忽略了地球自转的影响。对于长距离导航，这实际上很重要。
最后，我们推导的模型是针对捷联式IMU的。这些是物理上固定在车辆上的IMU，不包含安装在万向节上的旋转轮。虽然后者可能精确得多，但由于其体积和成本，它们很少用于汽车应用。
总结 📝
在本节课中，我们学习了六自由度IMU由三个陀螺仪和三个加速度计组成。陀螺仪测量传感器坐标系中的旋转速率。加速度计测量传感器坐标系中的非引力比力。
由于捷联式IMU校准困难且会随时间漂移，我们需要另一个传感器来定期校正我们的位姿估计。为此，我们可以使用现代全球导航卫星系统。我们将在下一讲中详细讨论。
022：全球导航卫星系统（GNSS）📡
在本节课中，我们将要学习一种大家非常熟悉的传感器：全球导航卫星系统（GNSS）接收器。我们将了解为什么这种导航传感器对自动驾驶汽车至关重要，因为它能够在全球范围内提供误差有界的定位信息。
上一节我们讨论了惯性测量单元（IMU），本节中我们来看看另一种关键的定位传感器——GNSS。与IMU不同，GNSS能提供绝对位置信息，但其工作原理和误差来源也更为复杂。
GNSS概述与历史背景
全球导航卫星系统（GNSS）接收器是一种能够提供全球定位的传感器。它通过测量与多颗卫星的距离来计算自身位置，其定位误差是有界的。
大多数现代智能手机都至少配备了一种GNSS接收器。虽然我们现在认为它理所当然，但第一个现代全球定位卫星系统——GPS，是在20世纪80年代为军事用途而建造的。到1995年第二代系统全面投入运行时，GPS已免费向公众开放。
这一部分原因与1983年韩国航空007号班机空难的高度公开有关。007航班是一架从纽约飞往首尔的波音747客机，在安克雷奇加油。在从安克雷奇飞往首尔途中，它偏离了计划航线，在苏联领空飞行数小时后被战斗机击落。飞越北太平洋时，007航班依靠惯性导航系统进行引导。飞行员未能正确初始化系统，错误地使飞机保持特定的磁航向，导致飞机偏离计划航线超过300公里。007航班被击落后不久，时任美国总统罗纳德·里根发布指令，允许美国的全球定位系统在完全开发后免费向公众开放。
虽然GPS是最初用于全球定位的导航卫星系统，但如今“全球导航卫星系统”（GNSS）一词已成为多个此类卫星星座的总称。截至2018年，两个完全投入运行的系统是GPS和俄罗斯的格洛纳斯（GLONASS）。其他几个系统，包括欧洲的伽利略星座，也接近完成。本视频将更仔细地研究GPS，尽管其他GNSS系统也遵循类似的工作原理。
GPS系统工作原理
GPS星座由24至32颗卫星组成，分布在六个轨道平面上。卫星会定期退役和更换。每颗卫星都位于中地球轨道，高度约为20000公里，轨道周期略低于12小时。该星座的设计确保在地球表面任何地点、任何时间都至少能看到四颗卫星。
每颗卫星以两个频率广播信号，一个用于民用，一个用于军用。每个广播信号都包含一个伪随机码，用于标识卫星位置和信号发射时间。
GPS的基本原理是到达时间测距。接收器通过比较其内部时钟与信号发射时间，计算到每颗可见卫星的距离。时间差利用电磁信号以光速传播的知识转换为距离。
要计算三维位置，测距方程至少需要四颗可见卫星。如果已知高度且仅需要二维位置，则只需要三颗卫星。从到多个已知地标的距离中恢复位置的过程称为三边测量。
请注意，这与三角测量不同，三角测量是基于角度测量来计算位置的。
以下是GPS接收器使用的伪距测量模型：
ρ = r + c * (δt_r - δt_s) + I + T + ε
其中：
ρ是测量的伪距。r是卫星与接收器之间的真实几何距离。c是光速。δt_r是接收器时钟误差。δt_s是卫星时钟误差。I是电离层延迟。T是对流层延迟。ε是测量噪声。
该模型考虑了接收器时钟误差、大气传播延迟和测量噪声。“伪距”一词指的是距离信息被上述误差源所破坏。
每个伪距测量在2D中定义一个圆，在3D中定义一个球体。如果我们恰好有四颗卫星，我们可以显式求解接收器位置和接收器时钟误差。如果我们有超过四颗卫星，我们可以使用最小二乘法在高斯噪声假设下找到最大似然位置。
GNSS误差来源
GPS存在多种误差源。
首先，电离层中的带电离子会以未知量延迟信号。其次，周围的地形和建筑物可能导致信号反射，从而增加信号到达接收器前传播的距离，这称为多径误差。
时钟同步或卫星位置信息中的任何微小误差都可能带来灾难性后果。即使一微秒的定时误差也可能导致300米的显著位置误差。星历数据和卫星时钟经常更新和重新校准，但校准可能已过时。
最后，可见卫星的几何配置也会导致定位精度的变化，这被称为几何精度衰减因子。为了获得更高的精度，卫星在天空中分散分布的配置更可取。
提高GNSS精度的方法
幸运的是，对于某些应用，我们可以通过各种方式增强系统来提高GNSS精度。
差分GPS可以通过利用一个或多个固定基站更精确已知的位置来校正接收器的定位估计。校正信息通过单独的频率广播给移动车辆中的GNSS接收器。
实时动态（RTK）GPS利用载波相位信息，在某些情况下可将定位精度提高到两厘米。
虽然这两种技术都可以显著提高GPS的精度，但实施起来通常成本相当高。
正如我们在上一个视频中提到的，惯性传感器对导航非常有用，但它们会随时间漂移或累积无界误差。相比之下，GPS系统提供有界误差的定位更新。配备GPS的自动驾驶汽车将始终保持有保证的定位精度水平，除非GPS接收器发生故障或失去对至少四颗卫星的跟踪。
总结与下节预告
本节课中我们一起学习了全球导航卫星系统（GNSS）的核心知识。
全球导航卫星系统通过结合至少四颗卫星的伪距来确定三维位置。GPS或GNSS误差可能来自几个不同的来源，包括电离层延迟、多径效应，也可能来自几何精度衰减因子。为了提高GNSS精度，可以使用差分GPS或RTK GPS等技术。
本模块到此结束。在模块4中，我们将描述如何融合来自IMU的惯性测量和来自GPS的位置测量，为自动驾驶汽车生成精确的定位估计。这些传感器是互补的，并且几乎在所有自动驾驶汽车中一起使用。下个模块再见。😊
023：为何需要传感器融合
概述
在本节课中，我们将探讨为什么在自动驾驶汽车的状态估计和定位中，融合多种传感器数据是必要且关键的。我们将理解单一传感器的局限性，并学习如何通过结合不同传感器的优势来构建更鲁棒、更准确的感知系统。
我们已经认识到状态估计非常重要。
同时，我也断言状态估计是困难的，因为它涉及复杂的数据关联问题，且数据本身也很复杂。
随之而来的下一个问题是：如何能让这项工作变得稍微容易一些？
或许我们应该说，与其只使用一种传感器，不如使用多种传感器。
例如，我们或许可以通过同时使用摄像头来消除激光雷达在区分建筑物拐角和公交车拐角时产生的混淆。因为公交车的外观通常与建筑物的拐角不同。
这样，我们就使用了两种模态。我们从摄像头获取外观信息，即像素的颜色；同时从激光雷达获取几何信息，即物体的距离。
你可能会发现，这样做会容易得多。然后我们可能会说，如果我有一个多普勒雷达，我还可以测量物体的速度。
现在，我有了速度信息、外观信息和形状信息。或许，我还可以使用陀螺仪和加速度计来了解自身状态的变化率。
这太棒了。现在，我希望将所有传感器的数据都输入到我的状态估计过程中。
我希望利用所有这些不同的传感器数据流，来更好地判断我自身的位置、周围环境的情况以及其他实体的行为。因此，你绝对应该这样做。
事实上，我认为只使用一种模态是相当不明智的，因为你总会遇到那种对该模态不友好的环境。
例如，如果我仅使用激光雷达，进入一个完全平坦、反射率没有变化的广阔区域，那么使用激光雷达就会变得相当困难，或者比在封闭、狭窄的环境中更困难。同样，如果我仅使用摄像头，而灯光完全熄灭，那也会很困难。
因此，组合使用这些传感器似乎是更明智的做法。
那么，一位优秀的自动驾驶工程师和算法工程师的艺术就在于，能够思考并构建一个优化问题。
这个优化问题能够利用所有的传感器数据流，并表明：只要我能找到这个函数的最小值，而这个最小值能解释我观察到的所有传感器数据，那么我就能很好地了解自己的位置和状态。
但是，你需要在处理所有那些令人困惑、相互冲突的数据关联问题的背景下完成这项工作。
所以，你需要处理外部世界的数据关联和状态估计，这两个问题相互交织，使得每一个问题都变得更加复杂和丰富。
尽管如此，状态估计依然重要，依然困难，也依然令人兴奋。
总结
本节课中，我们一起学习了传感器融合在自动驾驶状态估计中的必要性。我们了解到，单一传感器存在固有的局限性，容易受到特定环境条件的干扰。通过融合摄像头、激光雷达、雷达、IMU等多种传感器的数据，我们可以综合利用它们各自的优势——如外观、几何、速度、自身运动等信息——来构建一个更全面、更鲁棒、更准确的感知系统。这虽然增加了数据关联和优化问题的复杂性，但却是实现可靠自动驾驶的关键一步。
024：激光雷达传感器工作原理
在本节课中，我们将要学习激光雷达传感器。激光雷达是自动驾驶汽车的一项关键技术，因为它能提供全方位的精确距离信息。事实上，目前道路上大多数自动驾驶汽车都配备了某种类型的激光雷达传感器。
概述
欢迎来到课程的第四模块。在本模块中，我们将讨论激光雷达传感器。激光雷达因其能够全方位感知并提供非常精确的距离信息，已成为自动驾驶汽车的一项赋能技术。本模块中，您将学习激光雷达传感器的工作原理、处理激光雷达数据的基本传感器模型、激光雷达点云、应用于点云的各种变换操作，以及如何使用一种称为点云配准的技术，利用激光雷达为自动驾驶汽车定位。
具体在本视频中，我们将探讨激光雷达的工作原理，并了解2D和3D激光雷达的传感器模型。我们还将描述这些传感器的测量噪声和误差来源。在后续课程中，我们将更详细地讨论如何利用激光雷达数据进行自动驾驶汽车的状态估计。
激光雷达简介与应用
如果您见过像Waymo或Uber那样的自动驾驶汽车，您可能注意到车顶上有东西在旋转。那个东西就是激光雷达传感器，它的职责是提供车辆周围环境的详细3D扫描。实际上，激光雷达是自动驾驶汽车和许多其他移动机器人最常用的传感器之一。
激光雷达有多种形状和尺寸，可以测量到单个点的距离、对世界进行2D切片扫描，或执行完整的3D扫描。当今一些最流行的型号由加利福尼亚的Velodyne、日本的Hokuyo和德国的SICK等公司制造。本视频中，我们主要将以Velodyne传感器为例，但基本技术同样适用于其他类型的激光雷达。
在深入了解激光雷达传感技术之前，让我们快速回顾一下这一重要传感器的历史。激光雷达最早于20世纪60年代推出，就在激光本身发明后不久。第一个使用激光雷达的团体是美国国家大气研究中心的气象学家，他们部署激光雷达来测量云层高度。这些地基云高仪至今仍在使用，不仅用于测量水云，还用于探测火山灰和空气污染。
如今，机载激光雷达传感器常用于农业、地质学、军事等领域的地表勘测和测绘。但首次让激光雷达进入公众视野的应用是阿波罗15号任务，这是第四次载人登月任务，也是首次使用激光高度计绘制月球表面地图。
激光雷达工作原理：飞行时间测距
我们已经看到激光雷达可用于测量距离并创建某种类型的地图。但它们实际上是如何工作的？我们如何在自动驾驶汽车上使用它们？
要构建一个一维的基本激光雷达，您需要三个组件：一个激光器、一个光电探测器和一个非常精确的秒表。激光器首先沿某个已知射线方向发射一个短脉冲光，通常在近红外频段。同时，秒表开始计时。激光脉冲以光速从传感器向外传播，并击中一个远处的目标，可能是我们前方道路上的另一辆车，或像停车标志或建筑物这样的静止物体。
只要目标表面不是太光滑或闪亮，激光脉冲就会从表面向各个方向散射，其中一些反射光会沿着原始射线方向返回。光电探测器捕捉到返回的脉冲，秒表告诉您脉冲首次发出和返回之间经过了多少时间。这个时间称为往返时间。
我们知道光速略低于每秒3亿米。因此，我们可以将光速乘以往返时间，以确定激光脉冲行进的总往返距离。由于光速比汽车快得多，在发生这一切的几纳秒内，将激光雷达和目标视为基本静止是一个很好的近似。这意味着从激光雷达到目标的距离，就是我们刚刚计算的往返距离的一半。
这种技术称为飞行时间测距。虽然这不是构建激光雷达的唯一方法，但它是一种非常常见的方法，也用于雷达和声纳等其他类型的测距传感器。
值得一提的是，光电探测器还会告诉您返回脉冲的强度相对于发射脉冲的强度。这种强度信息在自动驾驶中较少使用，但它提供了关于环境几何形状和光束反射材料的一些额外信息。强度数据之所以有用，部分原因在于可以从激光雷达强度数据创建2D图像，然后您可以在下一门课程中学到的计算机视觉算法中使用这些图像。由于激光雷达是自身的光源，它实际上为自动驾驶汽车提供了一种在黑暗中“看见”的方式。
从点到云：2D与3D扫描
现在我们知道如何使用激光器、光电探测器、秒表和飞行时间方程来测量到单个点的单个距离。但显然，只关注前方一个点是不够的。那么我们如何使用这种技术来测量2D或3D中的一大堆距离呢？
诀窍是在激光雷达中内置一个旋转镜，将发射的脉冲导向不同的方向。随着镜子的旋转，您可以测量传感器周围2D切片中各点的距离。如果您随后给镜子添加一个上下点头运动，配合旋转，您可以使用相同的原理在3D中创建扫描。
对于Velodyne类型的激光雷达，其镜子和整个传感器主体一起旋转，很难使用点头运动来进行3D扫描。相反，这些传感器实际上会从一系列以固定角度间隔排列的独立激光器中创建多个2D扫描线，这有效地让您用水平条纹的激光“描绘”世界。
下图展示了一个典型的原始激光雷达数据流，来自安装在车顶的Velodyne传感器。中间的黑色空洞是传感器本身所在的盲点。从那里向外扩散的同心圆是由旋转的Velodyne传感器产生的各个扫描线。扫描中的每个点都根据返回信号的强度着色。3D扫描中所有点的集合称为点云，我们将在接下来的视频中讨论如何使用点云进行状态估计。
坐标转换：传感器模型
但在讨论点云之前，我们需要考虑3D中的单个点。通常，激光雷达使用球坐标测量3D点的位置：距离（从传感器原点到3D点的径向距离）、仰角（从传感器的XY平面向上测量）和方位角（从传感器的X轴逆时针测量）。这是合理的，因为方位角和仰角告诉您激光脉冲的方向，而距离告诉您目标点在该方向上有多远。方位角和仰角使用编码器测量，编码器告诉您镜子的方向；距离则使用我们之前见过的飞行时间法测量。对于Velodyne类型的激光雷达，给定扫描线的仰角是固定的。
现在，假设我们想确定扫描点在传感器坐标系中的笛卡尔X、Y、Z坐标。当我们将多个激光雷达扫描合并到地图中时，这通常是我们想要做的事情。为了从球坐标转换到笛卡尔坐标，我们使用您在力学课程中遇到过的相同公式。
这为我们提供了一个逆传感器模型。我们称之为逆模型，因为我们的实际测量值是以球坐标给出的，而我们正试图重建产生这些测量值的点的笛卡尔坐标。请注意，我们还没有讨论测量噪声，我们稍后会回到这个问题。
要朝另一个方向转换，即从笛卡尔坐标到球坐标，我们可以推导出此处给出的逆变换。这是我们3D激光雷达的前向传感器模型，给定一组笛卡尔坐标，它定义了传感器实际会报告什么。
大多数时候，我们正在研究的自动驾驶汽车使用像Velodyne这样的3D激光雷达传感器。但有时您可能想单独使用2D激光雷达，无论是用于检测障碍物，还是在停车场等更结构化的环境中进行状态估计。有些汽车战略性地放置了多个2D激光雷达，以充当单个3D激光雷达，以不同密度覆盖不同区域。
对于2D激光雷达，我们使用完全相同的正向和逆向传感器模型，但仰角和传感器坐标系中3D点的Z分量均为0。换句话说，我们所有的测量都局限于传感器的XY平面，我们的球坐标简化为熟悉的2D极坐标。
测量噪声与误差源
我们现在已经了解了如何在传感器测量的球坐标和我们通常对状态估计感兴趣的笛卡尔坐标之间进行转换。但是测量噪声呢？对于激光雷达传感器，有几个重要的噪声源需要考虑。
首先，反射信号到达的确切时间存在不确定性，这源于我们用于计算飞行时间的秒表必然具有有限的分辨率。同样，在2D和3D激光雷达中，镜子的确切方向也存在不确定性，因为用于测量方向的编码器也具有有限的分辨率。
另一个重要因素是与目标表面的相互作用，这会降低返回信号的质量。例如，如果表面完全是黑色的，它可能会吸收大部分激光脉冲。或者，如果它像镜子一样非常闪亮，激光脉冲可能会完全散射到原始脉冲方向之外。在这两种情况下，激光雷达通常会报告一个最大距离误差，这可能意味着沿光束方向是空的空间，或者脉冲遇到了高吸收性或高反射性的表面。换句话说，您根本无法判断那里是否有东西。如果您的自动驾驶汽车仅依赖激光雷达来检测和避开障碍物，这可能是一个安全问题。
最后，光速实际上会根据其穿过的材料而变化。例如，空气的温度和湿度也会微妙地影响我们飞行时间计算中的光速。通常通过假设球坐标上存在加性零均值高斯噪声，并配合经验确定或手动调整的协方差，来考虑这些因素。正如我们之前所见，高斯噪声模型对于状态估计特别方便，即使在大多数情况下它并不完全准确。
另一个无法轻易解释的非常重要的误差来源是运动畸变。这是因为激光雷达所依附的车辆通常相对于其扫描的环境在运动。虽然汽车不太可能以光速的显著比例运动，但它通常以传感器自身旋转速度的显著比例运动，而传感器在扫描10到100米距离的物体时，旋转速度通常在5到20赫兹左右。这意味着激光雷达扫描中的每一个点都是从略微不同的位置和方向获取的，这可能导致伪影，例如在激光雷达扫描中出现重复的物体。这使得自动驾驶汽车更难理解其环境，纠正这种运动畸变通常需要由GPS和INS等提供的车辆精确运动模型。
总结
本节课中，我们一起学习了激光雷达传感器。我们了解到，激光雷达传感器通过发射脉冲激光并测量脉冲的飞行时间来测量距离。2D或3D激光雷达通过使用镜子将激光扫过环境并在多个方向上测量距离，扩展了这一原理。
在下一个视频中，我们将更仔细地研究由2D和3D激光雷达创建的点云，以及如何利用它们为我们的自动驾驶汽车进行状态估计。
025：激光雷达传感器模型与点云
概述
在本节课中，我们将学习如何利用激光雷达传感器生成的点云数据进行自动驾驶汽车的状态估计。我们将首先了解点云的基本数据结构，然后探讨如何对点云进行平移、旋转和缩放等空间操作。最后，我们将学习如何使用最小二乘法将平面拟合到点云中，以检测道路或其他表面。
点云数据结构
上一节我们介绍了激光雷达的基本工作原理。本节中，我们来看看如何存储和处理激光雷达扫描产生的海量数据点。
三维激光雷达传感器为每个扫描点返回距离、仰角和方位角的测量值。我们可以使用逆传感器模型将这些球坐标转换为笛卡尔坐标 (X, Y, Z)，从而利用一次激光雷达扫描的所有测量值构建一个大型点云。
对于某些激光雷达设置，这些点云包含超过一百万个点的情况并不少见。那么，我们如何处理这些庞大的点云呢？
以下是存储点云的一种常见数据结构：
- 为每个点分配一个索引（例如点1到点n）。
- 将每个点的
(X, Y, Z)坐标存储为一个3x1的列向量。 - 将这些向量并排堆叠成一个矩阵，我们称之为
P。
用矩阵形式存储点云，便于利用像 Python 的 NumPy 库这样的标准线性代数库。我们可以利用快速的矩阵运算，而不是遍历列表并独立处理每个向量。
点云的空间操作
在了解了点云的数据结构之后，接下来我们探讨对点云进行状态估计至关重要的三种基本空间操作：平移、旋转和缩放。
对于状态估计，更有用的思考方式是：世界中的物体基本保持不动，而附着在车辆上的参考系在移动，并从不同角度观察世界。
平移
考虑我们的参考系平移（例如车辆向前行驶10米）将如何影响我们对点云中单个点的感知。
点 P 在新坐标系 S' 中的坐标，可以通过从其在原坐标系 S 中的坐标减去坐标系间的平移向量来获得。
对于整个点云，可以通过从点云矩阵 P 中减去一个平铺了平移向量的大矩阵 T 来实现批量操作。在 Python 的 NumPy 库中，通常可以利用广播语义隐式完成此操作，无需显式构建 T 矩阵。
旋转
如果我们旋转参考系，物理点 P 本身没有改变，改变的是我们用来表示向量坐标的基向量组。
旋转矩阵 C 告诉我们如何从向量在原坐标系中的坐标，找到该向量在旋转后坐标系中的坐标。因此，如果我们知道从坐标系 S 到 S' 的旋转矩阵 C，只需将其与 P 在 S 系中的坐标相乘，即可得到 P 在 S' 系中的坐标。
对于整个旋转点云，操作完全相同，这得益于矩阵乘法的性质。
缩放
缩放操作与旋转非常相似，但我们改变的是坐标系中基向量的长度，而不是方向。
从数学上讲，这意味着将每个点的坐标预乘一个对角矩阵 S，其非零元素就是沿每个维度所需的缩放因子。通常（但不总是），这些缩放因子是相同的，此时矩阵乘法等价于乘以一个标量。在这种情况下，我们称缩放是各向同性的，即每个方向都相等。
与旋转一样，我们可以对单个点或整个点云使用相同的矩阵乘法。
组合空间变换
通常，我们感兴趣的是平移和旋转（有时包括缩放）的组合变换。例如，我们经常需要估计能最佳对齐两个点云的平移和旋转，以便估计自动驾驶汽车的运动。我们将在下一个视频中讨论如何做到这一点。
幸运的是，我们可以轻松地将所有三个操作组合成一个方程。首先平移每个向量，然后旋转到新坐标系，最后应用任何缩放。当然，这个操作也适用于批量处理的情况。
平面拟合
在进入下一节之前，还有一个重要的操作需要讨论：平面拟合。对于自动驾驶汽车，平面拟合最常见和最重要的应用之一是找出道路表面在哪里，并预测汽车继续行驶时道路将位于何处。
回想一下高中几何，三维空间中平面的方程是：
Z = A + B*X + C*Y
这个方程告诉你，当你在 X 和 Y 方向上移动时，平面的高度 Z 如何变化。它依赖于三个参数 A、B 和 C，它们分别告诉你平面在每个方向上的斜率以及平面与 Z 轴的交点。
在我们的例子中，我们从激光雷达点云中获得了一堆 (X, Y, Z) 的测量值，我们希望找到参数 A、B、C 的值，从而给出通过这些点的最佳拟合平面。
为此，我们将回顾第一模块中的工具：最小二乘估计。
以下是使用最小二乘法拟合平面的步骤：
- 为点云中的每个点定义一个测量误差
e，它是因变量Z的预测值Ž与实际观测值Z之间的差值。Ž通过将当前对参数Â、B̂、Ĉ的猜测以及X、Y的实际值代入平面方程得到。 - 将所有误差项堆叠成矩阵形式，得到一个系数矩阵
A乘以参数向量x，再减去堆叠的测量值向量b。 - 最小化该误差的平方和，以获得我们的解。这与我们在模块1中解决的电阻器问题原理相同。
- 将平方误差展开为参数向量
x的矩阵多项式。 - 对平方误差函数关于参数向量
x求偏导数，并令其为零以找到最小值。这给出了我们需要求解以获得最终最小二乘估计的线性方程组。 - 使用高效的数值求解器（如 Python NumPy 的
solve函数）求解此线性系统，或直接使用伪逆来获得平面参数的最终答案。
这里需要注意的一点是，我们没有考虑 X、Y、Z 测量中的传感器噪声。我们所做的只是找到通过一组点的最佳拟合平面。当然，可以用更复杂的方式来设置这个问题，以考虑传感器噪声。你可以使用类似于我们刚才讨论的批处理方法，甚至可以考虑将道路参数纳入卡尔曼滤波器，在传感器数据传入时实时估计它们。对于你的自动驾驶应用，最佳解决方案将取决于你对激光雷达数据的信任程度以及对道路表面不确定性的考量程度。
点云库
尽管我们这里描述的所有操作都可以用 NumPy 或任何其他线性代数库轻松实现，但有一个出色的开源工具叫做点云库，它为处理点云提供了各种有用的函数。
事实上，它非常有用，在工业界几乎随处可见。其核心库是用 C++ 构建的，但也有非官方的 Python 绑定可用。如果你想了解更多关于 PCL 的功能，强烈建议访问 pointclouds.org 查看。
PCL 中最有用的算法之一称为迭代最近点算法，它是使用两个激光雷达点云估计自动驾驶汽车运动的常用方法。我们将在下一个视频中讨论 ICP 的工作原理。
总结
本节课中，我们一起学习了：
- 点云是捕获激光雷达扫描所有测量值的一种方式，通常存储为一个大矩阵。
- 如何使用线性代数对点云进行有用的操作，如平移、旋转和缩放。
- 如何使用最小二乘算法将三维平面拟合到点云中以找到道路表面。
- 点云库在 C++ 中实现了一系列用于处理点云的有用工具，其中迭代最近点算法是用于运动估计的关键算法。
026：基于激光雷达数据的姿态估计
概述
在本节课中，我们将学习如何利用激光雷达（LiDAR）数据来估计自动驾驶汽车的运动。核心内容围绕点集配准问题展开，并详细介绍解决该问题的迭代最近点（ICP）算法。我们将了解ICP的基本原理、实现步骤、潜在挑战以及如何应对现实世界中的动态物体干扰。
点集配准问题简介
上一节我们学习了点云的平移、旋转和缩放操作。本节中，我们来看看如何将这些操作应用于真实的点云数据，以估计自动驾驶汽车的运动。
我们通常通过解决点集配准问题来实现这一目标，这是计算机视觉和模式识别中最重要的问题之一。
让我们回到自动驾驶汽车通过车顶激光雷达观测一棵树的例子。
- 在时间 T=1，激光雷达返回一个点云，其所有点的坐标都相对于扫描时激光雷达的姿态给出。我们称此坐标系为 S。
- 在时间 T=2，汽车向前行驶了一段距离，激光雷达再次扫描到树并返回第二个点云，其坐标相对于 T=2 时刻激光雷达的姿态给出。我们称此坐标系为 S‘。
点集配准问题可以描述为：给定两个在不同坐标系下的点云，并且已知它们对应或包含世界中的同一物体，我们应如何对齐它们，以确定传感器在两次扫描之间必须发生的运动。
更具体地说，我们希望找到两个传感器参考系之间的最优平移和最优旋转，以最小化两个点云之间的距离（我们稍后将定义“距离”的含义）。为简化问题，我们假设拥有理想的激光雷达传感器，可以一次性测量整个点云，从而忽略运动畸变的影响。
迭代最近点（ICP）算法
如果我们事先知道第二个点云中的每个点具体对应第一个点云中的哪个点，那么这个问题很容易解决。我们只需找到将每个点与其对应点对齐的平移和旋转即可。
然而，通常我们并不知道点之间的对应关系。迭代最近点（ICP）算法是解决这类问题最流行的算法。
ICP的基本直觉是：当我们找到（或预先知道）最优的平移和旋转，并用它们将一个点云变换到另一个点云的坐标系时，真正相互对应的点对将是欧几里得意义上彼此最接近的点。
如果我们没有最优的变换参数，如何决定哪些点应该配对呢？ICP使用一个简单的启发式方法：对于第一个点云中的每个点，在第二个点云中最佳的对应点候选就是当前距离它最近的点。
这个想法是，这个启发式方法应该能为我们提供对应关系，从而让我们能做出一个比当前猜测稍好一点的下一个平移和旋转猜测。随着我们的猜测越来越好，对应关系也应该越来越好，直到最终收敛到最优的运动和最优的对应关系。这种使用最近点启发式方法的迭代优化方案正是ICP名称的由来。
ICP算法步骤
以下是ICP算法的核心步骤：
首先，我们需要一个初始变换猜测。这是因为无法保证最近点启发式方法一定能收敛到最佳解。在这类迭代优化方案中，很容易陷入局部最小值，因此需要一个良好的起始猜测。初始猜测可以来自多种来源，对于自动驾驶等机器人应用，最常见的来源之一是运动模型（例如来自IMU、轮速计，或简单的恒定速度/零速度模型）。初始猜测的好坏取决于汽车行驶的平滑度相对于激光雷达扫描速率的关系。
步骤一：初始猜测
假设我们使用一个有噪声的IMU和运动模型来提供初始猜测，该模型告诉我们传感器向前平移了一点，并且向上倾斜了。
步骤二：关联点
使用初始猜测，将一个点云中的点坐标变换到另一个点云的参考系中。然后，将第二个点云中的每个点与第一个点云中最近的点进行匹配。请注意，一个点云中的两个点有可能与另一个点云中的同一个点相关联。
步骤三：求解最优变换
获取所有匹配点对，并找到能够最小化所有对应点之间距离平方和的变换。你可以将其想象为在每个匹配点对之间缠绕一根橡皮筋，然后松开手，等待系统找到其能量最低的配置。与这个最低能量配置相关的平移和旋转就是最优解。
步骤四：迭代
使用刚刚求解出的平移和旋转作为新的初始猜测，重复上述过程。我们持续迭代，直到优化收敛（即变换参数的变化非常小）。
求解最优变换：最小二乘与奇异值分解（SVD）
我们如何在步骤三中实际求解最优变换呢？你可能已经猜到，我们将使用我们最喜爱的工具：最小二乘法。
我们的目标是找到最能对齐两个点云的旋转矩阵 R 和平移向量 t。具体来说，我们希望最小化每对匹配点之间的欧几里得距离平方和，即以下损失函数：
L(R, t) = Σ || (R * p_i + t) - q_i ||²
其中 p_i 是第一个点云中的点，q_i 是第二个点云中与之匹配的点。
这个最小二乘问题比我们之前遇到的更复杂，因为旋转矩阵 R 在损失函数内部，并且旋转矩阵不能像向量一样简单相加。3D旋转属于特殊正交群（SO(3))，需要特殊处理。
幸运的是，这个问题有一个优美的闭式解。以下是求解该问题的四步算法：
- 计算质心：分别计算两个点云的质心（所有点坐标的平均值）。
- μ_p = (1/N) * Σ p_i
- μ_q = (1/N) * Σ q_i
- 构造协方差矩阵 W：这是一个3x3矩阵，捕获两个点云的分布关系。
- W = Σ (q_i - μ_q) * (p_i - μ_p)^T
- 通过SVD求解最优旋转矩阵 R：对矩阵 W 进行奇异值分解（SVD）。
- W = U * S * V^T
- 最优旋转矩阵 R 可通过以下公式计算：R = U * C * V^T，其中 C 是一个对角矩阵，用于确保 R 是一个正确的旋转（没有反射），其对角线元素为 [1, 1, det(U)*det(V)]。
- 求解最优平移向量 t：有了旋转矩阵后，平移向量很容易计算。
- t = μ_q - R * μ_p
关于奇异值分解（SVD）的简要说明：SVD是将一个矩阵分解为两个酉矩阵（U 和 V）和一个对角矩阵（S）的乘积。对于本问题，我们可以将 U 和 V 视为旋转，S 视为缩放矩阵。由于我们处理的是刚体运动（无缩放），我们需要调整 S 矩阵以消除缩放并确保得到的是纯旋转。
ICP的挑战与改进
不确定性估计：从安全角度以及后续讨论传感器融合时，我们需要考虑对ICP算法输出的置信度。可以通过公式估计运动参数的不确定性（协方差），但通常为了实用，我们会手动调整ICP的协方差矩阵以获得可接受的结果。
ICP变体：上面介绍的是最基本的点到点ICP。另一种在结构化环境（如城市或室内）中表现良好的流行变体是点到平面ICP。它不是最小化点对之间的距离，而是将第一个点云拟合出若干平面（如墙面、路面），然后最小化第二个点云中每个点到第一个点云中最近平面的距离。
动态物体的挑战：到目前为止，我们都假设激光雷达观测到的物体是静止的。如果物体在移动呢？例如，我们的汽车在繁忙的高速公路上行驶，扫描前方的车辆。如果两车同速行驶，ICP可能会错误地得出汽车没有移动的结论。虽然现实中点云会包含许多静止物体（道路、建筑、树木），但天真地使用ICP在存在移动物体的情况下，会倾向于将运动估计拉离真实运动。
应对异常值（Outliers）：
- 传感器融合：将ICP运动估计与GPS、INS（惯性导航系统）的估计融合。我们将在课程后面讨论如何操作。
- 识别并忽略移动物体：可以使用计算机视觉技术。
- 使用鲁棒损失函数：这是一种更简单的方法。标准的最小二乘损失函数对由异常值引起的大误差非常敏感。鲁棒损失函数（如L1范数、Cauchy损失、Huber损失）的斜率不会随着误差增大而持续增加，而是保持恒定或逐渐减小，从而降低异常值的影响。
- 数学上，我们将损失函数推广为：L(R, t) = Σ ρ( || (R * p_i + t) - q_i || )，其中 ρ 是鲁棒函数。
- 使用鲁棒损失函数会使ICP问题稍微复杂，因为不再有闭式解，需要在主循环内增加一个迭代优化步骤，但其好处通常大于增加的复杂度。
总结
本节课中，我们一起学习了如何利用激光雷达数据进行车辆运动估计。
- 迭代最近点（ICP）算法是一种通过对齐来自激光雷达等传感器的点云来确定自动驾驶汽车运动的方法。
- ICP通过迭代最小化每个点云中相邻点之间的欧几里得距离来工作，这也是算法名称的由来。
- 移动物体对ICP来说是一个问题，因为它们违反了ICP所基于的静态世界假设。
- 可以通过多种技术来缓解此类异常值测量，包括鲁棒损失函数，它比通常的平方误差损失函数更少地赋予大误差权重。
本模块内容回顾：
- 激光雷达（LiDAR）通过测量激光脉冲的飞行时间来测距。
- 2D和3D激光雷达传感器可以扫描汽车周围的大片环境。
- 每次扫描返回的点集合存储为点云，可以使用平移、旋转和缩放等标准空间操作进行处理。
- 这些操作是迭代最近点（ICP）算法的重要组成部分，而ICP是使用激光雷达定位自动驾驶汽车的常用技术。
在下一个模块中，我们将讨论状态估计的一些实际方面，并探讨如何将激光雷达和ICP与IMU、GPS等其他传感器结合使用，以获得准确可靠的车辆状态估计。
027：优化状态估计
在本节课中，我们将学习状态估计的核心方法——优化。我们将探讨如何将传感器数据处理为一个可最小化的函数，并介绍几种常见的优化算法。同时，我们也会讨论系统鲁棒性和多传感器融合的重要性。
优化：状态估计的核心方法 🎯
最引人注目的状态估计方法本质上都是某种形式的优化。这意味着你需要一种巧妙的方法，在一个可能维度很高的空间中进行搜索。这个搜索基于当前和最近的图像流、所有历史观测数据以及各种传感器数据。
给定这些数据，你需要构建一个希望最小化的函数。这个函数可能非常复杂，包含了特征点在图像中的投影、具有不同反射特性的激光雷达数据、多普勒信号等等。同时，数据关联问题——即确定哪个观测数据对应哪个地标或物体——本身也可以被构建为一个最小化问题。
将所有因素考虑在内后，快速求解这个优化问题，就能告诉你车辆可能的位置，或者一组可能的位置集合。同样重要的是，你还需要估计与这些位置相关的不确定性。在后续进行规划决策时，清楚自己不确定什么至关重要。
常见的优化算法 ⚙️
有许多算法使用优化思想。以下是一些常见的例子：
- 最小二乘法：你可能遇到的最简单的算法。但通常不建议直接使用原始的最小二乘法，因为它对异常值非常敏感，会花费大量精力去拟合那些无意义的数据点。因此，需要为其添加异常值剔除机制。
- 卡尔曼滤波器：这是另一种在线的最小均方误差估计器。
- 大规模批处理优化器：在某些情况下，你可能希望使用这类方法。
- 粒子滤波器：这也是一个可选方案。
所有这些方法都在寻找某个最大值或最小值，具体取决于你如何构建问题（即你的“代价函数”或“目标函数”）。
系统的鲁棒性与传感器融合 🛡️
上一节我们介绍了优化算法，本节中我们来看看如何保证状态估计系统的可靠性。在一个理想的世界里，传感器会持续稳定地提供数据。但你必须预料到可能出现问题，因此系统需要具备自省能力。
这意味着需要有一些监控进程，能够判断数据流是否出现异常、是否暴露不当，或者某些传感器是否已停止工作。这引出了一个更高层次的需求：不能只依赖单一的感知模态。
如果你只有一个摄像头，而这个摄像头失效了，情况就会变得很困难。因此，在实际系统中，你会结合车辆里程计、陀螺仪、激光雷达和雷达等多种传感器。当其中任何一个失效时，你已经有备用或互补的传感器，使你能够立即评估这是否是关键错误，并据此做出靠边停车或减速等规划决策。
数据融合在这里也起到了帮助作用。例如，你可能拥有17个摄像头，它们都能提供位置信息；你可能拥有前视和后视摄像头；你可能拥有多个激光雷达，它们都在激光地图中进行索引。你会希望系统的设计不会存在某个“关键传感器”，一旦它失效，你就完全不知所措。那将是一个糟糕的设计方案。
总结 📝
本节课中我们一起学习了状态估计的优化方法。我们了解到，状态估计的核心是将多源传感器数据融合并构建为一个优化问题，通过最小化一个复杂的代价函数来求解最可能的状态（如车辆位置）及其不确定性。我们介绍了几种常见的优化算法，如最小二乘法、卡尔曼滤波器和粒子滤波器。最后，我们强调了系统设计必须具备鲁棒性，通过多传感器融合和冗余设计来应对传感器故障，确保自动驾驶系统能够安全、可靠地运行。
028：实践中的状态估计
在本节课中，我们将学习如何将状态估计理论应用于真实的自动驾驶汽车。我们将探讨如何整合来自多种传感器的数据，并考虑实际工程中的关键因素，如传感器标定、系统性能要求以及应对环境变化和故障的策略。
现在你已经学习了估计理论、三维几何和一些常见传感模式的基础知识，是时候进行实践，并思考如何将这些工具结合起来，构建一个可用于真实自动驾驶汽车的估计器。
一辆真实的自动驾驶汽车，例如Autonomoose，会配备多种不同的传感器。例如，Autonomoose配备了五个摄像头、一个3D激光雷达、一个惯性测量单元、四个雷达单元、一个GPS或GNSS接收器以及一个车轮编码器。所有这些传感器以不同的速率提供不同类型的数据。例如，IMU可能每秒报告200次加速度和角速度，而激光雷达每秒仅完成20次完整扫描。
因此，在本模块中，我们将讨论如何组合所有不同的信息，以获得最佳的车辆状态估计。这个过程称为传感器融合，它是自动驾驶汽车最重要的技术之一。
但为了进行传感器融合，我们还需要标定传感器，以确保传感器模型准确，并了解所有传感器的参考坐标系如何相互关联。我们还将讨论当一个或多个传感器发生故障时会发生什么，并概述最终项目，你将有机会在Carla模拟器中使用传感器实现一个完整的车辆状态估计器。
在这个第一个视频中，我们将从宏观角度概述设计自动驾驶汽车系统时应考虑的一些实际问题。
需要考虑哪些问题？我们已经提到了传感器融合和标定，但还需要考虑速度和精度要求，以及定位失败和如何处理环境中移动和变化的部分。
让我们从传感器融合开始。如果我们有一辆像Autonomoose这样配备了多种不同传感器的汽车，我们希望做的是找出如何组合所有这些不同的信息，以获得最佳的车辆状态估计。
使用所有这些数据可能看起来是一项艰巨的任务，但实际上，我们已经掌握了所需的工具。在本模块的第二课中，我们将详细讨论如何使用扩展卡尔曼滤波器等熟悉的工具，将所有传感器数据组合成一个一致的车辆状态估计。
但为了进行传感器融合，我们首先需要了解传感器的一些信息以及它们在车辆上的配置方式。
一方面，我们的传感器模型可能依赖于特定于汽车或传感器本身的参数。一个很好的例子是使用车轮编码器来测量汽车的前进速度。车轮编码器测量车轴的角速度，但如果我们想用它来获取车辆的前进速度，我们还需要知道轮胎的半径。
我们需要了解的关于车辆的另一个方面是每个传感器相对于车辆参考坐标系的位姿（位置和方向）。因为我们要组合来自不同位置传感器的信息，所以需要知道如何转换所有测量值，使它们在一个共同的参考坐标系中表达。
最后，我们需要考虑传感器测量的同步程度，以便正确地进行融合。直观地说，你可能认为直接将刚刚收到的激光雷达扫描与5秒前收到的GPS测量值结合起来，效果不如激光雷达扫描和GPS测量值在同一时间进行。
因此，传感器同步越精确，你的状态估计就越好。这其中的一部分涉及确定传感器记录测量值与估计器接收并处理该测量值之间的时间偏移。
所有这些因素都是标定的关键形式，我们将在第3课中更详细地讨论。
对于一辆在道路上安全行驶的自动驾驶汽车，我们的估计器需要多精确？这取决于汽车的大小、车道的宽度和交通密度。但为了得到一个大概的估计，你可以考虑像车道保持这样的任务可用的误差范围。
一辆典型的汽车大约宽1.8米，一条普通高速公路车道大约宽3米，上下浮动。因此，我们的估计器需要足够好，能将汽车定位在车道两侧大约60厘米的范围内。这假设我们确切知道车道的位置，并且没有其他交通。
作为比较，GPS精度的乐观范围在1到5米之间，具体取决于硬件、可见卫星数量等因素。因此，仅靠GPS显然不足以进行车道保持。这就是为什么我们需要组合来自许多不同传感器的信息的一个重要原因。
那么速度呢？我们需要多快更新车辆状态，以便汽车能够对快速变化的环境或意外事件做出反应？这完全取决于汽车在什么样的环境中运行。
想象一下，你闭着眼睛开车，每秒钟只睁开眼睛一次，看看周围的情况并进行一些调整。这对应于1赫兹的更新率。如果你在一条没有交通的笔直乡村道路上行驶，你可能会觉得这样做相对安全。如果你正穿过一个繁忙的城市十字路口，周围有数十辆其他汽车、公共汽车、自行车和行人，你可能会觉得每秒只睁一次眼非常不安全。
根据经验，15赫兹到30赫兹的更新率对于自动驾驶来说是一个合理的目标。当然，这里有一个权衡需要考虑。一辆自动驾驶汽车只有有限的机载计算能力可用，计算机除了状态估计外，还需要处理许多不同的进程，如控制、路径规划和感知。此外，机载可用总计算能力可能受到计算机实际允许功耗的限制。
对于使用固定计算资源进行状态估计，我们的算法可以有多复杂与我们被允许花费多少时间来计算解决方案之间存在权衡。作为自动驾驶汽车工程师，需要决定你的汽车在这个权衡曲线上处于什么位置。
即使我们有一个快速准确的估计算法，也会出现定位失败的情况。这怎么会发生呢？一方面，我们的一个或多个传感器可能会报告错误数据，甚至完全失效。GPS就是一个很好的例子，它在隧道中完全不起作用，并且在有许多高楼的城市中处理反射信号时也可能遇到困难。
我们的状态估计算法本身也可能遇到错误。例如，如果我们使用带有高度非线性传感器模型的扩展卡尔曼滤波器，我们可能会发现估计器中固有的线性化误差意味着我们可能会失去状态估计的准确性，即使估计器对其输出相当有信心。
或者，我们的估计器可能根本没有信心。回想一下卡尔曼滤波器方程，你可能记得，当我们通过运动模型向前传播时，状态的不确定性会增加，只有在合并来自激光雷达或GPS等外部观测后，不确定性才会缩小。
如果我们的激光雷达坏了，并且我们在没有GPS的隧道中行驶，我们能依赖IMU和运动模型多长时间，直到我们的状态不确定性变得太大，不再安全驾驶？我们将在第4课中讨论检测和应对此类定位故障的策略。
最后，我们需要考虑汽车所处的世界。在很大程度上，我们是在假设世界是静态和不变的条件下，开发了像激光雷达这样的传感器模型。
但当然，在现实中，世界总是在运动和变化。例如，其他汽车、行人和骑自行车的人可能在移动。一天中的光照会发生变化，甚至世界的几何形状也会随着季节而变化。
自动驾驶汽车面临的一大挑战是找到方法来解释这些变化，无论是通过建模它们，还是通过找到方法来识别和忽略违反我们假设的物体。事实上，这仍然是一个非常活跃的研究领域。
因此，总结这个视频，实践中的状态估计通常依赖于传感器融合来组合来自IMU、激光雷达、摄像头和GPS或GNSS接收器等多种不同传感器的信息。为了使传感器融合按预期工作，我们需要通过确定传感器模型的参数、所有传感器的相对位置和方向以及轮询时间的任何差异来标定传感器。
我们还需要考虑算法中速度和精度之间的权衡，这可能因你正在开发的自动驾驶汽车类型而异。问问自己，对于我的特定用例，我需要多准确地知道车辆状态，以及我需要多频繁地更新它？
最后，我们需要思考如何安全地应对定位失败以及不符合我们假设的世界方面，例如移动的物体。
在接下来的三个视频中，我们将更详细地探讨其中一些主题，从下一个视频的多传感器融合开始。
029：多传感器融合在状态估计中的应用
在本节课中，我们将学习如何将惯性测量单元、全球导航卫星系统接收器和激光雷达的数据融合在一起，构建一个用于自动驾驶汽车定位的扩展卡尔曼滤波器。我们将推导一个估计车辆位置、速度和姿态的滤波器，并了解其基本流程和关键假设。
传感器选择与融合策略
上一节我们介绍了定位所需的基本硬件和软件。本节中，我们来看看如何将所有组件整合在一起。
我们将推导一个扩展卡尔曼滤波器，用于使用IMU、GNSS接收器和激光雷达来估计自动驾驶汽车的位置、速度和姿态。
尽管我们会做一些简化，但流程的基本结构将类似于现代自动驾驶汽车中使用的方案。
在深入算法细节之前，有必要退一步思考：为什么使用这些传感器？能否采用更简单的方案？
我们选择使用IMU、GNSS接收器和激光雷达，原因如下。
首先，在为状态估计融合信息时，一个重要的考虑因素是不同传感器的误差是否相关。换句话说，如果一个传感器失效，另一个是否也可能因同样原因失效。在我们的案例中，这三种传感器使用不同的测量方法，不太可能因相同原因失效。
其次，我们应尽力选择本质上互补的传感器。在我们的案例中，IMU充当GNSS位置估计的高频平滑器，而GNSS数据可以减轻IMU漂移带来的误差。也可以使用轮速里程计达到此目的。本课程中，我们将坚持使用IMU，因为它能提供完整的三维位置和姿态信息，而轮速里程计仅限于二维。
最后，激光雷达可以通过在已知地图和天空无遮挡的位置提供非常精确的位置估计，来补充GNSS信息。反过来，GNSS可以告诉激光雷达在进行定位时使用哪张地图。
对于扩展卡尔曼滤波器状态估计，我们可以实现所谓的松耦合估计器或紧耦合估计器。
在紧耦合扩展卡尔曼滤波器中，我们使用来自GNSS和激光雷达的原始伪距和点云测量值作为观测值。
在松耦合系统中，我们假设这些数据已经过预处理，生成了一个带有噪声的位置估计。
尽管紧耦合方法可以实现更精确的定位，但实现起来通常很繁琐，并且需要大量调参。因此，我们将在此实现一个松耦合的扩展卡尔曼滤波器。
下图展示了我们系统的图形化表示。
我们将使用IMU测量值作为运动模型的噪声输入。这将给出我们的预测状态，每次有IMU测量值时都会更新，频率可达每秒数百次。
以慢得多的速率，每当GNSS和激光雷达测量值可用时（例如每秒一次或更慢），我们将整合它们，并用它们来修正我们的预测状态。
状态定义与运动模型
那么，我们的状态是什么？
我们将使用一个10维的状态向量，包括一个3D位置、一个3D速度，以及一个4D单位四元数，用于表示车辆相对于导航坐标系的姿态。
我们假设IMU在传感器坐标系中输出比力和旋转速率，并将它们组合成一个单一的输入向量 u。
需要指出的是，我们不会跟踪加速度计或陀螺仪的偏差。这些偏差通常被放入状态向量中进行估计，然后从IMU测量值中减去。为了清晰起见，我们在此忽略它们，并假设我们的IMU测量值是无偏的。
我们用于位置、速度和姿态的运动模型将积分来自IMU的比力和旋转速率。
位置更新如下所示：
p_k+1 = p_k + v_k * Δt + 0.5 * (R_ns_k * (a_s_k - g_n) ) * Δt^2
接下来是速度更新：
v_k+1 = v_k + (R_ns_k * (a_s_k - g_n) ) * Δt
最后是姿态或四元数更新：
q_ns_k+1 = q_ns_k ⊗ q{(ω_s_k * Δt)}
我们需要使用以下几个定义：
p是位置。v是速度。q_ns是表示传感器坐标系s相对于导航坐标系n的姿态的四元数。R_ns是与四元数q_ns对应的旋转矩阵。a_s是传感器坐标系中的比力测量值。ω_s是传感器坐标系中的角速度测量值。g_n是导航坐标系中的重力矢量。Δt是时间步长。⊗表示四元数乘法。q{...}表示从角增量向量到四元数的转换。
请注意，增量姿态更新是通过四元数乘法应用的，其中姿态变化（即小角度 ω 乘以 Δt 的四元数形式）出现在右侧，因为姿态变化是在局部传感器坐标系中测量的。
由于我们表示为旋转矩阵的姿态参数，我们的运动模型是非线性的。为了在扩展卡尔曼滤波器中使用它，我们需要围绕预测状态对某个小误差或扰动进行线性化。
为此，我们将定义一个误差状态，由 Δp、Δv 和 Δφ 组成，其中 Δφ 是一个3x1的姿态误差向量。将姿态误差表示在全局或导航坐标系中，而不是局部坐标系中，有一些优势，因此我们将据此进行。这个选择将影响我们雅可比矩阵的推导以及误差状态更新的确切形式。
使用这个误差状态，我们可以推导出具有适当雅可比矩阵的误差动力学方程。
测量模型与滤波器流程
对于我们的测量模型，我们将使用一个非常简单的观测值：位置加上一些加性高斯噪声。
我们将为GNSS位置噪声定义一个协方差矩阵 R_GNSS，为激光雷达位置测量噪声定义一个协方差矩阵 R_Lidar。
这里需要指出，我们假设激光雷达和GNSS将在同一坐标系中提供位置测量值。在实际实现中，GNSS位置估计可能作为一种选择已知地图的方式，然后可以对照该地图比较激光雷达数据。
有了这些定义，我们现在可以描述我们的扩展卡尔曼滤波器。
我们的滤波器将在每次发生IMU测量时循环。
以下是滤波器的主要步骤：
-
状态预测：首先，我们使用运动模型基于上一个状态预测一个新状态。上一个状态可能是完全修正后的状态，也可能是仅使用运动模型传播的状态，这取决于我们在上一个时间步是否收到了GNSS或激光雷达测量值。
-
协方差预测：接下来，我们通过线性化的运动模型传播状态不确定性，同样需要考虑我们之前的状态是修正过的还是未修正的。
-
测量更新判断：此时，如果我们没有任何GNSS或激光雷达测量值可用，我们可以重复步骤1和2。如果我们有，则继续。
-
计算卡尔曼增益：我们将首先计算适用于给定观测值的卡尔曼增益。
-
计算误差状态：然后，我们计算一个误差状态，用于修正我们的预测状态。这个误差状态基于卡尔曼增益与预测位置和观测位置之差的乘积。
-
状态修正：然后，我们使用误差状态来修正预测状态。对于位置和速度，这种修正是直接的，但修正四元数需要一些更复杂的代数运算。我们需要这种特殊的更新，因为四元数是一个受约束的量，不是一个简单的向量。我们选择使用全局姿态误差，这意味着姿态更新涉及在左侧乘以误差状态四元数。这与传播步骤不同，在传播步骤中，我们在右侧乘以定义姿态变化的增量四元数。在完成本课程的最终项目时，应特别注意这一点。
-
协方差更新：最后，我们以通常的方式更新状态协方差。
至此，您就拥有了自己的定位流程。
总结与假设回顾
如果您跟上了到目前为止的所有内容，那么恭喜您。您正走在成为定位专家的道路上。
在本节课结束前，让我们回顾一下在构建流程时所做的几个假设。
我们使用了一个松耦合的扩展卡尔曼滤波器框架，将来自IMU的惯性测量与来自GNSS接收器和激光雷达的位置测量融合在一起。
我们假设GNSS和激光雷达在同一坐标系中为我们提供位置估计，这需要一些预处理，但在实际实现中是可行的。
其次，我们没有考虑IMU测量中的加速度计或陀螺仪偏差。这简化了我们的代数运算，但不是一个现实的假设。幸运的是，如果您遵循我们在此所做的，添加偏差并不是一个巨大的跨越。
接下来，我们没有讨论滤波器状态初始化。这通常被认为是定位过程开始时某个已知的状态。
最后，我们还假设我们的传感器在空间和时间上都是对齐的。我们假设传感器已经过校准，即我们不担心变化的时间步长，或者如何将多个传感器测量值全部对齐到一个坐标系中。
执行后一步是构建精确定位流程非常重要的一部分。我们将在下一节讨论它。
本节课中，我们一起学习了如何构建一个用于自动驾驶汽车定位的松耦合多传感器融合扩展卡尔曼滤波器。我们定义了系统的状态和运动模型，介绍了测量模型，并概述了滤波器的完整预测与更新流程。同时，我们也明确了当前方案所做的关键简化假设，为后续深入实际实现细节奠定了基础。
030：必要之恶
在本节课中，我们将要学习传感器标定。这是设计自动驾驶汽车状态估计器时必须考虑的关键环节。我们将探讨三种主要的标定类型：内参标定、外参标定和时间标定，并解释它们为何如此重要。
上一节我们介绍了如何融合多个传感器数据来估计车辆状态。本节中，我们来看看一个之前被暂时搁置的话题：传感器标定。这是工程师们不太喜欢谈论但绝对至关重要的工作。标定对于正确进行状态估计是必不可少的。
我们将讨论三种主要的传感器标定类型，以及为什么在设计自动驾驶汽车的状态估计器时必须考虑它们。
这三种主要标定类型是：
- 内参标定：处理传感器特定的参数。
- 外参标定：处理传感器在车辆上的位置和朝向。
- 时间标定：处理不同传感器测量之间的时间偏移。
内参标定
首先，让我们看看内参标定。在内参标定中，我们需要确定传感器模型的固定参数，以便在扩展卡尔曼滤波器等估计器中使用它们。
每个传感器都有与其相关的参数，这些参数对于该特定传感器是唯一的，并且通常被认为是恒定的。
以下是内参标定的两个例子：
- 车轮编码器：假设我们有一个安装在汽车车轴上的编码器，用于测量车轮转速
omega。如果我们想用omega来估计车轮的前进速度V，就需要知道车轮的半径R，以便使用公式V = omega * R。这里的R就是传感器模型的一个参数，它特定于编码器所连接的那个车轮。 - 激光雷达：另一个例子是像 Velodyne 这样的激光雷达传感器中扫描线的仰角。仰角是一个固定值，但我们需要提前知道它，以便正确解释每次扫描。
那么，如何确定这些内参呢？以下是几种实用的策略。
以下是几种确定内参的实用策略：
- 制造商标定：最简单的方法是让制造商为你完成。传感器通常在工厂进行标定，并附带规格表，上面列出了所有需要填入模型以理解测量值的数字。这通常是一个好的起点，但可能不足以进行非常精确的状态估计，因为没有两个传感器是完全相同的，参数的真实值总会存在一些差异。
- 手动测量：另一种需要稍多工作量的简单策略是尝试手动测量这些参数。这对于轮胎之类的东西来说很简单，但对于激光雷达来说就不那么直接了，因为用角度尺在传感器内部测量并不实际。
- 状态估计：一种更复杂的方法是将内参作为车辆状态的一部分进行估计，可以在线进行，或者更常见的是在传感器投入运行前作为一个特殊的标定步骤。这种方法的优点是能产生针对特定传感器的精确标定，并且可以处理参数随时间缓慢变化的情况。例如，如果你持续估计轮胎半径，这可能是检测轮胎漏气的好方法。
现在，因为本课程中讨论的估计器是通用的，我们已经具备了进行这种自动标定的工具。为了了解其工作原理，让我们回到汽车在一维空间中运动的例子。
假设我们在后轮上安装了一个编码器来测量车轮转速。如果我们想同时估计车轮半径、位置和速度，我们只需要将其添加到状态向量中，并推导出新的运动和观测模型应该是什么。
- 对于运动模型，除了在矩阵中增加一行一列以表明车轮半径在时间步之间应保持不变外，其他都与之前相同。
- 对于观测模型，我们仍然通过 GPS 直接观测位置，但现在我们也通过编码器观测车轮转速。因此，我们在模型中包含了这个额外的非线性观测。
从这里，我们可以使用扩展或无迹卡尔曼滤波器来估计车轮半径，以及车辆的位置和速度。
外参标定
因此，内参标定对于即使使用单个传感器进行状态估计也是必不可少的。但是，外参标定对于融合来自多个传感器的信息同样重要。
在外参标定中，我们感兴趣的是确定所有传感器的相对位姿，通常是相对于车辆坐标系。例如，我们需要知道 IMU 和激光雷达的相对位姿，以便 IMU 报告的角速度与激光雷达点云在同一坐标系中表达。
就像内参标定一样，有不同的技术来进行外参标定。
以下是几种外参标定的方法：
- CAD 模型：如果你幸运的话，可能会有一个精确的车辆 CAD 模型，其中所有传感器坐标系都已为你妥善布置好。
- 手动测量：如果你不那么幸运，可能会尝试手动测量。不幸的是，这通常很难或不可能精确完成，因为许多传感器的坐标系原点位于传感器内部，而你很可能不想拆解你的汽车和所有传感器。
- 状态估计：幸运的是，我们可以使用类似的技巧，通过将外参包含在我们的状态中来估计它们。这对于任意的传感器配置可能会变得有点复杂，目前仍有大量研究致力于寻找可靠完成此任务的不同技术。
时间标定
最后，一种经常被忽视但仍然重要的标定类型是时间标定。在我们所有关于多传感器融合的讨论中，我们一直隐含地假设我们组合的所有测量都是在完全相同的时刻捕获的，或者至少对于给定的精度水平足够接近。
但是，我们如何决定两个测量是否足够接近以被认为是同步的呢？显然的做法是在车载计算机接收到每个测量时为其打上时间戳，然后将时间最接近的测量匹配起来。例如，如果我们以 15 Hz 的频率获取激光雷达扫描，以 200 Hz 的频率获取 IMU 读数，我们可能希望将每个激光雷达扫描与时间戳最匹配的 IMU 读数配对。
但实际上，在激光雷达或 IMU 实际记录观测值到它到达计算机之间，存在一个未知的延迟。这些延迟可能是由传感器数据传输到主机计算机所需的时间，或由传感器电路执行的预处理步骤引起的，并且不同传感器的延迟可能不同。
因此，如果我们想获得非常精确的状态估计，就需要考虑我们的传感器实际上同步得有多好。有不同的方法来解决这个问题。
以下是处理时间同步的几种方法：
- 假设零延迟：最简单和最常见的做法是假设延迟为 0。通过这种方式，你仍然可以获得一个可用的估计器，但结果可能不如通过更好的时间标定得到的结果准确。
- 硬件同步：另一种常见策略是使用硬件定时信号来同步传感器。但这通常只适用于更昂贵的传感器设置。
- 状态估计：正如你可能猜到的，也可以尝试将这些时间延迟作为车辆状态的一部分进行估计，但这可能会变得复杂。事实上，我博士论文的整个一章都致力于解决相机-IMU 对的时间标定问题。
标定的重要性
我们已经看到了在设计自动驾驶汽车估计器时应考虑的不同类型的标定。但在实践中，良好的标定究竟能带来多大的不同呢？
这里有一个视频，比较了在激光雷达建图任务中良好标定和不良标定的结果。你会注意到，在标定情况下，点云比未标定情况下的点云清晰得多。在未标定情况下，点云是模糊的、有拖影的，你可能会看到物体被重复或实际上完全被遗漏，因为激光雷达没有与惯性传感器正确对齐。
总结
本节课中我们一起学习了传感器标定。没有标定，传感器融合就无法实现。在本视频中，你学习了：
- 内参标定：处理传感器模型参数的标定。
- 外参标定：为我们提供将传感器测量值转换到公共参考系所需的坐标变换。
- 时间标定：处理测量同步，以确保它们都对应于相同的车辆状态。
虽然解决所有这些问题有一些标准技术，但标定仍然是一个非常活跃的研究领域。在下一个视频中，我们将看看当一个或多个传感器发生故障时会发生什么，以及我们如何确保自动驾驶汽车在这些情况下能够继续安全运行。
031：单传感器或多传感器失效应对
在本节课中，我们将要学习传感器冗余的重要性，并探讨当自动驾驶汽车的一个或多个传感器发生故障时，系统应如何应对以确保安全。
概述
任何硬件设备都可能出现故障，传感器也不例外。在自动驾驶汽车这类长期自主运行的系统中，传感器可能因天气损坏、固件故障或线路松动等多种原因而失灵。我们已经了解到，即使所有传感器都正常工作，使用多个互补的传感器也能提供更鲁棒、更精确的定位估计。那么，当一个传感器失效时会发生什么？本节将讨论传感器冗余对鲁棒定位的重要性，并分析几个定位中传感失效的例子。
传感器故障与安全考量
上一节我们介绍了多传感器融合的优势，本节中我们来看看当传感器失效时面临的挑战。构建安全的车辆，关键在于理解并描述当一个或多个传感器发生故障时的情况，以及维持安全运行所需的最少传感配置。
进行此类分析时，一个重要的考量因素是传感器的量程和工作限制。
- GNSS接收器在桥下无法工作，在高楼之间精度也可能下降。
- IMU可能对温度敏感，需要定期重新校准。
- 更重要的是，对于LiDAR、声纳、摄像头或雷达这类感知外部世界的传感器，它们的感知范围在安全运行中扮演着至关重要的角色。
感知范围与故障应对
大多数汽车都配备了长、中、短不同范围的传感器。如果其中之一发生故障，通常需要限制车辆运动，以免影响安全。
以下是不同范围传感器的典型作用：
- 在定位中，我们可能使用短程传感器进行泊车，并确保不与附近车辆发生碰撞。
- 中程传感器可能有助于检测行人和骑行者，并保持车道。
- 长程传感器可以帮助我们检测并预测远处障碍物的运动。
如果其中任何一个失效，我们必须采取适当的措施，以确保车内乘员或周围人员的安全不受影响。这意味着自动驾驶汽车工程师必须考虑执行上述每一步所需的最少允许传感设备。
冗余系统设计范例
对于这类冗余系统设计，我们可以参考一个以严格安全标准著称的行业范例：商业航空。
作为一个冗余系统的例子，可以考虑波音777的主飞行计算机。777遵循三重冗余原则运行。所有主要系统，包括飞行计算机，都有两个备份，每个备份拥有独立的电源、不同的编程和规格。在这里可以看到，每台飞行计算机都使用不同的处理器，以确保架构本身的缺陷不会同时影响所有三台计算机。如果一台计算机失效或发生故障，777会使用共识算法无缝切换到另一台。
缺乏冗余的后果案例
尽管自动驾驶技术已经取得了长足进步，但我们距离2007年DARPA城市挑战赛等早期自动驾驶赛事也才过去十多年。这里可以看到，安全冗余的缺乏导致了MIT和康奈尔大学的车队相互碰撞。如果这次碰撞发生在更高的速度下，结果可能会严重得多。
再看一个更近的例子，这是2015年国际无人驾驶车辆会议上的一辆车，由于未能正确启动自动刹车，撞上了一个充气袋鼠。
最后，考虑这段如今广为人知的记录：一辆特斯拉Model S在加州高速公路上几乎笔直地撞上了分道护栏。这是一个发人深省的提醒，对于设计自动驾驶汽车的工程师和架构师来说，仔细考虑并描述所使用的不同传感模式的各种故障模式至关重要，以确保一个故障组件不会导致悲剧。
总结
本节课中我们一起学习了传感器冗余在自动驾驶定位中的核心作用。始终考虑任何传感器组的局限性，并使用多个互补的传感器来实现鲁棒的定位，这一点至关重要。
本模块内容到此结束。接下来，你将开始课程项目，在那里你将运用在本课程中学到的一切知识，实现一个真实的状态估计系统。
032：状态估计面临的挑战
概述
在本节课中，我们将要学习自动驾驶汽车状态估计所面临的核心挑战。我们将探讨为什么状态估计是一个困难的问题，并深入分析数据关联、传感器局限性和复杂环境条件如何共同构成了这些挑战。
状态估计为何困难？
上一节我们介绍了状态估计的重要性。本节中我们来看看为什么它不是一个简单的问题。
状态估计之所以困难，主要是因为我们需要处理的传感器数据本身就很复杂。系统需要回答“我在哪里”、“我如何移动”、“前方物体的运动趋势是什么”等问题，而处理这些问题的原始数据往往质量不佳。
数据可能难以解读且令人困惑。环境中存在阴影，可能有物体被风吹过街道。车辆自身在移动时可能也不清楚自己的精确位置，但同时却需要追踪和识别前方移动的个体。激光雷达数据可能很稀疏，阳光可能过强导致图像部分曝光不当，或者镜头上可能有雨水。这些情况都使得数据处理变得异常困难。
有时人们会说这是“噪声数据”。虽然传感器数据中确实存在噪声，但更常见的情况是数据本身就非常复杂。例如，处理雷达数据时，人们常说雷达噪声很大。实际上，并非噪声大，而是因为电磁波会从所有物体上反射回来，最终被接收器捕获，这个过程本身就极其复杂。相比之下，处理GPS数据可能显得简单一些，因为它几乎直接提供了位置信息，尽管也存在建筑物反射造成的信号杂乱问题。但处理雷达或激光雷达数据则要复杂得多。
因此，状态估计困难的一个常见原因就是处理复杂信号。其中，有一个特定问题尤其棘手，那就是数据关联。
数据关联的挑战
数据关联是指将你在数据中看到的某些元素和属性，与世界中的实体对应起来。
一个例子是：我想估计一群人的状态，并且我获得了许多速度测量值。那么，哪个测量值属于世界中的哪个实体？这是一个难题。或者，我有一个扫描的激光雷达，它看到了许多点，可能是角点。想象你在做一件简单的事：追踪激光数据中的角点。
当人或车辆移动时，我如何将扫描激光雷达返回的数据子集与一辆车关联起来？一开始没人告诉我那里有多少辆车。可能有一些车，或者那可能根本就是一个建筑的角落，一开始就没动过，但起初它看起来像一辆公交车。
所以，反复困扰我的就是这个状态关联问题。很多时候，如果我能知道自己的状态，即我知道自己在哪里以及周围有什么，那么数据关联问题就会变得微不足道。
但问题是，你必须解决数据关联问题才能解决状态估计问题，这就形成了一种可怕的递归循环。这既有趣又困难，并且在自动驾驶领域，几乎每天都会以不同的形式出现。
复杂环境条件的影响
在自动驾驶汽车所处的全天候户外世界中，是什么让状态估计变得困难？那就是当输入流（相机图像或激光雷达数据流）受到极具挑战性的条件影响时。
例如，使用相机时遇到暴雨和低角度的阳光。在英国，冬天太阳似乎总是紧贴地平线，大多数时候这都非常困难，因为所有东西都变得非常闪亮，世界的外观发生了变化。或者，你可能处于交通极其拥堵的情况下，使用激光雷达时，两辆巨型卡车停在你旁边，你基本上什么都看不见了，因为世界突然变成了卡车的墙壁。
当你要观察的目标和世界被遮挡，或者它们的外观发生变化时，这会令人困惑。或者当它们非常缓慢地经过你，而你实际上是静止的，并且此时你可能没有里程计信号。这再次回到了数据关联问题。
当数据关联问题变得困难，难以将你所看到的与你希望推断的信息联系起来时，状态估计就变得困难了。事实上，你可以将状态估计问题理解为：我能否写下一个方程，用我希望理解的状态估计来解释我所看到的现象？即，我能否用我的状态来解释一个测量值？我们有一些工具可以反推这个关系。
但是，如果你无法写出那种解释，例如，你无法用你的角加速度来解释为什么整个画面变白并且上面有雨滴，那么这就变得相当困难了。因此，总是输入信号的复杂性造成了这种困惑，否则我们只需运行一个标准的优化算法就能轻松完成任务了。
当前传感器的局限性
目前我们使用的传感器，没有一个是足够好的，无法完全满足我们的需求。
以相机为例。与我们的眼睛相比，它们在所能看到的动态范围方面差距很大。例如，当我从停车场驶出时，我的眼睛可以同时看清黑暗处的细节和即将驶入的明亮道路的细节。我们需要在这些相机技术上取得进步。目前有很多相关工作在进行，比如事件相机、高动态范围相机。我希望相机能变得可编程，这方面还有很多工作要做。
激光雷达也需要变得更好。它们仍然是基于飞行时间原理。我们发射出去的光子数量还远远不够，而且探测距离也不够远，所以这还不够好。
雷达则像是第零代产品，远未达到未来应有的水平。
但这里有一个非常棒的事实：就像计算机一样，所有这些技术都注定会变得更好。
技术与生态系统的共同演进
我们将用现有的传感器、现有的车辆、现有的软件、现有的算法、现有的数据以及当前已知的机器学习技术，来制造第一代自动驾驶汽车。
所有这些技术在三年后会变得更好，六年后会更好。就像计算机的发展一样：我们有更好的RAM、更好的硬盘、更好的处理器、更好的屏幕、更好的互联网、更好的网络。它们共同演进。正因为如此，我们现在才有了更好的软件。
所以，这是一个由硬件、软件和应用共同推进的类似生态系统。
总结
本节课中，我们一起学习了自动驾驶汽车状态估计面临的主要挑战。我们了解到，状态估计的困难主要源于处理复杂的传感器数据，尤其是数据关联问题——即如何将观测数据与真实世界中的实体正确匹配。此外，恶劣的环境条件（如暴雨、低角度阳光、遮挡物）会进一步加剧数据关联的难度。最后，我们认识到，尽管当前的各种传感器（相机、激光雷达、雷达）都远未达到理想状态，但整个技术生态系统会像计算机硬件和软件一样，随着时间的推移而共同演进和进步。理解这些挑战是设计更鲁棒的状态估计算法的第一步。
033：最终课程项目概述
在本节课中，我们将要学习本课程的最终项目概述。你已经学习了最小二乘法、线性和非线性卡尔曼滤波、用于位姿估计的GPS/INS传感以及激光雷达。现在是将所有知识整合起来，完成本课程最终项目的时候了。至此，你应该对车辆状态估计的常用方法有了扎实的理解，并准备好在一个更现实的场景中测试你的知识。
🎯 项目目标与背景
上一节我们介绍了课程的知识体系，本节中我们来看看如何将它们应用于实践。对于这个项目，你将有机会应用在整个课程中学到的大部分知识，并观察一个完整状态估计流程中各个组件的内部工作原理和相互作用。
项目将提供在CARLA模拟器中测试驾驶期间收集的数据集。该数据集包含一辆移动的自动驾驶汽车上传感器阵列的测量值。
以下是传感器阵列的组成：
- IMU：惯性测量单元。
- GNSS接收机：全球导航卫星系统接收机。
- LiDAR：激光雷达。
这些传感器以不同的速率提供可靠性各异的测量值。在驾驶过程中，车辆会受到环境、传动系统和悬架引起的各种动态效应的影响。这些效应难以精确建模。因此，在自动驾驶场景中，车辆控制器将依赖于传感器获取的测量值。
你的目标是实现一个状态估计器，融合可用的传感器测量值，以提供对车辆位姿和速度的合理估计。
🔧 项目任务：实现误差状态扩展卡尔曼滤波
具体而言，你将实现误差状态扩展卡尔曼滤波，正如在第2模块中所描述的，因为它具有高度的鲁棒性，并且是状态估计中的主流方法。
我们将为你提供估计器流程的框架，你需要用自己的实现来填充其中的每个独立步骤。
以下是主滤波器循环中的关键步骤：
- 预测步骤：首先使用IMU读数更新状态和不确定性。
- 更新步骤：每当GNSS或LiDAR测量值可用时，执行相应的卡尔曼增益计算、状态和协方差更新。
你将能够访问轨迹大部分部分的真实位姿和速度值，因此可以将你的轨迹估计与真实数据进行比较。
项目的评分将基于你对无法访问真实数据的那部分轨迹的估计精度。
📝 总结与下一步
通过完成这个项目，你应该对构成自动驾驶汽车标准状态估计流程的主要组件有很好的理解。
如果你对本视频未解答的问题有疑问，编程作业本身包含进一步的说明，你也可以随时在讨论区寻求更多信息。
祝你最终项目进展顺利，完成后我们再见。祝你好运。
034：最终项目解决方案
在本节中，我们将回顾最终项目的解决方案。我们将一起分析误差状态扩展卡尔曼滤波器的实现结构，并逐步解读关键代码片段，了解如何融合IMU、GNSS和激光雷达数据来估计车辆的轨迹。
概述
首先，祝贺你完成了最终项目的提交。你使用无车模拟器的测量数据，成功生成了对车辆轨迹的估计。现在，你将有机会将自己的实现与我们的项目解决方案进行比较。
让我们先回顾一下误差状态扩展卡尔曼滤波估计器的一般结构。
滤波器结构回顾 🧠
IMU测量值用于以高更新率驱动运动模型向前推进，产生预测的状态估计。每当来自GNSS接收器或激光雷达单元的位置测量值到达时，我们通过执行卡尔曼滤波器的更新步骤来融合该测量值，从而得到修正后的状态估计。只要车辆在行驶，这个“预测-修正”循环就会持续进行。
状态与协方差初始化 📍
现在，让我们从检查解决方案代码的部分开始，首先是分配状态向量空间的代码行。状态向量包含了每个时间步长下车辆的位置、速度和姿态。
请注意，我们还为每个时间步长分配了一个9x9的协方差（或不确定性）矩阵空间。
我们的运动模型的输入将是来自IMU的比力和角旋转速率测量值。
在本视频和幻灯片中，我们将用浅灰色背景高亮显示代码块。
以下幻灯片上的代码是已经为你提供的。
我们将车辆的初始位姿（即位置和姿态）设置为等于轨迹开始时的真实位姿。
在这里，我们还将初始位姿协方差设置为0，因为真实位姿是精确的。
在提供给你的代码中，我们使用非零协方差初始化了状态。这主要是为了说明相对精确的GNSS和激光雷达测量值如何能快速降低位姿的不确定性。
主循环与运动模型预测 🔄
现在，让我们看看误差状态扩展卡尔曼滤波器的主循环。作为循环的一部分，我们遍历每个IMU测量值，将状态估计在时间上向前传播。
我们的解决方案使用了在模块5第2课中定义的非线性运动模型。该模型以IMU的比力和角旋转速率测量值为输入，输出一个更新的状态估计，即车辆的位姿和速度。请注意，我们利用上一个时间步长的位姿和速度估计来预测当前状态。随着时间的推移，这种航位推算方法会累积误差。
幻灯片上显示的运动模型方程可以直接且相当容易地翻译成Python代码。
我们假设车辆在短时间间隔 ΔT 内经历恒定的线性加速度和恒定的角旋转速率。
我们的解决方案代码与幻灯片上显示的方程有一个不同之处。
我们没有从增量旋转四元数（定义了车辆在时间间隔 ΔT 内的旋转）计算4x4矩阵 Ω，而是选择使用四元数乘法。四元数类已经支持四元数乘法，并且实际上在内部计算这个4x4矩阵。这段代码是提供给你的。
误差状态与线性化动力学 📐
我们已经将标称状态（即我们的预测）在时间上向前传播。然而，标称状态忽略了噪声和扰动。
误差状态扩展卡尔曼滤波器算法在误差状态向量中捕获了这些偏差，该向量包含了一个最小的三参数姿态误差。
我们的下一步是考虑系统的线性化动力学。
公式和相关矩阵显示在此幻灯片上。
推导这些雅可比矩阵相当复杂，因此我们将在这里使用它们而不进行完整的推导。矩阵 L 实际上是常数，因此我们可以预先分配它。
作为一个重要的注意事项，雅可比矩阵 F 具有其特定形式，是因为我们选择使用全局姿态误差表示，而不是局部姿态误差。对此推导细节感兴趣的学习者可以通过阅读Sola的广泛技术报告了解更多信息，链接在模块的资源部分提供。
协方差传播与测量更新 🧮
接下来，我们将状态不确定性在时间上向前传播。
我们将使用之前计算的雅可比矩阵 F_{k-1}。如前所述，矩阵 L 是常数，可以预先计算。协方差计算的结果是时间步长 k 处更新后的协方差矩阵。
请注意，在来自GNSS接收器或激光雷达传感器的测量值到达之前，我们可能会多次执行此传播步骤。
在滤波器内完成预测步骤后，我们检查是否有GNSS接收器或激光雷达测量值可用。
如果来自任一传感器的测量值已到达，我们则执行相应的测量更新，即修正步骤。请记住，IMU的更新速率远快于GNSS单元或激光雷达扫描仪，因此我们可能在两次测量更新之间执行多次预测步骤。
为了方便起见，我们定义了一个新函数来执行测量更新。
该函数接受当前状态和当前状态协方差。在我们的案例中，GNSS接收器和激光雷达传感器都直接提供位置更新，因此测量雅可比矩阵 H 是常数，可以预先计算。
GNSS更新和激光雷达更新之间的唯一区别是测量噪声的方差，如幻灯片所示。
为了执行测量更新或修正，我们首先计算当前时间步长和该测量值的卡尔曼增益矩阵 K_k。
卡尔曼增益的方程可以很容易地翻译成Python代码。
我们使用卡尔曼增益矩阵来计算误差状态，考虑我们的预测车辆位置 p̂_k 与测量位置 y_k 之间的新息或差值。
然后，误差状态向量用于更新标称状态向量。
对于位置和速度状态，更新涉及简单的加法。然而，对于姿态更新，我们需要使用四元数乘法。另外，请注意，我们选择在全局坐标系中表示我们的姿态误差，因此四元数乘法是在左侧使用误差状态四元数进行的。
最后，我们计算修正后的标称状态协方差矩阵 P̂_k。
修正后的状态和状态协方差由函数返回。
结果评估与总结 📊
基本上就是这样。我们已经完成了用于车辆导航的完整误差状态扩展卡尔曼滤波器的实现。
尽管我们使用的传感器模型在某种程度上是简化的，但你构建的滤波器本质上与在真实自动驾驶汽车上运行的滤波器相同。
然后，可以将得到的轨迹估计值与真实值进行比较。
观察真实值和我们的估计值，我们看到车辆沿着一条有几个转弯的弯曲路径行驶。我们只绘制了预先提供给你的那部分轨迹的真实值。用于测试你代码的剩余轨迹被保留下来用于验证。然而，你可以通过检查估计器输出来查看完整轨迹的样子，结果确实相当准确。
现在，让我们看看我们的估计器相对于真实值的误差。
在这些图表上，红色虚线代表根据我们的估计器得出的三个标准差范围。这说明了我们的模型与车辆实际动力学的拟合程度，以及估计器的整体性能。估计误差应始终保持在三个西格玛界限内，正如这里所示。
这是最终项目第一部分的解决方案。在第2和第3部分中，你研究了传感器标定错误和传感器失效的影响。然而，你编写的滤波器代码在这些情况下不应改变。你应该只需要对某些参数进行一些小的调整。
扩展思考 💡
你本可以实现其他估计器来解决这个问题，例如普通的扩展卡尔曼滤波器或无迹卡尔曼滤波器。欢迎将这些实现与误差状态扩展卡尔曼滤波器进行比较，并告诉我们它们的表现如何。
再次祝贺你完成了课程2的最终项目。
状态估计是任何自动驾驶汽车软件栈的重要组成部分。除了GNSS和激光雷达测量值，自动驾驶汽车通常还使用许多其他传感器和信息源来产生可靠的估计。在专业课程的下一门课中，你将学习一个非常重要的传感器——摄像头，以及如何将其用于视觉感知。
继续努力。
自动驾驶汽车的状态估计和定位：P35：课程总结与展望 🎉
在本节课中，我们将对第二门课程“自动驾驶汽车的状态估计和定位”进行全面的回顾与总结，并展望后续的学习内容。
恭喜你完成了自动驾驶汽车专项课程的第二门课程。这是一段充满挑战与收获的旅程。现在，让我们花点时间来回顾和总结你在本课程中学到的核心知识。
你学习了如何将状态估计与定位技术应用于自动驾驶汽车。现在，你应该熟悉了用于估计车辆状态及其他参数的最小二乘法，以及它与用于递归进行状态估计的线性卡尔曼滤波器之间的关系。
上一节我们回顾了线性状态估计的基础，本节中我们来看看如何应对非线性问题。你现在应该了解了三种将线性卡尔曼滤波器适配到非线性状态估计的不同方法：
- 误差状态扩展卡尔曼滤波器：该方法估计的是对名义状态的小修正量，而非估计完整状态。
- 无迹卡尔曼滤波器：该方法利用无迹变换，通过对少量精心选择的样本点进行操作，将概率分布通过非线性函数进行传递。
你学习了自动驾驶汽车上常见的不同类型传感器，包括GPS（或GNSS）接收器、惯性传感器和激光雷达单元。
你现在知道如何为这些传感器建模，并利用它们进行状态估计。无论是单独使用还是组合使用，你通过为自动驾驶汽车开发一个完整的车辆状态估计器，掌握了在传感器融合流程中综合运用它们的方法。
希望你现在能够理解为何传感器融合与校准如此重要，同时也明白了传感器信号丢失会如何影响自动驾驶汽车的性能。
在下一门课程中，Stephen将向你讲授本课程中未涉及的另一种常见传感器：摄像头。
正如你将学到的，视觉感知在自动驾驶的诸多方面都有应用，因此它值得拥有一门独立的课程。有Stephen指导，你将受益匪浅。祝你在第三门课程中学习顺利。
本节课中，我们一起学习了状态估计的核心方法（最小二乘法与卡尔曼滤波器）、应对非线性的滤波技术（ES-EKF与UKF），以及多种传感器（GNSS、IMU、LiDAR）的建模与融合应用。最后，我们展望了下一阶段关于视觉感知的学习重点。
浙公网安备 33010602011771号