传感器融合是是当前自主导航的重要趋势，主要的原因是由于单一的传感器不能适用所有的场景，所以希望针对不同场景通过多个传感器的优势互补可以达到理想的定位效果。目前主要用于定位的传感器模块包括：IMU，地磁，Lidar，单目或者双目相机，Vicon以及GPS等。此外还有关于无线电定位的手段，如Wi-Fi，蓝牙，RFID等，但主要针对的是室内定位技术以及需要预先铺设大量的基础设施。在目前的行业中很多视觉+IMU的融合方案，视觉传感器在大多数纹理丰富的场景中可以准确识别，但是如果在玻璃，白墙等特征较少的场景，系统无法正确区分环境信息，单目的缺陷是无法获得深度信息而双目的有效估计受限于其基线距离，且实际应用中距离受限在在十几米以内；IMU长时间使用有非常大的累积误差，但是在短时间内，其相对位移数据又有很高的精度，所以当视觉传感器失效时，融合IMU数据，能够提高定位的精度。差分GPS在天气较好、遮挡较少的情况下能够获得很好的定位精度，但是在城市高楼区域、恶劣天气情况下效果下降非常多，这时候融合IMU+激光雷达（视觉）的方案能够填补不足。
目前融合方案分为紧耦合和松耦合两种方案，紧耦合（主要VIO）：IMU用于帧间预测，辅助视觉尺度估计，加入了视觉的特征点信息，算法复杂，系统稳定度低，增加传感器难度很大；松耦合：模块之间更新速率可以不一致，运算量低，时延低，容易添加定位模块，系统稳定度高。为了系统模块的稳定性和高扩展性我们采用松耦合的框架。
耦合框架主要解决的三部分问题：
1）传感器的状态估计：
1、对多传感器观测信息的在多参考系下进行合理性线性化，实现在高斯白噪声下最优估计
2、以误差四元数为模型，IMU作为控制量，以EKF为基础进行实时姿态估计，使得系统以最大传感器速率输出里程计结果
3、以IMU控制的核心状态量与辅助状态量分离，解决信号突然恶化导致的系统失效
2）传感器相对位姿的在线校准：
1、多源坐标系之间的坐标系转换与坐标校准以保证坐标的一致性
2、传感器本体的参数估计的校准与补偿，解决相对测量的尺度和漂移估计
3、GNSS信号绝对参考系动态实时维护坐标系的稳定性
3）GNSS与IMU的噪声的估计：
1、以GNSS的RAIM检验为基础，建立噪声模型来评估信号的正直性
2、通过ALLAN方差算法对IMU在时域估计出卡尔曼滤波器的滤波噪声协方差
为此，我们给出如上图所示的耦合架构，考虑到全局估计与优化，系统以IMU作为核心控制量，选取卡尔曼滤波作为系统控制器，并采用error-state作为状态估计量进行参数估计。系统分为三部分，即nominal的状态部分（无噪声+扰动），表示pose的状态：
Error-state状态部分（噪声+扰动），假设为正态分布，主要进行噪声传递：
与控制量，即IMU的参数：
观测量对各个参数进行更新：
随后将error-state注入nominal状态中，并重置error-state：
需要补充说明的是，针对不同传感器组合，会有不同的观测和校正方法，但都是以IMU作为唯一控制量，主要原因是我们需要将所有观测都放入状态量中而非作为加权方式进行定位，而IMU可以更新所有的旋转矩阵，是合适的控制量选取，且IMU理论上不应该被预测，这也是IMU参数不放入状态量的原因。
另外在实际应用中，Local和Vision的坐标系之间的p和q无法被观测到，所以实际置为0，并且在body框架下的之间的互观测量的p和q抖动极小。
预期成果：
基于实验室机器人平台，针对室内外切换和GNSS信号缺失与恶化情况，实现鲁棒性和高可用的多源融合辅助的GNSS定位算法，可通过该定位算法，实现在统一的坐标系下长时间、大范围广域复杂场景的无缝、无中断的定位，融合系统相比单个子系统精度均有显著提高。
