视觉惯性里程计（VIO）众所周知在长期的运行中会有累计误差。在本文中提出了GVINS，一个基于非线性优化的系统，它将GNSS原始测量值与视觉和惯导信息紧密地融合起来，用于实时和无漂移的状态估计。本文的系统的目标是在复杂的室内外环境下提供精确的全局6自由度姿态估计，在这种环境下，GNSS信号可能被大量丢失甚至完全不可用。为了将全局测量与局部状态联合起来，本文提出了一种由粗到精的初始化方法，可以有效地在线标定变换，并在很短的测量滑动窗口内对GNSS状态进行初始化。然后在因子图框架下，结合视觉和惯导约束，对GNSS伪距和多普勒频移测量进行建模和优化。对于复杂和GNSS不友好的区域，对退化场景进行了讨论和处理，以保证里程计的鲁棒性。该系统所涉及的工程挑战也包括在内，以便于相关的GNSS融合研究。由于采用了紧耦合的多传感器方法和系统设计，我们的系统充分利用了三种传感器的优点，能够无缝地应对室内和室外环境之间的过渡，即便在卫星丢失和重新捕获的情况下。我们通过仿真和实际的实验对所提出的系统进行了广泛的评估，结果表明，尽管GNSS测量有噪声，我们的系统仍然有效地消除了VIO的漂移，并保持了系统的局部精度。此外，实验还表明，我们的系统甚至可以从一颗卫星获得增益，而传统的GNSS算法至少需要四颗卫星。
坐标系与系统状态
-
传感器坐标系：相机坐标系\((.)^c\)，IMU坐标系\((.)^i\)以及机体坐标系\((.)^b\)。
-
局部世界坐标系local world frame：VIO系统的坐标系\((.)^w\)。
-
地心地固坐标系Earth-centered, Earth-fixed frame(ECEF)：是一个固定的笛卡尔坐标系。是相对于地球而言的\((.)^e\)。
-
ENU坐标系：俗称东北天坐标系，连接了local world和全局ECEF系\((.)^n\)。
\[
\begin{array}{l}\mathcal{X}=[\textbf{x}_0,\textbf{x}_1,\cdots\textbf{x}_n,\rho_0,\rho_1,\cdots\rho_m,\psi]\\ \textbf{x}_k=\left[\textbf{p}_{b_{t_k}}^{w},\textbf{v}_{b_{t_k}}^{w},\textbf{q}_{b_{t_k}}^{w},\textbf{b}_{a},\textbf{b}_{w},\delta\textbf{t},\dot{\delta}t\right],k\in[0,n]\\ \delta\textbf{t}=[\delta t_G,\delta t_R,\delta t_E,\delta t_C],\end{array}
\]
1.\(\psi\): local world和ENU系之间的偏航角。如图，local world系和ENU系的Z轴是重合的，和重力对齐。所以local world和ENU之间只需要一个yaw角就可以对齐。
2.\(\delta\textbf{t}\): 接收机时钟钟差。因为GVINS支持四个导航系统——GPS、GLONASS、Galileo和北斗卫星，不同的导航系统相应的接收机钟差是不同的。
3.\(\dot{\delta}t\): 接收机时钟钟差变化率，对于每个卫星系统是相同的。
GNSS介绍
全球导航卫星系统（GNSS），顾名思义，是一种基于卫星的系统，能够提供全球定位服务。目前有四个独立且全面运行的系统，即GPS、GLONASS、伽利略和北斗。每个GNSS系统由控制段、卫星段和用户段组成，用户段由无限数量的接收机组成。
卫星段是在约20,000公里高度绕地球轨道运行的卫星星座（北斗的GSO/IGSO卫星除外），其状态由控制段监测和更新。导航卫星不断发射特定的无线电信号，接收器可以从中唯一地识别卫星并检索导航信息。 GPS L1 信号的典型结构如图 3 所示。每颗卫星都有一个唯一的伪随机数 (PRN) 码，每 1 毫秒重复一次。包含卫星轨道和GNSS时间参数的导航电文（星历）首先与PRN码组合，然后用于调制高频载波信号。接收器接收到信号后，通过测量接收信号和设计信号之间的频率差来获得多普勒频移，伪距测量是从表示传播时间的 PRN 码位移推断出来的。最后，通过反向解调过程发现导航消息。
因子图优化
Code Pseudorange Factor
接收到信号后，信号的飞行时间 (ToF) 是根据 PRN 码移位来测量的。通过乘以光速，接收机获得伪距测量。伪距之所以称为“伪”，是因为它不仅包含卫星与接收机之间的几何距离，还包括信号产生、传播和处理过程中的各种误差。
卫星侧误差源主要包括卫星轨道误差和时钟误差。轨道误差来自其他天体的影响，这些天体不是由星历精确建模的，时钟误差是卫星星载原子钟相对于标准系统时间不完善的结果。轨道和时钟误差由系统控制部分监控并不断纠正。
信号从卫星到接收器的传播过程中，经过电离层和对流层，电磁信号的速度不再与真空中的速度相同，并且信号根据大气成分和传播路径而延迟。信号以不同方式到达接收器的现象称为多径效应，可能会发生并增加额外的延迟，特别是对于低仰角卫星。当信号到达时，ToF 是通过将信号传输时间（由卫星的原子钟标记）与接收器不太准确的本地时钟时间进行比较来计算的。因此，距离信息也被接收器时钟偏差相对于 GNSS 系统时间偏移。总之，伪距测量可以建模为：
\[
\tilde{P}_r^s=\|\textbf{p}_s^E-\textbf{p}_r^E\|+c\left(\boldsymbol{\zeta}_s^T\delta\textbf{t}-\Delta t^s\right) +T_r^s+I_r^s+M_r^s+\epsilon_r^s
\]
其中\(\textbf{p}_s^E,\textbf{p}_r^E\)为卫星和接收机在ECEF系下的坐标，\(c\)为光速；\({\boldsymbol{\zeta}_s^T}_{4 \times 1}\)是一个向量，用1记录了当前导航系统的是否使用；\(\Delta t^s\)是卫星时钟误差，可以从星历中获取；\(T_r^s,I_r^s\)为电离层和对流层造成的测量延时；\(M_r^s\)为多路效应multipath effect造成的延时；\(\epsilon_r^s\)是测量噪声。
受地球自身运动的影响，所以需要考虑Sagnac effect\(\\S_{r}^{s}\):
\[
\\S_{r}^{s}=\frac{ \omega_{E}}{ c\left(\left[\mathbf{p}_{s}^{e}\right]_{x}\left[\mathbf{p}_{r}^{e}\right]_{y}-\left[\mathbf{p}_{s}^{e}\right]_{y}\left[\mathbf{p}_{r}^{e}\right]_{x}\right)}
\]
其中\(\omega_{E}\)为地球自转角速度。
伪距测量的噪声模型符合均值为0的高斯分布\(\epsilon_{r}^{s} \sim N\left(0, \sigma_{r, \mathrm{pr}}^{s}\right)\)建模为：
\[
\sigma_{r, \mathrm{pr}}^{s}=\frac{n_{s} \times n_{\mathrm{pr}}}{\sin ^{2} \theta_{\mathrm{el}}}
\]
其中\(n_s\)是广播卫星空间精度指标;\(n_{pr}\)接收机上报的伪码测量噪声指标;\(\theta_{\mathrm{el}}\)卫星在接收机视野中的仰角。
ENU系到ECEF系的转换:
\[
\mathbf{R}_{n}^{e}=\left[\begin{array}{ccc} -\sin \lambda & -\sin \phi \cos \lambda & \cos \phi \cos \lambda \\ \cos \lambda & -\sin \phi \sin \lambda & \cos \phi \sin \lambda \\ 0 & \cos \phi & \sin \phi \end{array}\right]
\]
其中\(\lambda\)是纬度，\(\phi\)是经度。
local world系和ENU系只差一个偏航角\(\psi\), 转换关系为：
\[
\mathbf{R}_{n}^{w}=\left[\begin{array}{ccc} \cos \psi & -\sin \psi & 0 \\ \sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right]
\]
因此接收机在local world下的坐标和ECEF下的坐标关系转换为：
\[
\mathbf{p}_{r}^{e}=\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}\left(\mathbf{p}_{r}^{w}-\mathbf{p}_{\mathrm{anc}}^{w}\right)+\mathbf{p}_{\mathrm{anc}}^{e}
\]
因为ENU的中心和local world的中心重合:
\[
\mathbf{p}_{\mathrm{anc}}=0
\]
并且：
\[
\mathbf{p}_{r}^{w}=\mathbf{p}_{b}^{w}+\mathbf{R}_{b}^{w} \mathbf{p}_{r}^{b}\approx\mathbf{p}_{b}^{w}
\]
因此：
\[
\mathbf{p}_{r}^{e}-\mathbf{p}_{\mathrm{anc}}^{e}=\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}\left(\mathbf{p}_{r}^{w}-\mathbf{p}_{\mathrm{anc}}^{w}\right)
\]
因此误差函数可以建模为：
\[
r_{\mathcal{P}}(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}},\mathcal{X})=\|\mathbf{R}_{z}(\omega_{E}t_{f})\mathbf{p}_{s}^{e^{'}}-\mathbf{p}_{r_{k}}^{E}\|+c(\zeta_{s_{j}}^{T}\delta\mathbf{t}_{k}-\Delta t^{s_{j}})+ T_{r_{k}}^{s_{j}}+I_{r_{k}}^{s_{j}}-\tilde{P}_{r_{k}}^{s_{j}}
\]
伪距测量误差对\(\mathbf{P}^w_{b_{tk}}\)的导数：
设:
\[
{I}=\mathbf{R}_{z}\left(-\omega_{e} t_{f}\right) \mathbf{p}_{s_{j}}^{e^{\prime}}-\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}\left(\mathbf{p}_{b}^{w}\right)-\mathbf{p}_{\mathrm{anc}}^{e}
\]
则：
\[
\frac{\partial{r_{\mathcal{P}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial \mathbf{p}_{b_{t_{k}}}^{w}}=
\frac{\partial{(I^T I)^{\frac{1}{2}}}}{\partial \mathbf{p}_{b_{t_{k}}}^{w}}=
\frac{1}{2}(I^T I)^{-\frac{1}{2}} \cdot \frac{\partial{(I^T I)}}{\partial \mathbf{p}_{b_{t_{k}}}^{w}}=\frac{1}{2}(I^T I)^{-\frac{1}{2}} \cdot 2 I^T \cdot(-\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n})=-(\frac{I}{|I|})^T\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}
\]
伪距测量误差对\(\psi\)的导数
\[
\frac{\partial{r_{\mathcal{P}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial\psi}=\frac{\partial{({I^T I})^{\frac{1}{2}}}}{\partial \psi}=\frac{1}{2}(I^T I)^{-\frac{1}{2}} \cdot \frac{\partial{(I^T I)}}{\partial \psi}=-(\frac{I}{|I|})^{T}\mathbf{R}_{n}^{e} \dot{\mathbf{R}_{w}^{n}}\mathbf{p}_{b_{k}}^{w}
\]
\[
\dot{\mathbf{R}_{n}^{w}}=\left[\begin{array}{ccc} -\sin\psi & -\cos \psi & 0 \\ \cos \psi & -\sin \psi & 0 \\ 0 & 0 & 0 \end{array}\right]
\]
伪距测量误差对\(\delta \mathbf{t}_k\)的导数:
\[
\frac{\partial{r_{\mathcal{P}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial\delta \mathbf{t}_{k}}=c\boldsymbol{\zeta}_{s_{j}}
\]
Doppler Factor
多普勒频移是由接收到的载波信号与设计载波信号的差值来测量的，反映了接收机-卫星在信号传播路径上的相对运动。由于GNSS信号结构的特点，多普勒测量精度通常比伪距测量精度高一个数量级。多普勒频移模型为:
\[
\Delta\tilde{f}_r^s=-\dfrac{1}{\lambda}\left[\kappa_r^{sT}(\mathbf{v}_s^E-\mathbf{v}_r^E)+c(\dot{\delta}t-\dot{\Delta}t^s)\right]+\eta_r^s
\]
其中\(\mathbf{v}_s^E, \mathbf{v}_r^E\)是卫星和接收机在ECEF系下的速度;\(\lambda\)是载波信号的波长;\(\kappa_r^{s}\)是在ECEF下接收器到卫星的单位观测向量:
\[
\kappa_{r}^{s}=\frac{\mathbf{p}_{s}^{e}-\mathbf{p}_{r}^{e}}{|\mathbf{p}_{s}^{e}-\mathbf{p}_{r}^{e}|}=\frac{\mathbf{p}_{s}^{e}-\mathbf{p}_{r}^{e}}{\sqrt{(\mathbf{p}_{s}^{e}-\mathbf{p}_{r}^{e})^{2}}}
\]
其中，\eta_r^s是多普勒频移噪声，服从均值为0的高斯分布，其均方差建模为：
\[
\sigma_{r, \mathrm{dp}}^{s}=\frac{n_{s} \times n_{\mathrm{dp}}}{\sin ^{2} \theta_{\mathrm{el}}}
\]
其中，\(n_{\mathrm{dp}}\)为接收机的测量噪声.
接收机在ECEF系下的速度和在local world中的速度之间的转换为：
\[
\mathbf{v}_{r}^{e}=\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n} \mathbf{v}_{b}^{w}
\]
因此误差函数可以建模为：
\[
r_{\mathcal{D}}(\tilde{\textbf{z}}_{r_k}^{s_j},\mathcal{X}) = \frac{1}{\lambda} {\boldsymbol{\kappa}_{r_k}^{s_j}}^T (\textbf{v}_{s_j}^{E}-\textbf{v}_{r_k}^{E}) + \frac{c}{\lambda}(\dot{\delta}t_k-\Delta\dot{t}^{s_j})+\Delta\tilde{f}_{r_k}^{s_j}
\]
多普勒频移误差对\(\mathbf{P^w_{b_{tk}}}\)的导数:
\(\kappa_{r}^{s}\)是在ECEF下接收器到卫星的单位观测向量，而\(\mathbf{p}_{b_{t_{k}}}^{w}\)在local world系下，所以用\(\mathbf{p}_{r}^{e}\)做中间桥梁,由链式法则：
\[
\frac{\partial{r_{\mathcal{D}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial{\mathbf{p}_{b_{t_{k}}}^{w}}}=\frac{\partial{r_{\mathcal{D}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial{\mathbf{p}_{r}^{e}}} \cdot \frac{\partial{\mathbf{p}_{r}^{e}}}{\partial{\mathbf{p}_{b_{t_{k}}}^{w}}}
\]
\[
\frac{\partial{\mathbf{p}_{r}^{e}}}{\partial{\mathbf{p}_{b_{t_{k}}}^{w}}}=\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}
\]
3X1）对（3X1）的导数，结果是一个3X3的矩阵。
\[
\left(\frac{\partial \kappa}{\partial \mathbf{p}_{r}^{e}}\right)_{3 \times 3}=\left[\begin{array}{lll} \frac{\partial \mathbf{\kappa}_x}{\partial\left(p_{r}^{e}\right)_{x}} & \frac{\partial \kappa_x}{\left.\partial p_{r}^{e}\right)_{y}} & \frac{\partial \kappa_x}{\partial\left(p_{r}^{e}\right)_{z}} \\ \frac{\partial \kappa_y}{\partial\left(p_{r}^{e}\right)_{x}} & \frac{\partial \kappa_y}{\partial\left(p_{r}^{e}\right)_ y} & \frac{\partial \kappa_y}{\partial\left(p_{r}^{e}\right)_{z}} \\ \frac{\partial \kappa_z}{\partial\left(p_{r}^{e}\right)_{x}} & \frac{\partial \kappa_z}{\partial\left(p_{r}^{e}\right)_{y} } & \frac{\partial \kappa_z}{\left.\partial p_{r}^{e}\right)_{z}} \end{array}\right]
\]
其中：
\[
\begin{array}{l} \begin{aligned} \frac{\partial \mathbf{\kappa}_x}{\partial\left(p_{r}^{e}\right)_{x}}&=\frac{\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}(\frac{\partial{\left(p_{s}^{e}-p_{r}^{e}\right)_x}}{\partial{({p_r^{e}})_x}})-\left(p_{s}^{e}-p_{r}^{e}\right)_{x}\frac{\partial{\sqrt{(p_s^e-p_r^e)^2}}}{\partial{(p_r^e)_x}}}{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}\\ &=\frac{\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}(-1)-\left(p_{s}^{e}-p_{r}^{e}\right)_{x} \frac{1}{2} \cdot \frac{1}{\sqrt{\left(p_{s}^{2}-p_{1}^{P}\right)^{2}}} \cdot 2\left(p_{s}^{e}-p_{r}^{e}\right)_{x} \times(-1)}{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}\\ &=\frac{-\left(p_{s}^{e}-p_{r}^{e}\right)^{2}+\left(p_{s}^{e}-p_{r}^{e}\right)_{x}\left(p_{s}^{e}-p_{r}^{e}\right)_{x}}{\left(\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}\right)^{3}} \end{aligned} \end{array}
\]
\[
\begin{array}{l} \begin{aligned} \frac{\partial \mathbf{\kappa}_x}{\partial\left(p_{r}^{e}\right)_{y}}&=\frac{\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}(\frac{\partial{\left(p_{s}^{e}-p_{r}^{e}\right)_x}}{\partial{({p_r^{e}})_y}})-\left(p_{s}^{e}-p_{r}^{e}\right)_{x}\frac{\partial{\sqrt{(p_s^e-p_r^e)^2}}}{\partial{(p_r^e)_y}}}{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}\\ &=\frac{0+\frac{1}{\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}}\left(p_{s}^{e}-p_{r}^{e}\right)_{x} \cdot\left(p_{s}^{e}-p_{r}^{e}\right)_y}{\left(p_{s}^{e}-p_{r}^{2}\right)^{2}} \\ &=\frac{\left(p_{s}^{e}-p_{r}^{e}\right)_{x} \cdot\left(p_{s}^{e}-p_{r}^{e}\right)_{y}}{\left(\sqrt{\left(p_{s}^{e}-p_{r}^{e}\right)^{2}}\right)^{3}} \end{aligned} \end{array}
\]
多普勒频移误差对\(\mathbf{V^w_{b_{tk}}}\)的导数:
\[
\frac{\partial{r_{\mathcal{D}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial{\mathbf{v}_{b_{t_{k}}}^{w}}}=-\frac{1}{\lambda} \boldsymbol{\kappa}_{r_{k}}^{s_{j} T}\mathbf{R}_{n}^{e} \mathbf{R}_{w}^{n}
\]
多普勒频移误差对\(\psi\)的导数:
\[
\frac{\partial{r_{\mathcal{D}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial{\psi}}=-\frac{1}{\lambda} \boldsymbol{\kappa}_{r_{k}}^{s_{j} T}\mathbf{R}_{n}^{e} \dot{\mathbf{R}_{w}^{n}}\mathbf{v}_{b_{k}}^{w}
\]
多普勒频移误差对\(\dot{\delta t_k}\)的导数:
\[
\frac{\partial{r_{\mathcal{D}}\left(\tilde{\mathbf{z}}_{r_{k}}^{s_{j}}, \mathcal{X}\right)}}{\partial{\dot{\delta t_k}}}=\frac{c}{\lambda}
\]
Receiver clock factors
在\(t_k, t_{k+1}\)时刻，接收机时钟钟差的变化为：
\[
\delta \mathbf{t}_{k}=\delta \mathbf{t}_{k-1}+\mathbf{1}_{4 \times 1} \int_{t_{k-1}}^{t_{k}} \dot{\delta} t d t
\]
在离散情况下，接收机钟差误差可以建模为：
\[
\mathbf{r}_{\mathcal{T}}\left(\tilde{\mathbf{z}}_{k-1}^{k}, \mathcal{X}\right)=\delta \mathbf{t}_{k}-\delta \mathbf{t}_{k-1}-\mathbf{1}_{4 \times 1} \dot{\delta} t_{k-1} \tau_{k-1}^{k}
\]
接收机时钟钟差变化率可以建模为:
\[
r_{\mathcal{W}}\left(\tilde{\mathbf{z}}_{k-1}^{k}, \mathcal{X}\right)=\dot{\delta} t_{k}-\dot{\delta} t_{k-1}
\]
初始化
初始化分为两个部分，第一部分是visual imu对齐，采用松融合（loosely coupled）方法求解绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v。并没有对加速度计的偏置进行校正，这是因为重力是初始化过程中待求的量，而加速度计偏置与重力耦合，而且系统的加速度相对于重力加速度很小，所以加速度计偏置在初始化过程中很难观测，因此初始化过程中不考虑加速度计偏置的校正。
第二部分是GNSS VI对齐。
visual SFM
检查最新帧与之前所有帧之间的特征对应。如果我们能在滑动窗口中的最新帧和任何其他帧之间，找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(超过20个的旋转补偿像素)，使用五点法恢复这两个帧之间的相对旋转和尺度平移。否则，将最新的帧保存在窗口中，并等待新的帧。如果五点算法成功的话，任意设置尺度，并对这两个帧中观察到的所有特征进行三角化。基于这些三角特征，采用PnP来估计窗口中所有其他帧的姿态。最后，应用全局光束平差法(BA)最小化所有特征观测的重投影误差。由于我们还没有任何世界坐标系的知识，我们将第一个相机坐标系\(c_0\)设置为SfM的参考坐标系。所有帧的位姿\((\bar p^{c0}_{c_k}，q^{c0}_{c_k})\)和特征位置表示相对于\(c_0\).假设摄像机和IMU之间有一个粗略测量的外部参数\((p^b_c,q^b_c)\)，我们可以将姿态从相机坐标系转换到物体(IMU)坐标系。
纯视觉初始化时，我们采用第一帧\(c_0\)作为基准坐标系，若要转化为从 body 坐标系到\(c_0\)坐标系，可以进行如下变换，其中s是匹配视觉结构与距离尺度的尺度参数，解出尺度参数是实现成功初始化的关键。
\[
\mathbf{q}_{b_k}^{c_0}=\mathbf{q}_{c_k}^{c_0}\otimes(\mathbf{q}_{c}^{b})^{-1}\\ s\mathbf{\bar{p}}_{b_k}^{c_0}=s\mathbf{\bar{p}}_{c_k}^{c_0}-\mathbf{R}_{b_k}^{c_0}\mathbf{p}_{c}^{b}
\]
Visual-Inertial Alignment
陀螺仪bias：
考虑滑动窗口中连续两帧\(b_k\)和\(b_{k+1}\)，我们从视觉sfM中得到旋转\(q^{c0}_{b_k}\)和\(q^{c0}_{b_{k+1}}\)，从IMU预积分得到的相对约束\(γ^{b_k}_{b_{k+1}}\)。陀螺仪的误差有两部分测量噪声和陀螺仪偏置，噪声暂时可以忽略（毕竟太小），而视觉的误差就只有观测噪声（也可以忽略不管），因此两者差值的绝对值就是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型：
\[
\begin{array}{l}
\min _{\delta b_{w}} \sum_{k \in \mathcal{B}}\left\|\mathbf{q}_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes \mathbf{q}_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}\right\|^{2} \\
\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \mathbf{J}_{b_{w}}^{\gamma} \delta \mathbf{b}_{w}
\end{array}\right],
\end{array}
\]
第二个式子给出了\(γ^{b_k}_{b_{k+1}}\)对bg的一阶近似。
因为四元数最小值为单位四元数 [1; 0v]T，所以:
\[
\begin{align}
&q_{b_{k+1}}^{c_0}{}^{-1}\otimes q_{b_k}^{c_0}\otimes\gamma_{b_{k+1}}^{b_k}=\begin{bmatrix}1\\ 0\end{bmatrix}\\
&\hat{\gamma}_{b_{k+1}}^{b_k}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}J_{b_\omega}^{\gamma}\delta b_{\omega}\end{bmatrix}=q_{b_k}^{c_0}{}^{-1}\otimes q_{b_{k+1}}^{c_0}\otimes\left[\begin{smallmatrix}1\\ 0\end{smallmatrix}\right] \\
&\begin{bmatrix}1\\ \frac{1}{2}J^{\gamma}_{b_{\omega}}\delta b_{\omega}\end{bmatrix}={\hat{\gamma}^{b_{k}}_{b_{k+1}}}^{-1}\otimes {q^{c_{0}}_{b_{k}}}^{-1}\otimes q^{c_{0}}_{b_{k+1}}\otimes\begin{bmatrix}1\\ 0\end{bmatrix}
\end{align}
\]
只考虑虚部，得：
\[
J_{b_\omega}^{\gamma}\delta b_\omega=2\left(\widehat{\gamma}_{b_{k+1}}^{b_k}{}^{-1}\otimes q_{b_k}^{c_0}{}^{-1}\otimes q_{b_{k+1}}^{c_0}\right)_{vec}
\]
使用SVD分解等方法求解，得到了陀螺仪偏置\(b_w\)的初始校准。然后我们用新的陀螺仪偏置重新传递所有的IMU预积分项\(\hat{\alpha}_{\mathrm{b}_{k+1}}^{\mathrm{b}_{k}}, \hat{\beta}_{\mathrm{b}_{k+1}}^{\mathrm{b}_{k}}, \hat{\gamma}_{\mathrm{b}_{k+1}}^{\mathrm{b}_{k}}\).
速度，重力，尺度：
求解变量如下：
\[
\quad X_I=\left[\mathbf{v}_{b_0}^{b_0},\mathbf{v}_{b_1}^{b_1},\cdots\mathbf{v}_{b_n}^{b_n},\mathbf{g}^{c_0},s\right]
\]
分别为第k帧图像本体坐标系的速度，\(c_0\)坐标系中的重力向量,单目SfM到公制单位的尺度。
在\(c_0\)坐标系下预积分：
\[
\begin{align}
&\alpha_{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(p_{b_{k+1}}^{c_0}-p_{b_k}^{c_0}-\nu_{b_k}^{c_0}\Delta t_k+\frac{1}{2}g^{c_0}\Delta t_k^{2}) \\
&\beta_{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(\nu_{b_{k+1}}^{c_0}-\nu_{b_k}^{c_0}+g^{c_0}\Delta t_k)
\end{align}
\]
其中, 可由视觉SFM获得：
\[
\begin{array}{c}p_{b_k}^{c_{0}}=s\overline{p}_{b_k}^{c_{0}}\\ p_{b_{k+1}}^{c_{0}}=s\overline{p}_{b_{k+1}}^{c_{0}}\end{array}
\]
得：
\[
\alpha_{b_{k+1}}^{b_k}=R_{c_0}^{b_k}\left(s(\bar{p}_{b_{k+1}}^{c_0}-\bar{p}_{b_k}^{c_0}\right)-v_{b_k}^{c_0}\Delta t_k+\frac{1}{2}g^{c_0}\Delta t_k^2)
\]
将等式中速度都转换到\(c_0\)坐标系下：
\[
\begin{align}
&\alpha_{b_{k+1}}^{b_k}=R_{c_{0}}^{b_k}(s(\overline{p}_{b_{k+1}}^{c_{0}}-\overline{p}_{b_{k}}^{c_{0}})-R_{b_{k}}^{c_{0}}v_{b_{k}}^{b_{k}}\Delta t_{k}+\frac{1}{2}g^{c_{0}}\Delta t_{k}^{2}) \\
&\beta_{b_{k+1}}^{b_k}=R_{c_{0}}^{b_k}\left(R_{b_{k+1}}^{c_{0}}v_{b_{k+1}}^{b_{k+1}}-R_{b_{k}}^{c_{0}}v_{b_{k}}^{b_k}+g^{c_{0}}\Delta t_{k}\right)
\end{align}
\]
带入\(s\bar p^{c_0}_{b_k} = p^{c_0}_{c_k}-R^{c_0}_{c_k}p_c^b\), 并转化为\(Hx = b\)形式。得：
\[
\begin{bmatrix}
-I\Delta t_{k}&0&\frac{1}{2}R_{c_0}^{b_k}\Delta t_{k}^{2}&R_{c_0}^{b_k}(\vec{p}_{c_{k+1}}^{c_0}-\vec{p}_{c_{k}}^{c_0}) \\
-I&R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}&R_{c_{0}}^{b_k}\Delta t_{k}&0
\end{bmatrix}_{6\times10}
\begin{bmatrix}
v_{b_k}^{b_k} \\v_{b_{k+1}}^{b_{k+1}} \\ g^{c_0} \\ s
\end{bmatrix}_{10\times 1}
=
\begin{bmatrix}
\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}}R_{b_{k+1}}^{c_{0}}p_{c}^{b}-p_{c}^{b} \\ \beta_{b_{k+1}}^{b_k}
\end{bmatrix}_{6\times1}
\]
H矩阵一定是一个正定对称矩阵，以采用快速的Cholosky分解求解。
修正重力矢量
这里计算的重力吸收了重力加速度计的偏置，虽然不需要计算重力加速度计的偏置，但重力还是需要优化的，说到优化重力加速度，肯定包含两个量，大小和方向，也就是三个维度，但是一般来说大小是确定已知的（这里设为9.8），因此其实我们要做的就是优化方向，是一个两维的向量，下图是优化重力的方法以及b1,b2单位向量的方向确定模型。
对重力向量参数化：
\[
\hat{\mathrm g}^{3\times1}=\|\mathrm g\|\cdot\bar{\hat g}^{3\times1}+\omega_1\vec{b}_1^{3\times1}+\omega_2\vec{b}_2^{3\times1}=\|\mathrm g\|\cdot\bar{\hat g}^{3\times1}+\vec{b}^{3\times2}\omega^{2\times1}
\]
其中，\(g\)是已知重力大小，\(\bar{\hat g}\)是重力方向的单位向量，\(b_1, b_2\)为重力向量正切空间的一对正交基，\(w_1, w_2\)分别是在正交基上的位移。
整理得：
\[
\begin{bmatrix}
-I\Delta t_k & 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2\overline{b} &R_{c_0}^{b_k}(\overline{p}_{c_{k+1}}^{c_0}-\overline{p}_{c_k}^{c_0})\\
-I&R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}&R_{c_0}^{b_k}\Delta t_k\overline{b} & 0
\end{bmatrix}_{6\times9}
\left[\begin{matrix}{v_{b_{k}}^{b_{k}}}\\ {v_{b_{k+1}}^{b_{k+1}}}\\ {\omega}\\ {s}\end{matrix}\right]_{9\times 1}
=
\begin{bmatrix}\alpha_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b-\frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2\|g\|\cdot\bar{\hat{g}}\\ \beta_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}\Delta t_k\|\text{g}\|\cdot\bar{\hat{g}}\end{bmatrix}_{6\times 1}
\]
采用Cholosky分解求解。
通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系\(c_0\)之间的旋转\(q^w_{c_0}\),然后将所有变量从参考坐标系旋转到世界坐标系。本体坐标系的速度也将被旋转到世界坐标系。视觉SfM的变换矩阵将被缩放到度量单位。
GNSS VI Alignment
gnss vi对齐分为三步：
1. 锚点粗定位，使用gnss 伪距测量 spp算法
2. yaw offset校准，使用多普勒测量对齐 enu坐标系和局部世界系。
3. 锚点修正。
yaw offset calibration
\[
\underset{\delta t,\psi}{\text{minimize}}\sum\limits_{k=1}^{n}\sum\limits_{j=1}^{p_k}\left\|r_\mathcal{D}(\tilde{\mathbf{z}}_{r_k}^{s_j},\mathcal{X})\right\|_{\sigma_{r_k},dp}^{2}
\]
其中\(n\)为滑动窗口大小，\(p_k\)为第k次观测到的卫星数量，固定VIO速度\(\mathbf{v}_b^w\),假定窗口内接受机时钟漂移率\(\dot{\delta t_k}\)不变。
anchor point refinement
\[
\underset{\delta\mathbf{L},\mathbf{p}_{\delta,n,c}}{\text{minimize}}\Big(\sum_{k=1}^{n}\sum_{j=1}^{p_k}\big\|r_{\mathcal{P}}(\tilde{\mathbf{z}}_{r_k}^{s_j},\mathcal{X})\big\|_{\sigma_{\delta,n_k}^{s_j},p_r}^{2}+
\sum_{k=1}^{n} {\big\| \mathbf{r}_{\mathcal{T}}(\tilde{\mathbf{z}}_{k-1}^{k},\mathcal{X}) \big\|} _{\mathbf{D}_{t,k}} ^{2}\Big)
\]
demo
Reference
[1] GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation
[2] ftp://cddis.gsfc.nasa.gov/ ftp://cddis.gsfc.nasa.gov/highrate/2020/188
[3] ftp://nfs.kasi.re.kr/gps/data/daily
[4] ftp://www.igs.org/pub/
[5] https://zhuanlan.zhihu.com/p/566273634