由于 GNSS 定位信息更新频率低，不能满足自动驾驶中实时性的要求，且定位信号会因隧道、建筑群等障碍物的遮挡面而中断。而 INS(Inertial Navigation System) 中配备高频传感器，一定时间内可以提供连续的较高精度的汽车位置、速度和航向信息，但其定位误差会随着系统运行时间积累而剧增。将 GNSS 与 INS 相结合，可以利用 GNSS 提供的不随时间增加的高精度定位来纠正 INS 的累积定位误差。同时，INS 可以解决 GNSS 特定场景易受影响的问题。通过结合这两种系统的优点，就能得到实时和精准的定位。如果再与地图匹配技术相结合，利用高精度地图提供的信息，可进一步提高定位精度。

要实现多个定位系统融合，提高定位精度，设计一种融合多个传感器数据的系统尤为重要。这里将从多传感器融合系统简介、系统原理、误差分析以及融合算法等方面对多传感器融合定位系统进行介绍。

1. 多传感器融合系统简介

多传感器数据融合是 20 世纪 80 年代出现的一门新兴学科，它是将不同传感器对某一目标或环境特征描述的信息融合成统一的特征表达信息及其处理的过程。在多传感器系统中，各种传感器提供的信息可能具有不同的特征，如模糊的与确定的、时变的与非时变的、实时的与非实时的等。多传感器数据融合实际上是模拟人脑综合处理复杂问题的过程，通过对各种传感器及其观测信息的合理支配与使用，将各种传感器在空间和时间上的互补与冗余信息，依据某种优化准则加以组合，产生对观测环境或对象的一致性解释和描述，实现多个传感器共同或联合操作，提高整个传感器系统的有效性。数据融合的目标是利用各种传感器的独立观测信息，对数据进行多级别、多方位和多层次的处理，产生新的有意义的信息，这种信息是最佳协同作用的结果，是任何单一传感器无法获得的。

自动驾驶汽车定位的主要模式有 DR、GNSS、GNSS/DR 组合定位模式。在系统精度要求不高的前提条件下可以单独使用这 3 种定位模式。为了进一步提高定位系统的精度，保障自动驾驶的安全，在上述 3 种定位模式中引入了地图匹配，可组合产生出新的 3 种定位模式：DR/MM、GNSS/MM、GNSS/DR/MM。多传感器融合定位系统可在 6 种模式中自动切换以提高整个系统的定位精度和可靠性。接下来对多传感器融合系统体系结构以及系统分层进行介绍。

1.1 多传感器融合系统体系结构

多传感器融合系统体系结构主要包括松耦合(Loosely Coupled)、紧耦合(Tightly Coupled)以及深耦合(Deep Coupled)等组合结构。

1. 松耦合：在松耦合系统里，GNSS 给 INS 提供位置信息，二者硬件上相互独立且随时断开连接，分别输出定位信息与速度信息到融合滤波器，融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后进行输出。GNSS/INS 松耦合系统原理图如下所示：
   

2. 紧耦合：紧耦合系统是将由 GNSS 环码与载波跟踪环解算得到的伪距、伪距率与由惯性导航系统结合自身信息和卫星星历进行计算得到的伪距、伪距率做差，得到伪距与伪距率的测量残差，将其作为融合滤波器的输入观测量，得到惯性导航系统计算误差以及传感器偏差以完成对惯性导航系统的矫正并获得位置与速度的最优估计值。GNSS/INS 紧耦合系统原理图如下所示：
   

3. 深耦合：深耦合系统相对于紧耦合系统，增加了 INS 单元对 GNSS 接收机的辅助。利用 INS 单元结合星历信息可以对伪距和载波的多普勒频移进行估计，利用估计结果辅助接收机的捕获与跟踪环路，可以有效地提高 GNSS 接收机跟踪环路的动态性与灵敏度。

1.2 多传感器融合系统分层

如下图所示，按照信息处理的流程，可将多传感器融合系统划分为数据层融合、特征层融合和决策层融合。

1. 数据层融合：数据层融合也称像素级融合，首先将传感器的观测数据融合，然后从融合的数据中提取特征向量，并进行判断识别。数据层融合需要传感器是同质的（传感器观测的是同一物理量），如果多个传感器是异质的（传感器观测的不是同一个物理量），那么数据只能在特征层或决策层进行融合。数据层融合不存在数据丢失的问题，得到的结果也是最准确的，但计算量最大，且对系统通信带宽的要求很高。
2. 特征层融合：特征层融合属于中间层次，先从每种传感器提供的观测数据中提取有代表性的特征，这些特征融合成单一的特征向量，然后运用模式识别的方法进行处理。这种方法的计算量及对通信带宽的要求相对较低，但部分数据的舍弃使其准确性有所下降。
3. 决策层融合：决策层融合指在每个传感器对目标做出识别后，再将多个传感器的识别结果进行融合，属于高层次的融合。决策层融合由于对可能包含误差的传感器数据进行再处理，产生的结果相对而言最不准确，但其计算量及对通信带宽的要求最低。

2. 多传感器融合定位系统原理

多传感器数据融合定位系统的输入主要来自 GNSS-RTK、惯性导航系统和地图匹配定位系统。融合定位系统对其数据进行预处理、数据配准和数据融合等处理后，可输出汽车自身的速度、位置和姿态信息。下图为多传感器数据融合定位流程示意图：

数据预处理可以考虑为传感器初始化及校准，传感器初始化相对于系统坐标独立地校准每一个传感器。一旦完成了传感器初始化，就可以利用各传感器对共同目标采集得到的数据进行数据配准。所谓数据配准，就是把来自一个或多个传感器的观测或点迹数据与已知或已经确认的事件归并到一起，保证每个事件集合所包含的观测与点迹数据来自同一个实体的概率较大（直白点说就是匹配嘛）。具体的说，就是要把每批目标的观测或点迹数据与事件集合中各自的数据配对。在传感器配准过程中，收集足够的数据点来计算系统偏差，计算得到的系统偏差用来调整随后得到的传感器数据。其次，传感器的配准主要包括时间配准和空间配准两个方面。

1. 时间配准：时间配准，就是将关于同一目标的各传感器不同步的量测信息同步到同一时刻。由于各传感器对目标的量测是相互独立进行的，且采样周期（如观星测量单元和激光雷达大采样周期）往往不同，所以它们向数据处理中心报告的时刻往往也是不同的。另外，由于通信网络的不同延迟，各传感器和融合处理中心之间传送信息所需的时间也各不相同，因此，各传感器上的数据的发送时间有可能存在时间差，所以融合处理前需将不同步的信息配准到相同的时刻。
   
   时间配准的一般做法是将各传感器数据统一到扫描周期较长的一个传感器数据上，目前，常用的方法包括最小二乘法（Least Squares, LS）和内插外推法。这两种方法都对目标的运动模型做了匀速运动的假设，对于做变加速运动的目标，配准效果往往很差。下面仅对基于最小二乘法的时间配准法做简单介绍：
   
   假设有两类传感器，分别表示为传感器1和传感器2，其采样周期分别为 $\tau$ 和 T，且两者之比为 $\tau$：

2. 空间配准：空间配准，就是借助多传感器对空间共同目标的量测结果对传感器的偏差进行估计和补偿。对于同一系统内采用不同坐标系的各传感器的量测，定位时必须将它们转换成同一坐标系中的数据，对于多个不同子系统，各子系统采用的坐标系是不同的，所以在融合处理各子系统间信息前，也需要将它们转换到同一量测坐标系中，而处理后还需将结果转换成各子系统坐标系的数据，再传送给各子系统。
   
   如下图左所示，由于传感器1（传感器2）存在斜距和方位角偏差 $\Delta r_1$、$\Delta {\theta }_1$（$\Delta r_2$、$\Delta {\theta }_2$），导致在系统平面上出现两个目标，而实际上只有一个真实目标，所以需要进行空间配准，配准过程如下右图所示：

上图中，$r_1$、${\theta }_1$ 分别表示传感器 1 的斜距和方位角量测值；$r_2$、${\theta }_2$ 分别表示传感器 2 的斜距和方位角量测值；$(x_{s1},y_{s1})$ 表示传感器 1 在导航坐标平面上的位置；$(x_{s2},y_{s2})$ 表示传感器 2 在导航坐标平面上的位置；$(x_1,y_1)$ 表示传感器 1 在导航坐标系上的测量值；$(x_2,y_2)$ 表示传感器 2 在导航坐标系上的测量值。从上图右可以推导出如下的基本方程：
$$\{\begin{array}{l}{x}_1=x_{\mathrm{s}1}+r_1\sin {\theta }_1\\ y_1=y_{\mathrm{s}1}+r_1\cos {\theta }_1\\ x_2=x_{\mathrm{s}2}+r_2\sin {\theta }_2\\ y_2=y_{\mathrm{s}2}+r_2\cos {\theta }_2\end{array}$$如果忽略噪声，则有：
$$\{\begin{array}{l}{r}_1=r_1^{\prime }+\Delta r_1\\ {\theta }_1={\theta }_1^{\prime }+\Delta {\theta }_1\\ r_2=r_2^{\prime }+\Delta r_2\\ {\theta }_2={\theta }_2^{\prime }+\Delta {\theta }_2\end{array}$$其中，$r_1^{\prime }$、${\theta }_1^{\prime }$ 分别表示目标相对于传感器 1 的真实斜距和方位角；$r_2^{\prime }$、${\theta }_2^{\prime }$ 分别表示目标相对于传感器 2 的真实斜距和方位角；$\Delta r_1$、$\Delta {\theta }_1$ 表示传感器 1 的斜距和方位角偏差；$\Delta r_2$、$\Delta {\theta }_2$ 表示传感器 2 的斜距和方位角偏差。将上面两式合并，并且将所得到的方程相对于 $\Delta r_1$、$\Delta {\theta }_1$ 和 $\Delta r_2$、$\Delta {\theta }_2$ 进行一阶泰勒级数展开，可得：
$$\{\begin{array}{l}{x}_1-x_2\approx \sin {\theta }_1\Delta r_1-\sin {\theta }_2\Delta {\theta }_2+r_1\cos {\theta }_1\Delta {\theta }_1-r_2\cos {\theta }_2\Delta {\theta }_2\\ y_1-y_2\approx \cos {\theta }_1\Delta r_1-\cos {\theta }_2\Delta r_2-r_1\sin {\theta }_1\Delta {\theta }_1+r_2\sin {\theta }_2\Delta {\theta }_2\end{array}$$该式对与目标运动航迹无关的偏差估计方法提供了基础。(这里是为啥)

常用的与目标运动航迹无关的偏差估计方法主要有实时质量控制法(Real Time Quality Control,RTQC)、最小二乘法、极大似然法(Maximum Likelihood,ML)和基于卡尔曼滤波器的空间配准算法等。在给出的几种算法中，实时质量控制法和最小二乘法完全忽略了传感器量测噪声的影响，认为公共坐标系中的误差来源于传感器配准误差（传感器偏差，在下一节会讲）。广义最小二乘法(Generalized Least Square,GLS)和基于卡尔曼滤波器的方法虽然考虑了传感器量测噪声的影响，但只有在量测噪声相对小时，才会产生好的性能。为了克服前两种局限性，提出了精确极大似然(Exact Maximum Likelihood,EML)空间配准算法。

尽管前面已经介绍了多种不同的配准算法，但它们都是基于立体投影在一个二维区域平面上实现的。更准确地说，首先通过立体投影技术把传感器量测投影到与地球正切的局部传感器坐标上，然后变换到区域平面，并利用不同传感器量测之间的差异来估计传感器偏差。虽然立体投影能够减轻单个配准算法的计算复杂度，但这一方法还有一些缺点。首先，立体投影给局部传感器和区域平面的量测都引入了误差。尽管更高阶的近似可以将变换的精度保证到几米，但由于地球本身是一个椭圆形球而不是一个圆柱，因此地球非正圆球体造成的误差仍然存在。其次，立体投影扭曲了数据，值得注意的是立体投影的保角性只能保留方位角，而不能保留斜距。由此可以断定系统偏差将会依赖于量测，而不再是时不变的。这样，在区域平面上的二维配准模型就不能正确地表示实际的传感器模型。这时，一种直接在三维空间中对传感器偏差进行估计的基于地心坐标系的空间配准(Earth Centered Earth Fixed,ECEF)算法被提出以解决上述问题。

3. 多传感器融合误差分析

在多传感器融合系统中，来自多个传感器的数据通常要变换到相同的时空参照系中。但由于存在量测误差直接进行变换很难保证精度来发挥多传感器的优越性，因此在对多传感器数据进行处理时需要寻求一些传感器的配准算法，但配准误差也随之而来。

多传感器配准的主要误差来源有：

1. 传感器的误差，也就是传感器本身因制造误差带来的偏差；
2. 各传感器参考系中量测的方位角、高低角和斜距偏差。通常是因量测系统解算传感器数据时造成的误差；
3. 相对于公共坐标系的传感器的位置误差和计时误差。位置误差通常由传感器导航系统的偏差引起，而计时误差由传感器的时钟偏差所致；
4. 各传感器采用的定位算法不同，从而引起单系统内局部定位误差；
5. 各传感器本身的位置不确定为融合处理而进行坐标转换时产生偏差；
6. 坐标转换的精度不够，为了减少系统的计算负担而在投影变换时采用了一些近似方法（如将地球视为标准的球体等）所导致的误差。

由于以上原因，同一个目标由不同传感器定位产生的航迹基友一定的偏差。这种偏差不同于单传感器定位时对目标的随机量测误差，它是一种固定的偏差（至少在较长时间段内不会改变）。对于单传感器来说，目标航迹的固定偏差对各个目标来说都是一样的，只是产生一个固定的偏移，并不会影响整个系统的定位性能。而对于多传感器系统来说，本来是同一个目标的航迹，却由于互相偏差较大而被认为是不同的目标，从而给航迹关联和融合带来了模糊和困难，使融合处理得到的系统航迹的定位精度下降，丧失了多传感器处理本身应有的优点。

4. 多传感器融合算法

实现多传感器融合定位的算法有很多种，下面首先简要介绍一下各种数据融合算法及其优缺点。其中，卡尔曼滤波算法作为一种经典算法，由于其实时性强、融合精度高等优点，在自动驾驶领域中被广泛使用，下面将重点介绍卡尔曼滤波技术。

4.1 数据融合算法概述

目前，融合算法可概括为随机类和人工智能类。随机类多传感器数据融合算法主要有综合估计法、贝叶斯估计法、D-S证据推理、最大似然估计、贝叶斯估计、最优估计、卡尔曼滤波算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法以及专家系统等。下面简介上述算法：

用某种适当的模型来描述一个实际的物理系统，对分析、研究该物理系统是非常重要的。在导航、信号处理、通信、雷达、声呐等许多实际工程应用中，经常采用动态空间模型来描述其中的许多问题。动态空间模型是一个很重要的统计分析工具，如卡尔曼滤波器采用的高斯-马尔可夫线性模型就是一个很好的例子，它用状态方程（动力学方程）来描述状态随时间演变的过程，而用观测方程来描述与状态有关的噪声变量。同样的，只要将高斯-马尔可夫线性模型写成一般的数学映射，就可以用这两个方程来描述更一般的动态系统了：

$$\begin{gathered} 状态方程:X_k=f\left(X_{k-1},W_k\right) \\ 观测方程:L_k=h\left(X_k,V_k\right) \end{gathered}$$ 上式被称为动态空间模型。其中, $X_k\in {\mathbf{R}}^{k_x}$ 为系统在 $k$ 时刻的状态, $L_k\in {\mathbf{R}}^{k_x}$ 为系统状态 $X_k$ 的观测值; $W_k$ 、 $V_k$ 分别为过程和观测噪声。

1. 综合平均法：该算法是把来自多个传感器的众多数据进行综合平均。它适用于用同样的传感器检测同一个目标的情况。如果对一个检测目标进行了 $k$ 次检测，其平均值 $\bar{S}={\sum }_{i=1}^k{W}_i{S}_i/{\sum }_{i=1}^k{W}_i,W_i$ 为分配给第 $i$ 次检测的权值。
2. 贝叶斯估计法：贝叶斯估计理论是较经典的统计估计理论,具有更大的优势,逐渐 成为科学界推理的一个重要工具,提供了一种与传统算法不同的概率分布形式的估计。贝 叶斯推理技术主要用来进行决策层融合。贝叶斯估计法通过先验信息和样本信息合成为后 验分布,对检测目标做出推断。因此贝叶斯估计是一个不断预测和更新的过程。这样就包 括了观测值和先验知识在内的所有可以利用的信息,得到的估计误差自然较小。
3. D-S 证据推理：D-S 证据推理是目前数据融合技术中比较常用的一种算法,该算法通常用来对检测目标的大小、位置以及存在与否进行推断，采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理。提取的特征参数构成了该理论中的证据，利用这些证据构造相应的基本概率分布函数，对于所有的命题赋予一个信任度。基本概率分布函数及其相应的分辨框合称为一个证据体。因此，每个传感器就相当于一个证据体。而多个 传感器数据融合，实质上就是在同一分辨框下，利用 Dempster 合并规则将各个证据体合并成一个新的证据体，产生新证据体的过程就是 D-S 证据推理数据融合。
4. 卡尔曼滤波算法：卡尔曼滤波在控制领域得到广泛应用以后，也逐渐成为多传感器数据融合系统的主要技术手段之一。联合卡尔曼滤波器的设计思想是先分散处理、再全局融合，即在诸多非相似子系统中选择一个信息全面、输出速率高、可靠性绝对保证的子系统作为公共参考系统，与其他子系统两两结合，形成若干子滤波器。各子滤波器并行运行，获得建立在子滤波器局部观测基础上的局部最优估计，这些局部最优估计在主滤波器内按融合算法合成，从而获得建立在所有观测基础上的全局估计。
5. 模糊逻辑法：针对数据融合中所检测的目标特征具有某种模糊性的现象,利用模 糊逻辑算法来对检测目标进行识别和分类。建立标准检测目标和待识别检测目标的模糊子 集是此算法的研究基础。
6. 神经网络算法：神经网络是一种试图仿效生物神经系统处理信息方式的新型计算模型。一个神经网络由多层处理单元或节点组成，可以用各种方法互联。在指挥和控制多传感器数据融合的系统中,神经网络的输入可能是与一个目标有关的测量参数集，输出可能 是目标身份，也可能是推荐的响应或行动。基于神经网络的融合优于传统的聚类算法，尤其 是当输人数据中带有噪声和数据不完整时。然而，要使神经网络算法在实际的融合系统中得到应用,无论在网络结构设计或是算法规则方面，还有许多基砩工作要做,如网络模型、网络的层次和每层的节点数、网络学习策略、神经网络算法与传统分类算法的关系和综合应用等。
7. 专家系统：专家系统是一组计算机程序，它获取专家们在某个特定领域内的知识，然后根据专家的知识或经验导出一组规则，由计算机做出本应由专家做出的结论。目前，专家系统已在军用和民用领域得到了广泛应用。

此外，其他数据融合算法还有品质因数、模板算法、聚合分析、统计决策理论等。各种融合算法的特点比较如下表所示：

融合算法	运行环境	信息类型	信息表示	不确定性	融合技术	适用范围
综合平均法	动态	冗余	原始读数值	——	加权平均	低层融合
贝叶斯估计法	静态	冗余	概率分布	高斯噪声	贝叶斯估计	高层融合
D-S 证据推理	静态	冗余互补	命题	——	逻辑推理	高层融合
卡尔曼滤波	动态	冗余	概率分布	高斯噪声	系统模型滤波	低层融合
模糊逻辑法	静态	冗余互补	命题	隶属度	逻辑推理	高层融合
神经网络算法	动、静态	冗余互补	神经元输入	学习误差	神经元网络	低/高层融合
专家系统	静态	冗余互补	命题	置信因子	逻辑推理	高层融合

4.2 卡尔曼滤波算法

4.2.1 卡尔曼滤波

4.2.2 扩展卡尔曼滤波

4.2.3 无迹卡尔曼滤波

4.2.4 联邦卡尔曼滤波

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》，笔者不是自动驾驶领域的专家，只是一个在探索自动驾驶路上的小白，此系列丛书尚未阅读完，也是边阅读边总结边思考，欢迎各位小伙伴，各位大牛们在评论区给出建议，帮笔者这个小白挑出错误，谢谢！
此专栏是关于《自动驾驶技术概览》书籍的笔记。

---

---

3.定位系统之多传感器融合定位技术

3.1 多传感器融合介绍

1. GNSS：GNSS系统应用于无人车辆时，更新频率过低的缺点突出，10Hz左右的更新频率不能满足无人驾驶系统对实时定位的要求；
2. 由于误差积累，INS惯性导航系统的准确度随时间和行驶距离的增加而降低，故也无法完整保证定位的准确性，但INS惯性导航系统更新频率高，达200Hz以上，满足自动驾驶系统需求；
3. 通过适当的方法将GNSS和INS的数据进行融合，可以为车辆定位提供准确、实时的位置信息；
4. 多传感器信息融合是将不同传感器对某一目标或环境特征描述的信息，综合成统一的特征表达信息及其处理的过程；
5. 多传感器信息融合，协调利用多个传感器资源实现多传感器信息融合，通过对各种传感器及其观测信息的合理支配及使用，将各种传感器在空间和时间上的互补与冗余信息依据某种优化准则加以组合，产生对观测环境或对象的一致性解释和描述；
6. 多传感器信息融合的目标：利用各种传感器分离观测信息，对数据进行多级别、多方位、多层次的处理，产生新的有意义的信息；

3.2 多传感器融合原理

多传感器融合的数据主要包括：GNSS-RTK、惯性导航系统和特征匹配自定位系统的输入数据；对这些数据进行预处理、数据配准、数据融合等处理后，输出车辆自身的速度、位置、姿态信息；

1. 数据预处理。数据预处理可以考虑为传感器初始化及校准，传感器初始化即相对于系统坐标独立地校准每一个传感器；一旦完成传感器初始化，就可以利用共同的目标开始相对的数据配准过程；
2. 数据配准。把来自一个或多个传感器的观测或点迹与已知或已经确认的事件归并到一起，保证每个事件集合所包含的观测来自同一个实体的概率较大；
3. 传感器的配准。指多传感器数据"无误差"转换时所需要的处理过程，一般包括：时间配准和空间配准。
   1. 时间配准。
      将关于同一目标的各传感器不同步的测量信息同步到同一时刻；
   2. 空间配准。
      借助于多传感器对空间共同目标的测量，对传感器的偏差进行估计和补偿；对于同一系统内采用不同坐标系的各传感器的测量，定位时必须将它们转换成同一坐标系中的数据；对于多个不同子系统，各子系统采用的坐标系是不同的，在融合处理各子系统的信息前，需要将它们转换到同一测量坐标系中，处理后需要将结果转换成各子系统坐标系的数据后，再传送到各个子系统。
4. 数据融合。
   融合算法分为：随机类和人工智能类；
   随机类多传感器数据融合方法主要有：贝叶斯推理、D-S证据理论，及包括最大似然估计、综合平均法、贝叶斯估计、D-S法、最优估计、卡尔曼滤波、鲁棒估计等在内的估计理论；
   人工智能类多传感器数据融合方法主要有：基于神经网络的多传感器数据融合、基于模糊聚类的数据融合及专家系统等；

经典融合算法：

1. 综合平均法。
   把来自多个传感器的众多数据进行综合平均；适用于用同样的传感器检测同一个目标的情况；如果对一个检测目标进行了$k$次检测，平均值为：$$\overline{S}=\sum _{i=1}^k{W}_i{S}_i/\sum _{i=1}^k{W}_i$$
   $其中W_i为分配给第i次检测的权值$；
2. 贝叶斯估计法。
   主要用来进行决策层融合，通过先验信息和样本信息合成为后验分布，对检测目标做出推断；
3. D-S法。
   D-S法通常用来对检测目标的大小、位置及存在与否进行推断；采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理；提取的特征参数构成了该理论中的证据，利用这些证据构成相应的基本概率分布函数，对于所有的命题赋予一个信任度；基本概率分布函数及相应的分辨框合称为一个证据体；故每个传感器相当于一个证据体，多个传感器信息融合，实质是在同一分辨框下，利用Dempster合并规则将各个证据体合并成一个新的证据体；产生新证据体的过程就是D-S法信息融合；
4. 卡尔曼滤波方法。
   卡尔曼滤波器可以从一组有限的、包含噪声的、通过对物体位置的观察序列预测出物体的位置坐标及速度；卡尔曼滤波器运行时的两个阶段：预测阶段基于上一个时间点的位置信息去预测当前的位置信息；更新阶段通过对当前物体位置的观测去纠正位置预测，从而更新物体的位置信息；
   通过卡尔曼滤波器，将惯性传感器与GNSS数据进行融合；首先，依据上一次的位置估算使用惯性传感器对当前位置进行实时预测；在得到新的GNSS数据前，只能通过积分惯性传感器的数据预测当前位置；惯性传感器的定位误差随着时间增加，所以当接收到新的GNSS信号时，由于GNSS数据比较精确，可以使用GNSS数据对当前的位置预测进行更新；通过不断地执行这两个步骤，系统可以取两者所长，对无人车进行准确的实时定位；
   假设惯性传感器的频率是1kHz，GNSS频率是10Hz，那么每两次GNSS更新之间会使用100个惯性传感器数据点进行位置预测；

第十章 基于优化的定位方法

本章是基于先验地图的图优化方法，先验地图的构建可参考多传感器融合定位 第九章 基于优化的建图方法

代码下载：

1.环境配置：

出现以下问题，是由于 make_unique 是c++ 14的新特性，需要在CMakelists.txt 中添加c++14 的编译指向。参考链接

lidar_localization/src/models/sliding_window/ceres_sliding_window.cpp:25:38: error: ‘make_unique’ is not a member of ‘std’
     config_.loss_function_ptr = std::make_unique<ceres::CauchyLoss>(new ceres::CauchyLoss(1.0));

解决， 在CMakelists.txt 添加

set(CMAKE_CXX_STANDARD 14)

2.因子图优化代码补充

参考博客：

因子图优化 SLAM 研究方向归纳

多传感器融合定位 第九章 基于优化的建图方法

多传感器融合定位理论基础（十二）：滑动窗口原理及应用

本章因子图优化，涉及的内容较多，建议先修参考一下内容：

VINS系列相关代码：滑动窗口部分

邱笑晨博士的《预积分总结与公式推导》(发表于泡泡机器人公众号)

机器人状态估计：理论推导部分

深蓝学院《从零手写VIO》课程：滑窗和边缘话理论讲解 (重点参考)

多传感器融合定位理论基础（十二）：滑动窗口原理及应用 (重点参考)

以下代码补充的公式摘自GeYao助教的手推公式，图表摘自任乾老师课上的笔记，需要补全的代码主要有两大类：

代码参考：1. GEYAO助教的github 2.张嘉皓助教的github

1.实现预积分、地图匹配、边缘化、帧间匹配四种优化因子。

2.将上述四种约束因子，加入滑窗，进行优化

2.1 实现预积分、地图匹配、边缘化、帧间匹配四种优化因子

2.1.1 激光里程计因子

a.示意图

b.激光里程计因子 残差函数和雅克比矩阵 推导

注意的是，GeYao助教的公式推导和代码少了，位置残差对姿态的雅克比，以下为补充的推导。

c.代码补全

FILE: factor_prvag_relative_pose.hpp

    //
    // TODO: get square root of information matrix:
    //
    Eigen::Matrix<double, 6, 6>  sqrt_info =  Eigen::LLT<Eigen::Matrix<double, 6 ,6>>(
      I_
    ).matrixL().transpose() ;

    //
    // TODO: compute residual:
    //
    Eigen::Map<Eigen::Matrix<double, 6 ,1>>  residual(residuals);     //  残差 r_P  r_R

    residual.block(INDEX_P,  0 , 3 , 1)   =  ori_i.inverse() * (pos_j - pos_i) - pos_ij ;
    residual.block(INDEX_R, 0  , 3 , 1)   =  (ori_i.inverse()*ori_j*ori_ij.inverse()).log( );  

    //
    // TODO: compute jacobians:     因为是二元边，所以需要求解两个jacobian
    //
    if ( jacobians ) {
      // compute shared intermediate results:
      const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix();
      const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1));    //  右雅克比
      const Eigen::Vector3d pos_ij = ori_i.inverse() * (pos_j - pos_i) ;

      if ( jacobians[0] ) {      //   残差rL0(rp  rq )  对 T0(p q)  M0(v ba bg) 的雅克比 
        // implement computing:
        Eigen::Map<Eigen::Matrix<double, 6 , 15,  Eigen::RowMajor>>  jacobian_i (jacobians[0] );  //  col : rp_i[3] rq_i[3]  row : p[3] q[3] v[3] ba[3] bg[3]  
        jacobian_i.setZero();

        jacobian_i.block<3, 3>(INDEX_P, INDEX_P) =  -R_i_inv;
        jacobian_i.block<3, 3>(INDEX_R,INDEX_R)  =  -J_r_inv*(ori_ij*ori_j.inverse()*ori_i).matrix();
        jacobian_i.block<3, 3>(INDEX_P,INDEX_R) =  Sophus::SO3d::hat(pos_ij).matrix();

        jacobian_i = sqrt_info * jacobian_i ;        //   注意 sqrt_i 为对角的协方差矩阵对角线为观测的方差，可理解为传感器的测量误差，用于调整权重用
      }

      if ( jacobians[1] ) {      //  残差rL0(rp  rq )  对 T0(p q)  M0(v ba bg) 的雅克比 
        // implement computing:
        Eigen::Map<Eigen::Matrix<double, 6 ,15, Eigen::RowMajor>> jacobian_j (jacobians[1]);
        jacobian_j.setZero();

        jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv;
        jacobian_j.block<3, 3>(INDEX_R,INDEX_R)  = J_r_inv*ori_ij.matrix();

        jacobian_j = sqrt_info * jacobian_j ;
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //
    residual = sqrt_info * residual;

2.1.2 地图匹配因子

a.示意图

b.地图匹配因子 残差函数和雅克比矩阵 推导

c.代码补全

    //
    // TODO: get square root of information matrix:
    //
    // Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html
    Eigen::Matrix<double, 6, 6> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 6, 6>>(
      I_
    ).matrixL().transpose();

    //
    // TODO: compute residual:
    //
    Eigen::Map<Eigen::Matrix<double, 6 ,1>>  residual(residuals);

              residual.block(INDEX_P, 0 , 3 , 1 )   =  pos - pos_prior ;
              residual.block(INDEX_R,0 , 3 , 1 )    = (ori*ori_prior.inverse()).log();

    //
    // TODO: compute jacobians:   一元边
    //
    if ( jacobians ) {
      if ( jacobians[0] ) {
        // implement jacobian computing:
        Eigen::Map<Eigen::Matrix<double, 6, 15,  Eigen::RowMajor>> jacobian_prior(jacobians[0] );
        jacobian_prior.setZero();

        jacobian_prior.block<3, 3>(INDEX_P,  INDEX_P) = Eigen::Matrix3d::Identity();
        jacobian_prior.block<3, 3>(INDEX_R, INDEX_R)  = JacobianRInv(
                                  residual.block(INDEX_R, 0, 3, 1)) * ori_prior.matrix();
        
        jacobian_prior = sqrt_info * jacobian_prior ;
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //
		residual = sqrt_info * residual;

2.1.3 IMU预积分因子

a.示意图

b.IMU预积分因子 残差函数和雅克比矩阵 推导

GeYao助教的推导为基于SO3形式的推导，代码也是SO3的形式。四元数的推导部分上一章已推导完成，可参考多传感器融合定位 第九章 基于优化的建图方法

c.代码补全

    //
    // TODO: get square root of information matrix:
    // Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html
    Eigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_);
    // sqrt_info 为上三角阵
    Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();

    //
    // TODO: compute residual:
    //
     Eigen::Map<Eigen::Matrix<double, 15, 1>>  residual(residuals);
     
     residual.block<3, 1>(INDEX_P, 0) = ori_i.inverse().matrix() * (pos_j - pos_i - (vel_i - 0.5 * g_ * T_) * T_) - alpha_ij ;
     residual.block<3, 1>(INDEX_R,0) = (Sophus::SO3d::exp(theta_ij).inverse()*ori_i.inverse()*ori_j).log(); 
     residual.block<3, 1>(INDEX_V,0)  = ori_i.inverse()* (vel_j - vel_i + g_ * T_) - beta_ij ;
     residual.block<3, 1>(INDEX_A,0)  = b_a_j - b_a_i ; 
     residual.block<3, 1>(INDEX_G,0)  = b_g_j - b_g_i;

    //
    // TODO: compute jacobians:   imu预积分的残差 对状态量的雅克比，第九章已推导
    //
    if ( jacobians ) {
      // compute shared intermediate results:
      const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix();
      const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1));    //  右雅克比
     
      if ( jacobians[0] ) {
        Eigen::Map<Eigen::Matrix<double, 15 , 15,  Eigen::RowMajor>>  jacobian_i (jacobians[0] );  
        jacobian_i.setZero();
 
         // a. residual, position:
        jacobian_i.block<3, 3>(INDEX_P, INDEX_P) = -R_i_inv;
        jacobian_i.block<3, 3>(INDEX_P, INDEX_R) = Sophus::SO3d::hat(
          ori_i.inverse() * (pos_j - pos_i - (vel_i - 0.50 * g_ * T_) * T_)
        );
        jacobian_i.block<3, 3>(INDEX_P, INDEX_V) = -T_*R_i_inv;
        jacobian_i.block<3, 3>(INDEX_P, INDEX_A) = -J_.block<3,3>(INDEX_P, INDEX_A);
        jacobian_i.block<3, 3>(INDEX_P, INDEX_G) = -J_.block<3,3>(INDEX_P, INDEX_G);

        // b. residual, orientation:
        jacobian_i.block<3, 3>(INDEX_R, INDEX_R) = -J_r_inv*(ori_j.inverse()*ori_i).matrix();
        jacobian_i.block<3, 3>(INDEX_R, INDEX_G) = -J_r_inv*(
          Sophus::SO3d::exp(residual.block<3, 1>(INDEX_R, 0))
        ).matrix().inverse()*J_.block<3,3>(INDEX_R, INDEX_G);

        // c. residual, velocity:
        jacobian_i.block<3, 3>(INDEX_V, INDEX_R) = Sophus::SO3d::hat(
          ori_i.inverse() * (vel_j - vel_i + g_ * T_)
        );
        jacobian_i.block<3, 3>(INDEX_V, INDEX_V) = -R_i_inv;
        jacobian_i.block<3, 3>(INDEX_V, INDEX_A) = -J_.block<3,3>(INDEX_V, INDEX_A);
        jacobian_i.block<3, 3>(INDEX_V, INDEX_G) = -J_.block<3,3>(INDEX_V, INDEX_G);

        // d. residual, bias accel:
        jacobian_i.block<3, 3>(INDEX_A, INDEX_A) = -Eigen::Matrix3d::Identity();

        // d. residual, bias accel:
        jacobian_i.block<3, 3>(INDEX_G, INDEX_G) = -Eigen::Matrix3d::Identity();

        jacobian_i = sqrt_info * jacobian_i;
      }

      if ( jacobians[1] ) {
        Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>> jacobian_j(jacobians[1]);
        jacobian_j.setZero();

        // a. residual, position:
        jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv;

        // b. residual, orientation:
        jacobian_j.block<3, 3>(INDEX_R, INDEX_R) = J_r_inv;

        // c. residual, velocity:
        jacobian_j.block<3, 3>(INDEX_V, INDEX_V) = R_i_inv;

        // d. residual, bias accel:
        jacobian_j.block<3, 3>(INDEX_A, INDEX_A) = Eigen::Matrix3d::Identity();

        // d. residual, bias accel:
        jacobian_j.block<3, 3>(INDEX_G, INDEX_G) = Eigen::Matrix3d::Identity();

        jacobian_j = sqrt_info * jacobian_j;
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //
    residual = sqrt_info * residual;

2.1.4 边缘化先验因子

边缘化先验因子部分，算是滑窗算法的精华部分，可参考vins的做法

a.示意图

b.边缘先验因子 残差函数和雅克比矩阵 推导

c.代码补全

地图匹配H B阵

    //
    // TODO: Update H:
    //
    // a. H_mm:
    H_.block<15,  15>(INDEX_M, INDEX_M) += J_m.transpose() * J_m ;

    //
    // TODO: Update b:
    //
    // a. b_m:
    b_.block<15, 1>(INDEX_M , 0) +=  J_m.transpose() * residuals ;  //  因子图叠加

点云匹配H B阵

   //
    // TODO: Update H:
    //
    // a. H_mm:
    H_.block<15, 15>(INDEX_M, INDEX_M)  += J_m.transpose() * J_m;
    // b. H_mr:
    H_.block<15, 15>(INDEX_M, INDEX_R)   += J_m.transpose()* J_r;
    // c. H_rm:
    H_.block<15,  15>(INDEX_R, INDEX_M)  += J_r.transpose() * J_m; 
    // d. H_rr:
    H_.block<15,  15>(INDEX_R, INDEX_R)   += J_r.transpose() * J_r;

    //
    // TODO: Update b:
    //
    // a. b_m:
    b_.block<15,  1>(INDEX_M, 0) += J_m.transpose() * residuals ;
    // a. b_r:
    b_.block<15,   1>(INDEX_R,  0) += J_r.transpose() * residuals ;

IMU预积分H阵 B阵

    //
    // TODO: Update H:
    //
    // a. H_mm:
    H_.block<15, 15>(INDEX_M, INDEX_M)  += J_m.transpose() * J_m;
    // b. H_mr:
    H_.block<15, 15>(INDEX_M, INDEX_R)   += J_m.transpose()* J_r;
    // c. H_rm:
    H_.block<15,  15>(INDEX_R, INDEX_M)  += J_r.transpose() * J_m; 
    // d. H_rr:
    H_.block<15,  15>(INDEX_R, INDEX_R)   += J_r.transpose() * J_r;

    //
    // TODO: Update b:
    //
    // a. b_m:
    b_.block<15,  1>(INDEX_M, 0) += J_m.transpose() * residuals ;
    // a. b_r:
    b_.block<15,   1>(INDEX_R,  0) += J_r.transpose() * residuals ;

边缘化操作 (核心代码)

此部分代码主要参考自vins部分

  void Marginalize(
    const double *raw_param_r_0
  ) {
    // TODO: implement marginalization logic
    //  save x_m_0 
    Eigen::Map<const  Eigen::Matrix<double, 15 , 1>>  x_0(raw_param_r_0);
    x_0_ = x_0 ;
    //   marginalize
    const Eigen::MatrixXd  &H_mm = H_.block<15,  15>(INDEX_M, INDEX_M);
    const Eigen::MatrixXd  &H_mr   = H_.block<15,   15>(INDEX_M,INDEX_R);
    const Eigen::MatrixXd  &H_rm   = H_.block<15,   15>(INDEX_R,INDEX_M);
    const Eigen::MatrixXd  &H_rr     = H_.block<15,    15>(INDEX_R,INDEX_R);

    const Eigen::VectorXd  &b_m = b_.block<15,  1>(INDEX_M, 0);
    const Eigen::VectorXd  &b_r   = b_.block<15,   1>(INDEX_R, 0);

    Eigen::MatrixXd H_mm_inv = H_mm.inverse();
    Eigen::MatrixXd H_marginalized = H_rr - H_rm * H_mm_inv * H_mr ;
    Eigen::MatrixXd b_marginalized = b_r - H_rm * H_mm_inv * b_m ;
    // 线性化残差 和 雅克比
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H_marginalized);
    Eigen::VectorXd S = Eigen::VectorXd(
      (saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array(), 0)
    );
    Eigen::VectorXd S_inv = Eigen::VectorXd(
      (saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array().inverse(), 0)
    );

    Eigen::VectorXd S_sqrt = S.cwiseSqrt();
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();

    // finally:
    J_ = S_sqrt.asDiagonal() * saes.eigenvectors().transpose();         //  b0
    e_ = S_inv_sqrt.asDiagonal() * saes.eigenvectors().transpose() * b_marginalized;      //   eo
  }

关于函数SelfAdjointEigenSolver的解析（1.摘自 张嘉皓助教）(2.摘自游博的知乎)

1.由于每次迭代需要使用奇异值分解,从边缘化后的信息矩阵中恢复出来雅克比矩阵 linearized_jacobians 和残差 linearized_residuals ,这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。这一部分和vins是差不多的,如果看不懂的可以去 vins 那边参考参考。其中推导部分相对比较简单,如图片所示

2.游博知乎解释-更新先验残差

计算边缘因子传递的残差

    //
    // TODO: compute residual:
    //
    Eigen::Map<Eigen::Matrix<double, 15, 1>>  residual(residuals);
    residual = e_ + J_ * dx ;       //  e_prior

计算边缘因子传递的雅克比

    //
    // TODO: compute jacobian:
    //
    if ( jacobians ) {
      if ( jacobians[0] ) {
        // implement computing:
        Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>>  jacobian_marginalization(jacobians[0]);
        jacobian_marginalization.setZero();

        jacobian_marginalization = J_ ;        
      }
    }

2.2 将四种约束因子，加入滑窗，进行优化

2.2.1 更新状态量

FILE : param_prvag.hpp

        //
        // TODO: evaluate performance penalty of applying exp-exp-log transform for each update
        //
        ori_plus = (Sophus::SO3d::exp(ori) * Sophus::SO3d::exp(d_ori)).log();
        vel_plus = vel + d_vel;
        b_a_plus = b_a + d_b_a;
        b_g_plus = b_g + b_g_plus;

2.2.2 将因子添加到优化框架中

FILE : sliding_window.cpp

    //
    // add node for new key frame pose:
    //
    // fix the pose of the first key frame for lidar only mapping:
    if ( sliding_window_ptr_->GetNumParamBlocks() == 0 ) {
        // TODO: add init key frame
        sliding_window_ptr_->AddPRVAGParam(current_key_frame_, true);
    } else {
        // TODO: add current key frame
        sliding_window_ptr_->AddPRVAGParam(current_key_frame_, false);
    }

        // TODO: add constraint, GNSS position:
        sliding_window_ptr_->AddPRVAGMapMatchingPoseFactor(
            param_index_j, 
            prior_pose, measurement_config_.noise.map_matching
        );

        // a. lidar frontend:
        //
        // get relative pose measurement:
        Eigen::Matrix4d relative_pose = (last_key_frame_.pose.inverse() * current_key_frame_.pose).cast<double>();
        // TODO: add constraint, lidar frontend / loop closure detection:
        sliding_window_ptr_->AddPRVAGRelativePoseFactor(
            param_index_i, param_index_j, 
            relative_pose, measurement_config_.noise.lidar_odometry
        );

        //
        // b. IMU pre-integration:
        //
        if ( measurement_config_.source.imu_pre_integration ) {
            // TODO: add constraint, IMU pre-integraion:
            sliding_window_ptr_->AddPRVAGIMUPreIntegrationFactor(
                param_index_i, param_index_j,
                imu_pre_integration_
            );
        }

2.2.3 ceres 中添加残差块

FILE : ceres_sliding_window.cpp

创建问题

        // TODO: create new sliding window optimization problem:
        ceres::Problem problem;

添加待优化参数快

            // TODO: add parameter block:      添加待优化的参数快
            problem.AddParameterBlock(target_key_frame.prvag, 15, local_parameterization);

            if( target_key_frame.fixed ) {
                    problem.SetParameterBlockConstant(target_key_frame.prvag);
            }

添加边缘先验因子残差块

            // add marginalization factor into sliding window
            problem.AddResidualBlock(
                factor_marginalization,
                NULL,
                key_frame_r.prvag    //  一元边
            );

添加地图匹配因子残差块

                // TODO: add map matching factor into sliding window
                problem.AddResidualBlock(
                            factor_map_matching_pose,
                            NULL,     // loss_function 
                            key_frame.prvag     //  一元边
                );

添加帧间点云匹配匹配因子残差块

                // TODO: add relative pose factor into sliding window
                problem.AddResidualBlock(
                        factor_relative_pose,
                        NULL,   // loss_function
                        key_frame_i.prvag, key_frame_j.prvag        //  二元边
                );

添加IMU预积分因子残差块

                // TODO: add IMU factor into sliding window
                problem.AddResidualBlock(
                        factor_imu_pre_integration,
                        NULL,   // loss_function
                        key_frame_i.prvag,  key_frame_j.prvag           // 二元边
                );

3. evo 评估

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

3.1 EKF 与 因子图优化方法比较

Factor Graph	EKF IMU-Lidar Fusion
max 5.546784 mean 1.907476 median 1.682371 min 0.000001 rmse 2.167268 sse 21268.254731 std 1.028876	max 1.166536 mean 0.244853 median 0.187717 min 0.010400 rmse 0.298805 sse 391.959520 std 0.171264

结果：EKF的效果暂时来看要比图优化的效果要好，但是这不一定说明图优化的效果不好，原因是KITTI数据集自身也存在一定的问题，并且在这次对比实验中，EKF 和 factor graph 所使用的先验地图构建方法不一样，EKF 使用第四章的激光里程计mapping,而factor graph 使用的是第九章imu预积分图优化的方法构建的地图。

3.2不同滑窗长度比较

FILE : lidar_localization/config/matching/sliding_window.yaml

修改不同的滑窗长度

# sliding window size:
#
sliding_window_size: 20					#滑窗长度为20帧

sliding windows length	2	10	15	20	30	地图
max	5.590900	5.590900	5.546784	5.510841	8.340555	2.325375
mean	2.345147	2.26939	1.907476	1.886341	2.906110	0.801601
median	2.178942	2.111248	1.682371	1.718590	2.988234	0.701903
min	0.000001	0.000001	0.000001	0.000001	0.000001	0.023720
rmse	2.648810	2.531902	2.167268	2.143079	3.207210	0.928723
sse	31769.338233	29026.859453	21268.254731	20796.134607	46452.465579	1650.013734
std	1.231456	1.122670	1.028876	1.017106	1.356733	0.469001

总结：

1.滑动窗口的长度也是一个关键的因素,过高或者过低的窗口长度会造成精度的降低。

2.按照乾哥的说法EKF的滤波方法就相当于滑动窗口为1的情况，可以看出，选取适当滑窗大小是有必要的，能够直接影响最后的性能。

4. 注意

sqrt_info 的理解

在求解图优化因子雅克比、残差的过程中，都会乘以一个sqrt_info 矩阵，代码的写法是参考vins，以下参考博客均有对sqrt_info 矩阵的理解

    Eigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_);
    // sqrt_info 为上三角阵
    Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();

1.vins 的margin factor

2.后端优化 | VINS-Mono 论文公式推导与代码解析分讲

综上个人理解：在高斯牛顿法优化中, 非线性优化的式子为 J^T (W^-1) J = -J^T (W^-1) r，其中W为观测的协方差矩阵，为对角矩阵，对角线的每个元素为该残差的方差，即可理解为观测误差(传感器的噪声)。将W求逆即为残差的信息矩阵，可理解为权重矩阵，用于调整不同的残差项的权重(具有归一化的意义) 。因为ceres 只接受 min(e^T e ) 形式的最小二乘优化形式，所以可以通过cholesy分解的方式将残差的信息矩阵(W^-1)分解为上下三角形式，然后分别乘到残差和雅克比上，其形式与原高斯牛顿法的等式等价，相当于在把残差的信息矩阵开根号，然后分别乘到雅克比和残差上。

//  雅克比 乘 上 sqrt(残差信息矩阵)
jacobian_i = sqrt_info * jacobian_i;
//  残差 乘 上 sqrt(残差信息矩阵)
residual = sqrt_info * residual;

右乘BCH及其求逆公式

此处部分为课程中较多争议的一个部分，为扰动模型中，右雅克比在代码上的实现，摘抄自陈皓嘉助教的“第十章思路讲解”。

FILE : factor_prvag_imu_pre_integration factor_prvag_map_matching_pose factor_prvag_marginalization

//   右雅克比的逆   
static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
      Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

      double theta = w.norm();

      if ( theta > 1e-5 ) 
      {
          Eigen::Vector3d a = w.normalized();
          Eigen::Matrix3d a_hat = Sophus::SO3d::hat(a);
          double theta_half = 0.5 * theta ;
          double cot_theta = 1.0 / tan(theta_half);

          J_r_inv = theta_half * cot_theta * J_r_inv  +  (1.0 - 
          theta_half * cot_theta) * a * a.transpose() + 
          theta_half * a_hat ;
      }

      return J_r_inv;
  }

关于是否需要添加帧间里程计因子的讨论

摘自学员解答：

​ 由于我们播的是相同的数据集，点云地图匹配和激光里程计的结果差不多，如果地图变化比较大，激光里程计的存在会使系统更加鲁棒。

5. 课程答疑总结

5.1 进行舒尔补构建b矩阵是，正负号问题

答：公式上是没问题的，区别在于构建残差是是 (预测-观测) or (观测-预测)

5.2 因子图优化中Ti Tj 问题

问：我有个疑问，这里添加的因子是，激光里程计因子，是点云的scan2scan， 作残差时，是这两帧通过点云匹配相对位姿 Tij 和 Ti Tj计算的相对位姿比较，这里没有矛盾么，因为这里的因子是点云匹配出来的，而Ti Tj 一开始也是点云匹配给的，在做残差时，不相当于自己减自己？

Ti Tj 一般是预测值，一般是由scan2scan 上一时刻的后验POSE给的后验，作为预测。残差模型一般都是遵循 (观测 - 预测)模式，scan2map 先验地图匹配一般不作为先验因子，一般作为观测量。

5.3 EKF 与 factor graph 对比

问：因子图优化对比EKF在工程上的优势，因为我在第十章EKF 和 因子图对比中，发现EKF的效果要好点。

答：在课程的效果中，EKF 对比 因子图优化的效果要好，鉴于数据集本身存在分米级的误差，所以不用太在意。实际上，因子图优化开始取代EKF。在建图领域，大部分都使用优化的方法，在定位，目前大多数还是kalman的方法，包括apollo也是沿用kalman的定位方案，但是优化方法也在逐步取代。优化方法体现的优势在于：EKF的方法基于一阶马尔科夫性，只是局部最优，而图优化的方法是基于全局最优，即使是滑窗方法上的优化方法，也是选取多帧最优。

5.4 scan2map中的map

问：因子图优化中的激光里程计约束指的scan2scan 还是 scan2map, 如果是scan2map 的话，那它和“先验地图匹配因子”的区别是在于 “激光里程计因子”的map是局部分割的小地图，“先验地图匹配因子”的map是大地图是吗？ 使用全局的大地图匹配，对资源消耗不是很大吗？

答：激光里程计：前端激光里程计主要还是loam的方案，使用的是scan2map 方式构建，这里的map并不是只小的先验地图，只是指loam里的submap小特征地图。“先验地图匹配因子”里所说的先验地图，使用滑窗分割出的小地图，为了节省运算资源。

5.5 如何去评估地图的质量精度

a.使用更高精度的惯性传感器，进行建图时的轨迹评估；

b.标志物评判，实地选取一个标志物，使用RTK去测量当地标志物的经纬高，对应地图上同一个标志物，计算误差，多取几个，可以构建出误差系数。

5.6 slam中GPS的主要作用

gps的主要作用个人认为主要有两个：1.消除行驶过程中累积误差； 2.室外重定位过程中的初始化有很大作用。

5.7 点云匹配过程中，特征退化的现象(长廊、隧道)，有什么方法可以判断出来？（待补充）

1.特征退化的场景(如长廊、隧道)，因为只有两侧的观测，可以对匹配的协方差矩阵进行特征值分解，通过判断特征值的分布状况判断。

2.也可已增加imu uwb等辅助传感器。

5.8 IMU上电的bias需要怎么估计？(待补充)

kitti数据集中貌似已估

5.9 图优化 因子图 位姿图 概念（待补充）

5.10 SLAM 算法岗位应届生招聘要求

a.slam 的基础知识，前端匹配：icp ndt， 后端优化： 高斯牛顿法、LM优化

b.代码能力(leetcode top 100 、 剑指offer)

c.常见的开源框架， 优点(为什么选择他) ，缺点(如何改进)

d.项目经验(重点)： 为什么这样搞，这样搞的好处、优势。

​ edited by kaho 2022.3.19