总结分析一些GNSS相关开源融合方案
背景介绍：
近年来，自主移动机器人的需求在不断增长，与之相应的落地问题也越来越多，其中准确的状态估计是自主移动机器人的基本问题，为了实现局部准确和全局无漂移的状态估计，可以将具有互补特性的多个传感器融合在一起。 比如本地传感器（相机、IMU、LiDAR，轮子编码器等）能在小区域内提供精确的姿态，但长时间运行累积误差会越来越大，而全局传感器（GPS、磁力计、气压计等）能够在大规模环境中提供有较大噪声但全局无漂移的定位，那么接下来本文将介绍一些能够融合上述的两种数据的传感器融合框架，同时也会分析对比这些方法的优缺点。
首先从融合方法上说有：滤波方法 、优化方法两种。滤波方法的优势就是时间快，空间占用少，因为它只考虑当前时刻和上一时刻的状态。优化方法因为考虑更长时间的状态，所以精度也相对高一些，但是对算力&内存的要求也更多。虽然随着图优化理论和实践的发展，滤波的优势越来越小，跟优化方案比有这样或那样的缺点，但是它仍然是行业在当前发展阶段的一个重要方法。因为在运算资源不是很充足的应用场景下，比如一些嵌入式板子上，优化的方案无法实时运行，还只能是选择滤波的方法来做。
然后从融合方式上有： 紧耦合、松耦合两种。在松耦合中，两个系统各算各的互不影响，比如IMU输出的POSE 和视觉VO算的POSE是独立的，但紧耦合是相互耦合的相互影响的最终只输出一个POSE结果。紧耦合因为从传感器数据源头出发，考虑了更多的因素，精度也会相对好一些，但是缺点就是要了解每个传感器的模型，每一个数据的物理意义等，研发周期长。松耦合就是把两个传感器算出来的位置数据用合适的权重（一般是各种KF）融合起来，相对简单，理解开发起来会快一些。
最后就开始一一介绍下现有的一些GNSS融合相关的开源方案吧。
EKF
介绍：
KF or EKF 是做多传感器融合很常用的思路，在这个方案中，轮子编码器和IMU结合先做一个位姿预测，然后利用GPS数据做一个位姿的更新（修正）。IMU+轮子在递推的过程中，因为会累加过程噪声Q，所以在没有GPS的数据下，定位的协方差会越来越大，当有GPS数据输入的时候，会根据最小化系统协方差的原则，根据当前预测的协方差大小和观测协方差大小来调整两者之间的权重（这其实就是卡尔曼最优估计的过程）。最终会把系统收敛到协方差较小的那一方。
论文地址：（无）
代码地址：https://github.com/karanchawla/GPS_IMU_Kalman_Filter
优点：思路简单直观。
缺点： 1，只考虑了当前状态和上一时刻的状态的信息。
2，旋转的处理需要使用四元数或者旋转矩阵。
3，在工作点附近线性近似会带来误差。
4，EKF由于每次迭代都需要计算雅可比矩阵，计算复杂度比较高。
ESKF
介绍：
在现代的大多数IMU系统中，人们往往使用误差状态卡尔曼滤波器（Error state Kalman filter, ESKF）而非原始状态的卡尔曼滤波器。大部分基于滤波器的LIO或VIO实现（MSCKF）中，都使用ESKF作为状态估计方法。
参考书籍：Quaternion kinematics for the error-state Kalman filter.pdf
代码地址：https://github.com/ydsf16/imu_gps_localization
优点：相比于传统KF，ESKF的优点可以总结如下：
-
在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统KF需要用到四元数（4维）或者更高维的表达（旋转矩阵，9维），要不就得采用带有奇异性的表达方式（欧拉角）。
-
ESKF总是在原点附近，离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题。
-
ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替，计算也将因此变得更快。
-
误差状态的运动学也相比原状态变量要来得更小，因为我们可以把大量更新部分放到原状态变量中。
缺点：只考虑了当前状态和上一时刻的状态的信息。未充分GNSS失效的情况。
LIO-SAM
介绍：
LIO-SAM是一个基于激光雷达，IMU和GPS多种传感器的因子图优化方案，这个框架中包含四种因子，IMU预积分因子，激光里程计因子，GPS因子，闭环因子。从上图中以看到LIO-SAM的代码十分轻量，只有四个cpp文件，所以代码量也较少。论文中写到，当GPS接收可用时不断添加 GPS 因子是不必要的，因为激光雷达惯性里程计的漂移增长非常缓慢。只需要在估计的位置协方差大于接收到的 GPS 位置协方差时才添加 GPS 因子。
论文地址：https://arxiv.org/abs/2007.00258
代码地址：https://github.com/TixiaoShan/LIO-SAM
优点：代码量较小，此方案使用因子图融合了IMU,LIDAR,GPS三种数据，其中IMU和LIDAR是紧耦合，它们推算出来的Odometry和GPS是松耦合。使用了增量平滑和建图方法iSAM2执行因子图优化。iSAM2的优势是只需要设计因子，优化部分ISAM2自动搞定，这也是LIO-SAM代码量少的原因之一。
缺点：GPS信息利用的不是很全（GPS松耦合LIO信息）。
VINS FUSION
介绍：
2019年的 VINS-Fusion 是一个视觉和IMU紧耦合先输出一个Local Odometry（VIO），然后这个Local Odometry再松耦合GPS的图优化方案。在这一点上和LIO-SAM有些相似（都是松耦合GNSS）。VINS-Fusion在有GPS数据进来的时候，就对系统做一次全局优化，如果没有GPS信息的话就是VIO一直递推位姿，在代码中构建了VIO残差项和GPS残差项，使用了ceres库来求解非线性优化问题。
论文地址：https://arxiv.org/abs/1901.03642
代码地址：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
优点：GPS相关融合代码相对简洁易懂，不用像滤波方法那样考虑怎么预测和更新，构建好残差方程放在一起优化即可。
缺点：仅仅使用了GPS自带的协方差数据，但没有充分考虑到运动低速、卫星数少于4个等GNSS退化的情况。
GVINS
介绍：
GVINS是一个紧耦合了IMU、视觉、GNSS三种传感器的非线性优化系统。首先系统对所有传感器的测量值进行预处理。 在初始化阶段，通过将惯性信息与仅视觉 SfM 的结果对齐来完成视觉惯性初始化。 如果视觉惯性成功对齐，则执行从粗到细的过程以初始化 GNSS 状态。 一旦涉及到 GNSS 状态，系统就会监控和处理 GNSS 退化案例。 最后，通过非线性优化优化了滑动窗口内所有测量的约束。 如果 GNSS 状态无法初始化，系统仍然可以在视觉惯性模式下工作。 此外，该还采用边缘化策略来保证实时估计。
论文地址：https://arxiv.org/abs/2103.07899
代码地址：https://github.com/HKUST-Aerial-Robotics/GVINS
优点：前面介绍的方案，都是松耦合了GPS数据，但GVINS则紧耦合了GPS原始数据，比如GNSS时间、伪距和多普勒频移测量等，也考虑了GNSS不工作的情况(接受的卫星数小于4)，并在因子图框架中约束系统状态。效果是目前这几种方案里面最好的。
缺点：理论和模型比较复杂，计算量相对更大，研发周期长。
总结
在上文介绍的方法中，除了GVINS是紧耦合GNSS信息外，其他的方案都是松耦合GNSS信息。然后除了EFK,ESKF的方案，其他方案都使用了图优化来进行位姿更新。所以从精度上（也包括了系统鲁棒性）来说GVINS的精度最高，LIO-SAM次之（主要是雷达精度高），VINS-FUSION 略大于ESKF 大于 EKF 。如果从研发周期、系统的运行效率（时间、空间）来说，ESKF是最优的，EKF次之，然后是VINS FUSION 约等于 LIO-SAM 大于 GVINS。
除了传感器融合的方法方式的问题之外，我们还需要考虑到一些会影响系统稳定性的工程问题。
1，系统时间对齐（Time Synchronization）
根据对系统性能的影响，GNSS与本地惯性测量的对齐可以分为三个级别，如下图所示。理想情况下，来自不同传感器的测量在统一的时间系统中标记并由硬件触发（比如通过脉冲信号）。 如果在不同时间捕获测量但仍在同一时间系统下，情况会变得更糟，因此必须应用插值来正确融合不同时间戳的测量。 同步的最坏情况是两个时间系统没有对齐，因此 GNSS 和视觉惯性测量之间的时间关系是完全未知的。 在这种情况下，估计结果是不可靠的，系统行为是不可预测的。
2，电磁干扰（Electromagnetic Interference）
接收机天线感知的 GNSS 信号功率非常低，可能会完全淹没在热噪声中。因此，接收机的性能可能会受到附近 RF 源的影响。尽管特定频段已专门用于 GNSS 使用，但许多设备仍会无意中向GNSS 频段发射强大的射频功率。比如许多现代数字设备，如计算机或相机，往往会产生一个宽频谱，而该频谱又与 GNSS 频段重叠。不幸的是，此类干扰射频信号的功率电平通常远大于近地表 GNSS 信号的功率水平，因此必须认真考虑电磁干扰 (EMI)。一些外围设备的 I/O 电缆在传输信号过程中可以严重降低接收机的性能，所以通过适当的接地设计和电缆和连接器的屏蔽，可以在一定程度上减轻干扰。