GNSS-RTK/INS紧组合算法

前言

相较于GNSS/INS松组合系统，GNSS-RTK/INS紧组合中将两个系统的状态量写在同一个状态模型中，观测值直接使用伪距和载波相位等GNSS原始观测值，两系统相互影响的程度加深。利用INS推算出来的短时高精度结果，可以实现INS辅助周跳检测，INS辅助模糊度固定等可以提升RTK解算精度的算法，因此基于GNSS-RTK/INS紧组合算法的研究非常广泛。本博客主要参考了开源项目IGNAV，及多篇RTK/INS紧组合论文，重点推导整理了INS机械编排部分算法及RTK/INS紧组合算法的融合部分算法。

INS机械编排（e系）

由于卫星位置及卫地距在ECEF_XYZ系下计算比较方便，因此机械编排和滤波模型都使用e系下的模型，与n系下的模型推导思路一致

姿态更新

这里采用四元数及等效旋转矢量进行姿态更新：

1、等效旋转矢量更新b系

2、等效旋转矢量更新e系

3、计算当前时刻的姿态四元数
4、对更新后的姿态四元数进行归一化处理

速度更新

e系下地速哥氏方程：

由e系下地速哥氏方程，参考n系下的流程推导可得：

位置更新

RTK/INS紧组合模型

状态模型

与机械编排同样的思路，我们将n系下误差模型中的角标n替换为e，然后去掉变为0的项，可得e系下误差模型：

wg，wa 为陀螺和加表的零偏白噪声。陀螺和加表零漂建模为一阶马尔科夫过程， τg，τa为对应的相关时间， wcg，wca为对应的过程白噪声。

在IMU位置上做组合，那么状态方程可写为：

量测模型

首先由e系下杆臂向量：

可得INS推算的天线相位中心到卫星p的距离：

代入卫星位置及INS推算位置及杆臂向量（此行往后坐标全在e系下）：

上式于天线位置(xI+xlever, yI+ylever, zI+zlever)处泰勒展开得：

同理可得INS推算的天线相位中心到卫星q的伪距：

基站与卫星p和q的距离分别为：

站间单差星间双差得：

短基线下GNSS双差观测方程（B表示站间单差整周模糊度）：

INS双差与GNSS双差作差得：

由于上述扰动是e系下在GNSS天线位置的位置扰动，而INS状态扰动为IMU处的扰动，因此需要一个转换，推导如下：

扰动分析可得（~表示带扰动量）：

移项可得天线位置扰动和IMU位置扰动之间的关系:

将上式代入观测方程，可得：

至此，可整理得紧组合量测模型：

若同时观测到k颗卫星，假设第一个卫星为双差基准星，则：

后面就是离散化然后卡尔曼滤波时间更新、量测更新，与松组合一致。