一种考虑数据稀疏性的平方根卡尔曼滤波器，采用近似的施密特（Schmidt）更新方法，用于在高维GNSS/INS/视觉系统中高效估计姿态

《Information Fusion》：Sparsity-aware square-root Kalman filter with an approximate Schmidt update for efficient pose estimation in high-dimensional GNSS/INS/vision systems

　　• 结构稀疏性可以降低矩阵维度并加速QR分解。
• 施密特状态选择和近似更新可以降低计算成本。
• 在资源受限的平台上实现高效的高维姿态估计。
• 明确的计算复杂性分析和适用条件。

引言
连续和实时的导航信息对于基于位置的服务至关重要，包括移动机器人和无人地面车辆（UGVs）[1]、[2]。全球导航卫星系统（GNSS）、惯性导航系统（INS）和视觉传感器的集成可以提供可靠的姿态估计，因为它们具有互补的能力[3]。然而，异构传感器的融合需要处理大量的观测数据和估计状态，从而增加了计算成本，并限制了计算能力有限的资源受限平台的实时性能[4]。因此，在这些低功耗平台上平衡效率和精度仍然是一个关键的瓶颈。
GNSS/INS/视觉集成系统通常分为基于优化的方案和基于滤波器的方案。前者通过迭代线性化实现更高的精度，但代价是计算成本的增加[5]、[6]、[7]、[8]、[9]、[10]、[11]。相比之下，后者可以在保持相当精度的同时提供更高的效率，使其更适合在低功耗平台上部署。早期研究采用了松耦合系统，这些系统融合了视觉姿态[12]、GNSS导出的位置[13]、[14]和速度[15]。这种方法在复杂环境下面临精度快速下降的问题。最近，紧密耦合系统因利用原始观测数据来实现更高的精度而受到更多关注。Li[16]融合了来自惯性测量单元（IMU）、单目相机和单频GNSS的原始观测数据以实现精确导航。Li[17]和Wang[18]进一步融合了多频GNSS伪距和载波相位观测数据，验证了视觉信息对GNSS定位的增强效果。Xu[19]利用仅姿态表示通过相机姿态参数化特征深度，这样可以避免线性化误差并提高精度。尽管如此，现有的研究主要集中在姿态精度上，而对效率的关注有限。异构传感器的紧密耦合集成引入了大量的观测数据和估计状态，需要基于EKF的方案来处理更高维的矩阵，如创新矩阵和状态协方差。在GNSS/INS/视觉系统中，视觉状态的维度随着滑动窗口大小的变化而变化，众多视觉地标进一步增加了观测数量。同时，多星座和多频GNSS观测还会引入伪距、载波相位观测数据以及与卫星和频率相关的额外模糊参数。在本研究中进行的实验中，总状态维度通常在90到140之间，视觉观测维度通常在100到400之间，有时甚至超过500。观测和状态维度的增长不可避免地增加了计算成本并降低了实时性能。

对于高维观测数据，基于EKF的方案的计算成本主要来自大规模密集创新协方差的反演，因为其复杂度随着观测维度的增加而立方级增长。虽然限制观测数量可以确保实时性能，但可能会导致精度下降[20]。平方根卡尔曼滤波器（SRKF）通过避免创新协方差的反演来降低复杂性，并且与其他滤波器相比具有更好的数值稳定性[21]、[22]、[23]、[24]，因此更适合资源受限的平台。Luo[25]和Sun[26]采用顺序更新来减少矩阵反演的维度。然而，这种方法阻止了使用三级BLAS来加速矩阵运算。Peng[27]、[28]利用Sherman-Morrison-Woodbury（SMW）引理和置换QR（P-QR）分解来执行向量运算。这种方法产生了一个下三角结构，减少了所需的反演维度，从而在高维观测下显著降低了复杂性。此外，还应用了高效的Cholesky分解来提高效率[29]。这些方法主要在高维观测下降低计算成本。然而，当状态维度增加时，它们的效率会下降。SRKF仍然由于复杂的状态更新方程和为保持平方根协方差的上三角结构所需的频繁QR分解而面临计算挑战。对于典型的紧密耦合GNSS/INS/视觉系统，状态通常包括INS状态、滑动窗口内的视觉姿态和GNSS模糊度。视觉状态维度随着窗口大小的增加而增加，而GNSS载波相位观测引入了额外的模糊参数。总状态维度随滑动窗口大小和载波相位观测数量的增加而自然增长。状态维度的增加扩大了处理的矩阵大小，进一步加剧了这些问题。

为了解决第一个限制，一个直观的想法是选择性地更新一部分状态及其对应的平方根协方差。施密特卡尔曼滤波器通过将一部分状态视为不更新的干扰参数（施密特状态）来实现这一点[30]。DuToit[31]通过指定映射为施密特状态，仅更新了互协方差项而不是协方差本身，从而在不牺牲估计一致性的情况下提高了效率。Geneva在视觉惯性系统（VINS）中将历史关键帧[32]和成熟地标[33]视为施密特状态，相应的协方差不进行更新，从而保证了线性计算复杂性。选择适当的施密特状态至关重要。指定过多的状态可能会降低精度，而指定太少的状态则只能在效率上带来微小的改进。由于视觉观测对GNSS模糊度没有直接约束，并且这些参数在收敛后保持稳定，因此可以在视觉更新期间自然地将其视为施密特状态。这种方法简化了状态更新方程，而不影响模糊状态或它们的平方根协方差，有效地降低了高维状态下的计算成本。

对于第二个限制，频繁的QR分解在传播、更新和边缘化阶段引入了额外的矩阵运算，这使得SRKF通常不如EKF方法高效[29]。估计状态的增长扩大了需要分解的矩阵维度，进一步增加了计算成本并降低了效率。幸运的是，由于异构传感器测量和状态参数之间缺乏直接约束，这个问题也可以得到缓解。例如，视觉观测相对于GNSS模糊度的雅可比矩阵恒为零，这在要分解的矩阵中引入了结构稀疏性。利用这种结构稀疏性，可以构建三角子矩阵以避免处理完整矩阵，从而减少矩阵维度并加速矩阵运算。这一属性还表明视觉观测不直接约束GNSS模糊状态，从而便于选择施密特状态。上述两个属性对于进一步提高高维状态下SRKF的效率具有巨大潜力。

总之，GNSS/INS/视觉集成系统中观测数据和状态的快速增长严重增加了矩阵维度和计算成本，使得现有的基于EKF的方案难以满足实时性能要求。以20 Hz的图像流为例，每个帧应在50毫秒内处理完成。正如后面实验所示，基于EKF的方法在RK3588平台上的峰值运行时间超过了150毫秒，远远超出了这一要求。为了解决这个问题，我们提出了一种具有近似施密特更新的稀疏感知平方根卡尔曼滤波器，以实现高维GNSS/INS/视觉系统中的高效姿态估计。在现有SRKF方案的基础上，所提出的方法利用结构稀疏性来减少需要QR分解的子矩阵的维度。此外，我们将收敛的GNSS模糊度视为施密特状态，并采用近似施密特更新来降低计算成本。我们工作与现有方法之间的区别总结在表1中。本研究的核心科学问题是确定SRKF在何种维度和结构条件下可以利用结构稀疏性和施密特状态来提高效率，同时保持相当的姿态精度。这些条件主要取决于观测和状态维度、不受观测直接约束的状态数量以及矩阵置换后保留的三角子矩阵。主要贡献如下：
- 提出了一种适用于GNSS/INS/视觉集成系统的稀疏感知SRKF方法。它利用结构稀疏性构建三角子矩阵，从而减少需要QR分解的矩阵维度并加速矩阵运算。
- 在视觉更新期间，对收敛的GNSS模糊度应用近似施密特更新。通过避免更新相应的状态和平方根协方差，进一步降低了计算成本。
- 详细分析了基于EKF和SRKF的方法在视觉更新期间的矩阵计算复杂性。通过理论分析和矩阵运算实验，本工作展示了所提方法优于基线SRKF和EKF方法的适用条件。
- 在真实世界数据集上进行的大量实验表明，所提方法具有更高的效率，并验证了其在资源受限平台上的实时性能。

本文的其余部分组织如下：第2节介绍初步和基本原理。第3节介绍了所提出的GNSS/INS/视觉集成系统的方法论和实现。第4节进行了EKF和SRKF方法之间的复杂性分析和矩阵运算实验。第5节通过真实世界实验数据集验证了效率和实时性能。最后，第6节总结了本文并讨论了潜在的研究方向。

**初步部分**
本节定义了坐标框架和符号，并回顾了P-QR分解和施密特滤波器的基本原理，为后续提出的方法的详细描述奠定了基础。

**提出的高效GNSS/INS/视觉系统**
本节描述了一种适用于GNSS/INS/视觉集成系统的稀疏感知平方根卡尔曼滤波器及其近似施密特更新方法。3.1节提供了系统概述。3.2节和3.3节描述了状态传播、增强和边缘化过程中的平方根协方差运算。3.4节和3.5节分别基于基线SRKF方法说明了GNSS和视觉测量更新，后者利用了结构...

**计算复杂性分析**
在本节中，我们分析了矩阵计算复杂性并评估了运行时间。理论分析和实验结果均表明，所提方法在高维观测和状态下实现了最低的计算运行时间。

**真实世界实验评估**
在本节中，通过真实世界实验评估了所提方法的计算优势和效率。所提出的改进主要旨在提高计算效率而非姿态精度。因此，效率 and 精度分别进行了讨论。首先，我们验证了所提方法在高维观测和状态下显著降低了计算成本。5.2计算运行时间分析，5.3计算...

**结论**
本研究提出了一种高效且实时的GNSS/INS/视觉集成系统，用于在高维观测和状态下降低计算复杂性。一方面，为了降低高维观测下的矩阵反演复杂性，所提方法基于SRKF方法，将大规模创新协方差重构为下三角矩阵，其维度缩小到状态大小。为了进一步提高效率，随着状态维度的...

**未引用的参考文献**
表6、表9、表15和图21。