GVINS论文精度（已经其相关知识点）

1、贡献

• 初始化GNSS视觉惯性状态的在线粗到精方法
• 基于优化的紧密耦合方法，在概率框架下将视觉惯性数据与多星座GNSS原始测量数据融合；
• 实时估计器，能够在复杂环境中提供无漂移六自由度全局估计，全球导航卫星系统信号可能在很大程度上被截获，甚至无法访问；
• 在仿真和真实环境中对所提出的系统进行广泛评估。

2、注释和定义

2.1 Frames

1. 传感器坐标系：传感器框架以传感器为坐标原点，是一个局部坐标系，传感器在其中输出读数。在我们的系统中，传感器帧是相机帧$(\cdot {)}^c$和IMU帧$(\cdot {)}^i$。我们选择IMU frame作为我们的估计目标frame，并将其表示为body帧$(\cdot {)}^b$。
2. 局部世界坐标系：我们表示视觉惯性系统作为局部世界框架$(\cdot {)}^w$。在VIN系统中，本地世界帧的原点是任意设置的，z轴通常被选择为重力对齐。如图2所示
3. 以地球为中心的地球固定（ECEF）坐标系：ECEF框架$（\cdot {）}^e$是相对于地球固定的笛卡尔坐标系。如图2所示，ECEF框架的原点连接到地球中心，xy平面与地球赤道平面重合，x轴指向本初子午线，z轴被选择为垂直于地球赤道平面，方向为地理北极，最后，采用右手坐标系ECEF坐标系y轴。在本文中，我们使用WGS84表示了ECEF坐标系。
4. ENU坐标系：为了连接局部世界和全局ECEF坐标系，引入了半全局坐标系ENU。ENU框架$（\cdot {）}^n$的x、y和z轴分别指向东、北和向上方向（见图2）。给定ECEF坐标系中的一个点，可以确定唯一的ENU坐标系，其原点位于该点上。请注意，ENU框架的z轴也是重力对齐的。
5. 地心惯性（ECI）坐标系：ECI坐标系是以地球质量中心为原点的惯性坐标系，ECI框架$（\cdot {）}^E$的三个轴指向恒星的固定方向，即不随地球旋转。GNSS信号在ECI坐标系中以直线传播，这可以大大简化公式。在本文中，ECI坐标系是通过在接收GNSS信号时冻结ECEF帧而形成的。

就时间而言，GNSS数据在GNSS时间系统中标记（例如GPS时间），而视觉和惯性测量在当地时间系统中进行标记。我们假设这两个时间系统事先是一致的，并没有相应地加以区分。

2.2 Notation

在本文中，我们使用$R_a^z$和$p_a^z$表示从帧a到帧z的变换的旋转和平移部分。对于旋转部分，还使用了相应的Hamilton四元数$q_a^z$，其中$\otimes$ 表示其乘法运算。我们使用下标表示特定时间实例的移动帧。例如，$R_{a_t}^z$表示在时间t的移动帧a到固定帧z的旋转。
对于常量，我们使用$g^w$表示局部世界框架中的重力矢量。c是真空中的光速，${\omega }_E$是地球的角速度。

2.3 States

待估计的系统状态包括：

1. body坐标系相对于局部世界坐标系的位置$p_b^w$和方位$q_b^w$；
2. 速度$v_b^w$、加速度计偏差$b_a$和陀螺仪偏差$b_w$;
3. 每个特征点逆深度$\rho$；
4. 局部世界帧和ENU帧之间的偏航偏移$\psi$、接收机时钟偏差$\delta t$和接收机时钟漂移率$\delta \dot{t}$。由于我们的系统支持所有四个GNSS系统，即GPS、GLONASS、Galileo和北斗，因此分别估计它们的时钟偏差。注意，每个星座的接收机时钟漂移率是相同的。

我们的系统采用滑动窗口优化方法，窗口内的状态$X$可以概括为:

其中n是窗口大小，m是窗口中特征点的数量。$\delta t$中的四个分量分别对应于接收机相对于GPS、GLONASS、伽利略和北斗的时间的时钟偏差。

3 GNSS 基础

由于我们的系统需要GNSS原始测量处理，因此有必要了解GNSS的背景知识。在本节中，我们首先概述全球导航卫星系统。然后，介绍了两种原始测量，即码伪距和多普勒频移，并对其进行了建模。最后，本节末尾介绍了用于全局定位的单点定位（SPP）算法的原理。

3.1 GNSS 概述

顾名思义，全球导航卫星系统是一个基于卫星的系统，能够提供全球定位服务。目前，全球导航卫星系统有四个独立和全面运行的系统，即GPS、GLONASS、伽利略和北斗。每个全球导航卫星系统中的导航卫星连续广播无线电信号，接收器可以从中唯一地识别卫星并检索导航信息。以GPS L1C信号为例，最终传输的信号由三层组成，如图3所示。导航信息包含轨道参数、时钟误差修正、电离层延迟系数以及其他与卫星状态相关的信息。轨道参数，也称为星历，包含14个变量，用于计算卫星在特定时间的ECEF坐标。卫星的时钟误差被建模为二阶误差多项式，即具有三个参数。每颗卫星都被分配了一个唯一的伪随机噪声（PRN）码，每1ms重复一次。50位/秒的导航信息首先与PRN码进行异或运算，然后用于调制高频载波信号。接收到信号后，接收器通过测量其与设计信号之间的频率差来获得多普勒频移。码伪距测量是从PRN码移位推断出来的，它指示传播时间。最后，通过反向解调过程发现导航消息。

3.2 伪距测量

接收到信号后，从PRN代码移位测量信号的飞行时间（ToF）。通过将ToF与光速相乘，接收器获得前缀为“伪”的伪距测量，因为它不仅包含卫星与接收器之间的几何距离，还包含信号生成、传播和处理过程中出现的各种误差。
卫星侧的误差源主要包括卫星轨道误差和时钟误差；轨道误差来自于其他天体的影响，而这些天体不是由星历表精确建模的，时钟误差是卫星上的原子钟相对于标准系统时间而言不完美的结果。轨道和时钟误差由系统控制段监控并不断修正。在从卫星传播到接收器的过程中，信号经过电离层和对流层，其中电磁信号的速度不再与真空下的速度相同，因此，信号会根据大气成分和传播路径而延迟。信号以不同方式到达接收器的现象称为多径效应，可能会发生并增加额外延迟，尤其是对于低仰角卫星。当信号到达时，ToF是通过比较由卫星原子钟标记的信号传输时间与接收机较不准确的本地时钟时间来计算的。因此，与GNSS系统时间相关的接收机时钟误差也会抵消距离信息。伪距测量可以建模为：

其中$p_s^E$和$p_r^E$分别是卫星$s$和接收器$r$的$ECI$坐标；$\zeta s$设计为4×1指示矢量，对应的卫星星座为1，其他三个实体为0；$\Delta t^s$是卫星的时钟误差，可以从广播导航信息中计算得出；$T_r^s$和$I_r^s$分别代表对流层和电离层延迟。我们用$M_r^s$表示由多径效应和${ϵ}_r^s$表示测量噪声。这里，延迟项$T_r^s$,$I_r^s$r和$M_r^s$用长度单位表示，即乘以c。

3.3 多普勒测量

多普勒频移是根据接收到的载波信号与设计的载波信号之间的差值来测量的，它反映了接收器-卫星沿信号传播路径的相对运动。由于GNSS信号结构的特点，多普勒测量的精度通常比码伪距高一个数量级。多普勒频移建模为:

其中$v_r^E$和$v_s^E$分别表示ECI帧中表示的接收器和卫星的速度。我们用$\lambda$表示载波信号的波长，用${\kappa }_r^s$表示ECI帧中从接收器到卫星的单位矢量。$\Delta {\dot{t}}^s$是导航信息中报告的卫星时钟误差的漂移率，最后，${\eta }_r^s$表示多普勒测量噪声。

3.4 SPP算法

SPP算法利用码伪距测量，通过三边测量确定GNSS接收机的三个DoF全局位置。因此，理论上，接收器的坐标可以借助三颗不同的卫星获得。然而，如第3.2节所述，码伪距测量被接收机时钟偏差偏移。由于接收器的时钟偏差可能会导致数百公里的误差，因此必须连同位置一起估计，才能得到合理的结果。为此，需要至少四个码伪距测量，以完全限制三个DoF全局位置和接收机时钟偏差。由于不同的导航系统使用不同的时间基准，因此不同系统之间存在时钟偏移。如果卫星来自多个星座，则需要进行额外测量，以估计系统间时钟偏移。总之，为了唯一定位接收器，需要同时跟踪至少（N+3）颗卫星，其中N是被跟踪卫星中的星座数。
收集足够的测量值后，将（2）中的约束叠加在一起，用$p_r^E$形成一系列方程$\delta t$未知。修正应用于伪距测量，使其仅为$p_r^E$和$\delta t$的函数。在我们的系统中，对流层延迟$T_r^s$由Saastamoinen模型[23]估算，而电离层延迟$I_r^s$则由Klobuchar模型[24]和星历中的参数计算。通过排除低仰角卫星，我们忽略了多径效应引起的延迟$M_r^s$。在实践中，将使用超过（N+3）个测量值，并通过最小化残差平方和获得解决方案。如[25]所示，SPP解决方案的噪声不仅取决于测量噪声，还与卫星的几何分布有关。图4中简化的二维情况显示了卫星分布对最终解决方案噪声特性的影响。因此，在测量噪声不变的情况下，对于均匀分布的卫星，SPP算法的性能会更好。

4 系统概述

系统结构如图5所示。

估计器将原始GNSS、IMU和相机测量值作为输入，然后对每种类型的测量值进行必要的预处理。如[6]所示，IMU测量值是预积分的生成的，稀疏特征点是从图像序列中检测和跟踪的。对于GNSS原始数据，我们首先过滤掉低海拔和不健康的卫星，这些卫星容易出错。为了拒绝不稳定的卫星信号，只允许连续锁定一定时间的卫星进入系统。由于星历数据是通过慢速卫星接收器无线链路（GPS L1C上为50位/秒）获取的，因此在其对应的星历完全传输之前，GNSS测量无法使用。在预处理阶段之后，所有测量都为估计器做好了准备。在执行优化之前，需要一个初始化阶段来正确初始化非线性估计器的系统状态。
初始化从运动的视觉结构（SfM）开始，根据该结构联合估计最相似的运动和结构。然后，通过将来自IMU的轨迹与SfM结果对齐来执行VI初始化，以恢复刻度、速度、重力和IMU偏差。VI初始化完成后，粗到精的GNSS进行初始化过程,首先，利用SPP算法得到粗定位结果。然后，使用VI初始化和GNSS多普勒测量得到的局部速度，在偏航对准阶段将局部帧和全局帧关联起来。最后，初始化阶段以锚定优化结束，锚定优化利用精确的局部轨迹并施加时钟约束，以进一步优化锚定的全局位置。在初始化阶段之后，将检查并仔细处理GNSS退化情况，以确保稳健的性能。然后，在非线性优化框架下，利用所有测量值的约束，联合估计滑动窗口内的系统状态。请注意，如果GNSS不可用或无法正确初始化，我们的系统自然降级为VIO。为了确保实时性能并处理视觉惯性退化运动，在每次优化后还应用了双向边缘化策略[26]，该策略根据视差测试选择要移除的帧。

5 概率公式

在本节中，我们首先在概率框架下提出并推导状态估计问题。整个问题被表示为一个因子图，传感器的测量值形成一系列因子，这些因子反过来约束系统状态。本节将详细讨论概率图中的每种因素。请注意，视觉和惯性因子的公式是从[6]、[26]和[27]继承而来的，因此不是本工作的贡献。包含相关内容只是为了完成本文。

5.1 最大后验估计

我们将最佳系统状态定义为给定所有测量值的后验概率最大化的状态。假设所有测量值彼此独立，且每个测量值的噪声均为零均值高斯分布，则最大后验概率（MAP）估计问题可以进一步转化为最小化一系列成本的总和，每个成本对应一个特定的测量值

其中z表示n个独立传感器测量值的聚合， { r p ， H p } \{r_p，H_p\} {rp​，Hp​}封装了系统状态的先验信息。$r（\cdot ）$表示每次测量的残差函数，$||.|{|}_P$是马氏规范。
注意，该公式自然适合因子图表示[28]。因此，我们将优化问题分解为与状态和度量相关的单个因素。
图6显示了我们系统的因子图。除了来自测量的因素外，一个先验因素用于约束局部世界框架初始姿势的四个不可观察方向，然后，随着我们边缘化旧框架，它将成为一个紧密连接的先验。在下面，我们将详细讨论每个因素。

5.2 惯性系数测量

惯性系数中涉及的测量值包括偏置、噪声线性加速度和平台角速度。当加速度计在地球表面附近工作时，线性加速度测量也包含重力分量。考虑到低成本IMU的噪声测量，IMU的公式忽略了地球自转产生的科里奥利力和离心力。因此，惯性测量可以建模为:


式中， { a t ，∆ ω t } \{at，∆ωt\} {at，∆ωt}是IMU在时间t的输出，{at，ωt{代表IMU传感器框架中平台的线性加速度和角速度。假设加性噪声na和nw为零均值高斯分布，即na∼ N（0，∑a）和nw∼ N（0，∑w）。与加速计和陀螺仪相关的缓慢变化的偏差被建模为随机行走，如下所示：