# FAR-AVIO：一种基于舒尔补码的快速且鲁棒的水下声学-视觉-惯性融合里程计及传感器校准方法 郝伟 $^{ID}$，IEEE 研究生会员；王培基；王乾浩 $^{ID}$；顾阳 $^{ID}$，IEEE 研究生会员；秦彤 $^{ID}$，IEEE 高级会员；高飞 $^{ID}$，IEEE 会员；司玉林 $^{ID}$，IEEE 会员。 摘要—水下环境对视觉-惯性里程计系统提出了严峻挑战：强烈的光衰减、海洋浮游物和浑浊度，加上运动激励不足，会降低惯性传感器的可观测性，导致长时间运行时频繁出现跟踪失效。虽然通过声学多普勒速度日志仪（DVL）与视觉-惯性测量数据紧密耦合的声学-视觉-惯性融合技术能够提供精确的状态估计，但相关的基于图优化的方法在资源受限的平台上往往计算量过大，无法实时部署。在此，我们提出FAR-AVIO——一种专为水下机器人设计的、基于舒尔补码的水下声学-视觉-惯性紧密耦合里程计框架。FAR-AVIO将舒尔补码公式嵌入扩展卡尔曼滤波器（EKF）中，在确保精度的同时实现位姿与路标点的联合优化，并通过高效边缘化路标点状态来维持恒定的更新时间。在此基础上，我们引入了自适应权重调整与可靠性评估（AWARE）模块，该在线传感器健康监测模块能够持续评估视觉、惯性和DVL测量的可靠性，并自适应调节其协方差权重；我们还开发了一种高效的在线校准方案，无需专门的校准动作即可联合估计DVL与惯性测量单元（IMU）之间的外部参数。数值仿真和真实水下实验均表明，无论在定位精度还是计算效率方面，FAR-AVIO都优于现有的先进水下SLAM基准算法，能够在低功耗嵌入式平台上实现稳定运行。我们的实现代码已作为开源软件发布。 索引词—声学-视觉-惯性融合；误差状态卡尔曼滤波器；水下机器人。 ## I. 引言 自主水下航行器（AUV）和遥控潜水器（ROV）等海洋机器人，已成为海底基础设施巡检、海上能源维护以及海洋探索不可或缺的平台 $^{[1]}$，[^2]。执行这些任务时，精确且漂移可控的定位能力至关重要，但水下环境无法使用卫星导航，实际场景中通常也缺乏外部定位基础设施或此类设施受到严重限制。因此，摄像头作为紧凑且低成本的有效载荷，正越来越多地被应用于水下机器人，而视觉同步定位与建图（SLAM）也就顺理成章地成为水下精确定位的首选方案。 最先进的视觉和视觉-惯性SLAM系统，如ORB-SLAM3 [^3]、VINS-Mono [^4]、DM-VIO [^5]、OV2-SLAM [^6]、OKVIS2-X [^7]，在具有丰富视觉纹理和良好激励惯性运动的地面及空中场景中展现了出色的性能。然而，水下环境给这些方法带来了巨大挑战。快速的光衰减、昏暗的光线以及密集的“海洋雪”常常导致视觉退化，这种状态会持续数十秒到数分钟，超出了纯视觉跟踪保持可靠的时间范围 [^2]。此外，水下机器人通常以缓慢、准静态的方式运动，导致加速度计信噪比低且IMU初始化困难；因此，传统的视觉-惯性处理流程在长时间视觉退化期间难以维持稳定的精度 [^8]、[^9]。 为克服这些局限，许多水下SLAM与导航框架引入了额外的本体感知和外部感知传感器，如DVL、深度/压力传感器和声纳。近期紧密耦合的视觉-惯性-DVL [^10] 及基于声纳的系统 [^11] 在复杂水下场景中展现出了卓越的鲁棒性与精度。不过，这些方法大多依赖大规模图优化或因子图SLAM后端，会带来巨大的计算开销，难以在资源受限的嵌入式平台上实时部署。相比之下，现有的基于滤波的多传感器融合方法通常将视觉和DVL信息简化为低维的姿态或速度测量值 [^12]、[^13]， --- ![Image](images/e11babc66c0d2eb67eb0bd206ef2b54edbac2800f57796f298d821f2b5d75282.jpg)
Fig. 1. Real-world deployment of the proposed system in underwater environments. (a) illustrates the experimental setup, where an ROV inspects the target underwater structure. (b), (c), and (d) present typical visual challenges encountered underwater, including motion blur, longtime textureless regions, and marine snow. (e) shows the dense reconstruction result and estimated trajectory produced by FAR-AVIO.
![Image](images/f5da7a7e128907ce343ac06a89e0772a95a250e3a22b7f85036b122dc4873a68.jpg) 却未能充分利用点级约束或进行多传感器外参的在线标定，这限制了其最终精度和长期一致性。 - FAR-AVIO引入了一个AWARE模块，该模块采用在线健康评分机制，能够根据实时可靠性评估动态调节传感器融合的协方差尺度，从而在传感器退化或失效条件下仍能实现稳健运行。 这些现象反映出当前水下定位系统在精度与效率之间仍存在明显差距。为弥合这一差距，本文提出FAR-AVIO——一种具备在线标定功能和AWARE模块的快速且鲁棒的声学-视觉-惯性里程计。FAR-AVIO的灵感来源于基于Schur补的滑动窗口滤波器 [^14]、[^15]，它在EKF框架内运用Schur补技术，既能实现姿态与路标点的联合优化，又能利用路标点的独立性来实现高效、恒定时间的更新。这种设计架起了基于优化的精度与基于滤波的效率之间的桥梁。本研究的主要贡献如下： - 我们提出了首个基于Schur补的紧密耦合声学-视觉-惯性里程计框架，该框架严格依据多普勒频移原理对DVL测量值进行建模，并将其嵌入基于滤波的后端优化中，同时同步完成DVL与IMU的外参在线标定；由此在无需专门标定流程的情况下，便能在资源受限的水下平台上实现高精度定位与实时性能。 - FAR-AVIO已经通过包括数值模拟和真实水下实验在内的大量实验得到验证，与现有最先进方法相比，它展现出更高的精度和计算效率（详见图1中的运行结果示例）。为了造福研究界，我们已将完整的实现代码作为开源软件发布。 图2展示了所提出的系统架构，本文其余部分的结构安排如下：第二节回顾相关文献。第三节详细介绍所提出的FAR-AVIO框架，包括基于舒尔补码的视觉更新、DVL测量更新、在线传感器校准以及
Fig. 2. Proposed system architecture of FAR-AVIO. Input is from an underwater robot equipped with a stereo camera, an IMU, and a DVL.
AWARE模块。第四节展示在真实水下数据集上的实验评估结果，第五节对全文进行总结并探讨未来研究方向。 ### 第二节 相关工作 本节我们将简要讨论相关工作。视觉及视觉惯性SLAM在陆地和航空领域已被广泛研究[^3]、[^4]、[^5]、[^6]、[^16]、[^17]、[^18]、[^19]。当视觉纹理丰富且惯性激励充足时，这些方法能提供可靠的基准，但如第一节所述，它们在水下环境中的性能会严重下降。接下来我们将重点介绍水下多传感器融合方法，尤其侧重基于DVL的定位技术。 早期的水下SLAM框架将DVL、立体视觉和陀螺仪测量结果整合到一个松散集成的流程中[^13]、[^20]，其中DVL的输出仅作为视觉估计器的外部速度先验。尽管这种配置能在复杂水下场景中给出合理的位姿估计，但它既未利用加速度计数据，也未对陀螺仪偏差建模，从而导致横滚和俯仰角出现漂移，最终限制了解决方案的长期一致性。为改进这一问题，研究人员提出了多种紧耦合的视觉-DVL及视觉惯性-DVL框架。[^21]中提出的一种视觉-DVL融合方法直接将DVL速度注入因子图后端，以联合优化相机位姿和DVL测量值，但未纳入IMU数据，因此在快速运动或视觉严重退化的情况下鲁棒性有限。另一种基于滤波器的紧耦合视觉惯性DVL里程计方法[^12]可联合融合这三种模态的数据，但该方法假设外部参数已知且固定，因此对校准漂移或硬件变化较为敏感。 近期的研究进一步将DVL融合技术拓展到更丰富的多传感器SLAM系统中。用于自主水面艇的基于图的激光雷达-视觉惯性-DVL框架[^22]能在水面轨迹上实现精准定位，但该框架针对的是具有强激光雷达约束的二维水面运动，无法直接应用于完全浸没的三维轨迹。一种紧耦合的视觉惯性声学系统[^23]以及AQUA-SLAM[^10]进一步在ORB-SLAM3风格的后端中整合了DVL、相机和IMU，并支持在线外部参数校准，后者还提供了 --- 用于基准测试的Tank数据集[^2]。然而，这些基于因子图的方法计算成本较高，难以在资源受限的平台上部署以实现实时应用。 ### 第三节 滤波器描述 ### A. 状态定义与传播 根据[^24]中的状态表述，并结合传感器外部参数进行扩展，我们将系统状态定义如下： $$ \begin{array}{r}{\boldsymbol{x}_{b}=\left[\boldsymbol{p}_{b}^{w}\quad\boldsymbol{v}_{b}^{w}\quad\boldsymbol{R}_{b}^{w}\quad\boldsymbol{b}_{a}\quad\boldsymbol{b}_{g}\quad\boldsymbol{T}_{c}^{b}\quad\boldsymbol{T}_{D}^{b}\right]^{\top},}\end{array} $$ 其中，$p_b^w$、$v_b^w$和$R_b^w$分别表示机体坐标系$\{b\}$在世界坐标系$\{w\}$下的位置、速度和姿态。向量$b_a$和$b_g$代表惯性测量单元（IMU）的加速度计和陀螺仪偏差。项$T_c^b$和$T_D^b$表示机体坐标系$\{b\}$到相机坐标系$\{c\}$以及到多普勒速度计（DVL）坐标系$\{D\}$的外部变换，每种变换都包含旋转和平移操作（例如$T_c^b = \{R_c^b, R_c^b\}$）。相应地，相关的误差状态向量$\delta x_b$被定义为： $$ \begin{array}{r}{\delta\boldsymbol{x}_{b}=\left[\delta\boldsymbol{p}_{b}^{w}~\delta\boldsymbol{v}_{b}^{w}~\delta\boldsymbol{\theta}_{b}^{w}~\delta\boldsymbol{b}_{a}~\delta\boldsymbol{b}_{g}~\delta\boldsymbol{\mathcal{T}}_{c}^{b}~\delta\boldsymbol{\mathcal{T}}_{D}^{b}\right]^{\top}.}\end{array} $$ 对于非旋转部分，采用标准的加法误差模型$\boldsymbol{x} = \hat{\boldsymbol{x}} + \delta \boldsymbol{x}$，其中$\hat{\boldsymbol{x}}$表示标称状态。对于旋转分量，则在SO(3)上利用近似方法进行扰动描述： $$ \boldsymbol{R}=\mathrm{E x p}(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\hat{\boldsymbol{R}}, $$ 其中，$\delta\boldsymbol{\theta} \in \mathbb{R}^3$是最小旋转误差表示，$\lfloor \cdot \rfloor \times$表示斜对称算子，而$\text{Exp}(\cdot)$则是矩阵指数运算。$\delta\boldsymbol{T} \in \mathbb{R}^6$表示相机和DVL外部变换的最小扰动，每种扰动都包含了平移和旋转误差状态。 标称状态的连续时间动力学由公式（4）给出： $$ \begin{aligned}\dot{\hat{\boldsymbol{p}}}_{b}^{w}&=\hat{\boldsymbol{v}}_{b}^{w},\dot{\hat{\boldsymbol{b}}}_{a}=\mathbf{0},\dot{\hat{\boldsymbol{b}}}_{g}=\mathbf{0}\\\mathcal{\hat{T}}_{d}^{b}&=\mathbf{0},\mathcal{\hat{T}}_{d}^{b}=\mathbf{0}\\\dot{\hat{\boldsymbol{v}}}_{b}^{w}&=\hat{\boldsymbol{R}}_{b}^{w}\left(\tilde{\boldsymbol{a}}-\hat{\boldsymbol{b}}_{a}\right)+\mathbf{g}^{w}\\\dot{\hat{\boldsymbol{R}}}_{b}^{w}&=\hat{\boldsymbol{R}}_{b}^{w}\lfloor\left(\tilde{\boldsymbol{\omega}}-\hat{\boldsymbol{b}}_{g}\right)\rfloor_{\times},\end{aligned} $$ 其中，$\tilde{a}$和$\tilde{\omega}$分别为原始的加速度计和陀螺仪测量值，而$\mathbf{g}^{w}$则是世界坐标系中的重力矢量。通过对公式（4）线性化，可得到误差状态的传播关系： $$ \delta\dot{\boldsymbol{x}}_{b}=\boldsymbol{F}\delta\boldsymbol{x}_{b}+\boldsymbol{G}\boldsymbol{n}_{b}, $$ 其中，F为状态转移雅可比矩阵，G为噪声输入矩阵。$n_{b}=[n_{a}^{\top}n_{aw}^{\top}n_{g}^{\top}n_{gw}^{\top}]^{\top}$包含了过程噪声，$n_{a}$和$n_{g}$分别表示加速度计和陀螺仪测量值的高斯白噪声，而$n_{aw}$和$n_{gw}$则用于描述驱动加速度计和陀螺仪偏差的随机游走过程。 ### B. 视觉测量与等效残差模型 为了实现多视图约束，我们通过克隆当前的IMU位姿来维持一个大小为N的关键帧滑动窗口；当新帧到达时，若其成为关键帧，则最旧的帧会被边缘化，否则会丢弃第二新的帧。对于在这些关键帧中观测到的每个路标，假设第j个三维路标$\hat{\pmb{\xi}}_j^w \in \mathbb{R}^3$在世界坐标系w下，在关键帧i处被相机观测到。投影模型可表示为： $$ \hat{\boldsymbol{z}}_{i j}=\pi\Big(\hat{\boldsymbol{x}}_{b},\hat{\boldsymbol{\xi}}_{j}^{w}\Big), $$ 其中，$\hat{z}_{ij} \in \mathbb{R}^2$是路标在图像平面上的预测像素坐标，$\pi(\cdot)$表示相机投影函数，它根据当前状态估计$\hat{x}_b$将世界坐标系中的三维路标映射到二维像素坐标。围绕标称状态线性化后，可得重投影残差： $$ \boldsymbol{r}_{i j}=\boldsymbol{z}_{i j}-\hat{\boldsymbol{z}}_{i j}\simeq\boldsymbol{H}_{x,i j}\delta\mathcal{X}+\boldsymbol{H}_{f,i j}\delta\boldsymbol{\xi}_{j}^{w}+\boldsymbol{n}_{i j}, $$ 其中，$r_{ij}$和$z_{ij}$分别为重投影残差以及前端跟踪结果中的视觉测量值，$n_{ij}$表示测量噪声，$H_{x,ij}$和$H_{f,ij}$分别是投影函数关于系统状态和路标位置的雅可比矩阵，而$\delta \mathcal{X}$和$\delta \xi_{j}^{w}$则分别对应状态和路标位置的扰动。它们的具体形式如下： $$ \begin{aligned}\boldsymbol{H}_{x,ij}&=\boldsymbol{J}_{\pi}\left[\begin{array}{cc}\left(\hat{\boldsymbol{R}}_{c}^{b}\right)^{\top}\lfloor\hat{\boldsymbol{\xi}}_{j}^{b}\rfloor_{\times}\boldsymbol{R}_{b_{i}}^{w^{\top}}&-\boldsymbol{R}_{c_{i}}^{w^{\top}}\end{array}\right],\\\boldsymbol{H}_{f,ij}&=\left[\boldsymbol{J}_{\pi}\boldsymbol{R}_{c_{i}}^{w^{\top}}\right],\end{aligned} $$ $J_{\pi}$ 表示相机像素投影函数相对于相机坐标系中三维点的雅可比矩阵，其具体形式取决于所采用的相机模型。将滑动窗口内多个关键帧中的所有观测值按照式(7)进行堆叠后，我们便能得到相应结果。 $$ \boldsymbol{r}=\boldsymbol{H}_{x}\delta\boldsymbol{\mathcal{X}}+\boldsymbol{H}_{f}\delta\boldsymbol{\xi}+\boldsymbol{n}, $$ 其中，r 和 $[H_x H_f]$ 分别是堆叠后的残差与雅可比矩阵；堆叠后的噪声项为 $n = [u, u, \ldots, u]^\top$，其协方差为 $\mathcal{R} = \text{diag}(u^2, u^2, \ldots, u^2)$。接着，我们将式(9)中的测量模型直接投影到雅可比空间 $[H_x H_f]^\top$，从而构建出等价的观测模型。 $$ \left[\begin{array}{c}\boldsymbol{H}_{x}^{\top}\\ \boldsymbol{H}_{f}^{\top}\end{array}\right]\boldsymbol{r}=\left[\begin{array}{c}\boldsymbol{H}_{x}^{\top}\\ \boldsymbol{H}_{f}^{\top}\end{array}\right]\left[\begin{array}{l l}\boldsymbol{H}_{x}&\boldsymbol{H}_{f}\end{array}\right]\left[\begin{array}{c}\delta\mathcal{X}\\ \delta\boldsymbol{\xi}\end{array}\right]+\boldsymbol{n}^{\prime}, $$ $n'$ 即为等价的观测噪声，其对应的协方差由下式给出： $$ \mathcal{R}^{\prime}=\left[\begin{array}{c}{H_{x}^{\top}}\\ {H_{f}^{\top}}\end{array}\right]\mathcal{R}\left[\begin{array}{l l}{H_{x}}&{H_{f}}\end{array}\right]. $$ 因此，式(10)和式(11)在雅可比空间中定义了等价的观测模型与噪声模型，它们能够完整保留所有视觉测量信息。 --- 这些信息涵盖了所有视觉测量的内容，有效消除了原有测量维度上的限制。为表述清晰，式(10)可展开为如下形式： $$ \underbrace{\left[\begin{array}{c}{\boldsymbol{H}_{x}^{\top}\boldsymbol{r}}\\ {\boldsymbol{H}_{f}^{\top}\boldsymbol{r}}\\ \end{array}\right]}_{\left[\begin{array}{l}{b_{1}}\\ {b_{2}}\\ \end{array}\right]}=\underbrace{\left[\begin{array}{l l}{\boldsymbol{H}_{x}^{\top}\boldsymbol{H}_{x}}&{\boldsymbol{H}_{x}^{\top}\boldsymbol{H}_{f}}\\ {\boldsymbol{H}_{f}^{\top}\boldsymbol{H}_{x}}&{\boldsymbol{H}_{f}^{\top}\boldsymbol{H}_{f}}\\ \end{array}\right]}_{\left[\begin{array}{l l}{C_{1}}&{C_{2}}\\ {C_{2}^{\top}}&{C_{3}}\\ \end{array}\right]}\left[\begin{array}{c}{\delta\mathcal{X}}\\ {\delta\xi}\\ \end{array}\right]+\underbrace{n^{\prime}}_{\left[\begin{array}{l}{n_{1}^{\prime}}\\ {n_{2}^{\prime}}\\ \end{array}\right]}\;. $$ 由于系统维度的限制，滤波状态中并不包含路标扰动项 $\delta\xi$，因此必须通过对 $\delta\xi$ 进行边缘化处理来构建标准的观测模型。为此，我们采用基于舒尔补的消元法 [^14]，从式(12)的观测模型中消去路标状态，最终得到如下结果。 $$ \left[\boldsymbol{b}_{1}-C_{2}C_{3}^{-1}\boldsymbol{b}_{2}\right]=\left[C_{1}-C_{2}C_{3}^{-1}C_{2}^{\top}\right]\delta\mathcal{X}+\boldsymbol{n}_{1}^{\prime\prime}, $$ $$ \begin{array}{r}{\mathcal{R}_{1}^{\prime\prime}=\left[C_{1}-C_{2}C_{3}^{-1}C_{2}^{\top}\right]u^{2},}\end{array} $$ 其中，$n_1''$ 是得到的等价观测噪声，其协方差为 $\mathcal{R}_1''$。式(13a)和式(13b)定义的等价观测模型与噪声模型仅依赖于误差状态 $\delta \mathcal{X}$，它们既完整保留了所有视觉测量的信息，又成功将路标状态 $\delta \xi$ 边缘化。得到的等价残差随后可被代入标准的EKF更新流程中。完成EKF位姿更新后，我们会执行一次轻量级的高斯-牛顿步骤，通过最小化滑动窗口内的多视图重投影残差，来更新每个路标的三维位置 $\xi_j^w$。这种解耦式的点更新方式能在不增加滤波器维度的情况下，保证路标与更新后的状态在几何上保持一致。 ### C. DVL测量模型 1) 单波束多普勒速度：DVL换能器发射一个已知载波频率为 $f_t$ 的窄带声波，并接收由海床或水中散射体反射的回波。设 $f_r$ 为接收到的频率，$\Delta f = f_r - f_t$ 为测得的多普勒频移。在标准的窄带、小速度假设下（其中 $|v_r| \ll c_s$，$c_s$ 表示水中的声速），经典的单站多普勒关系式可得出沿声束方向的径向速度 $v_r$，其表达式为： $$ v_{r}\approx-\frac{c_{s}}{2f_{t}}\Delta f. $$ 此处符号约定为：当 $v_r > 0$ 为正时，表示载具沿声束方向朝向海床运动。对于四条DVL声束（由 $i = 1, \ldots, 4$ 索引，如图3所示），设 $\tilde{v}_i$ 为通过式(14)将测得的多普勒频移转换得到的标量径向速度。理想情况下，$\tilde{v}_i$ 应与该声束方向上的真实径向速度 $v_{r,i}$ 一致；但实际中它会因信噪比低或失去海底锁定而受到噪声干扰，偶尔还会出现异常值。我们将单波束测量建模为： $$ \tilde{v}_{i}=v_{r,i}+n_{i},\quad n_{i}\sim\mathcal{N}(0,\sigma_{i}^{2}), $$ 其中，$n_i$ 表示均值为零的高斯测量噪声，其方差为 $\sigma_i^2$。 ![Image](images/7e026819acc437aa8e708eb0f9638fceff020311fe612896d12fbd9f37e86e6b.jpg)
Fig. 3. DVL transducer measurements are illustrated in both 2-D and 3-D views. The instrument comprises four transducers oriented in different directions, with Transducer 1 shown as a representative example.
2) DVL速度测量模型：DVL声学中心的线速度，以DVL坐标系表示 $\{D\}$，即 $$ {}^{D}\tilde{\pmb{v}}=\left[{}^{D}\tilde{v}_{x},{}^{D}\tilde{v}_{y},{}^{D}\tilde{v}_{z}\right]^{\mathsf{T}}. $$ 根据图3中的DVL几何结构，每条波束的方向 $e_i$ 相对于坐标系 $\{D\}$ 由固定倾角 $\alpha$（相对于水平面 $x_{DYD}$）以及绕 $\beta_i$ 轴的方位角 $z_D$ 来参数化。方位角 $\beta_i$ 是在 $x_{DYD}$ 平面内，从 $x_D$ 轴到第i条波束投影的夹角。对于四波束Janus配置，各波束具有相同的倾角 $\alpha$，且方位角均为 $\beta_i$。换能器1的示例 $e_i$ 可表示为 $$ \boldsymbol{e}_{1}=\left[\cos\beta_{1}\cos\alpha,\sin\beta_{1}\cos\alpha,\sin\alpha\right]^{\mathrm{T}}. $$ 那么这条波束测得的真实径向速度就是三维速度在波束方向上的投影： $$ v_{r,i}=e_{i}^{\top}{}^{D}\tilde{\mathbf{v}}. $$ 将式(18)代入式(15)，单波束DVL测量模型可写为 $$ \tilde{v}_{i}=e_{i}^{\top}{}^{D}\tilde{\mathbf{v}}+n_{i}, $$ 该式表明每个换能器提供了沿自身声轴方向的载体速度分量的观测值。 将四个标量波束测量结果堆叠成向量 $^b\tilde{\boldsymbol{v}} = \left[ \tilde{v}_1, \tilde{v}_2, \tilde{v}_3, \tilde{v}_4 \right]^\top$，并将波束方向向量整理成矩阵，单波束模型(19)可写成紧凑形式： $$ {}^{b}\tilde{\boldsymbol{v}}=\boldsymbol{E}^{D}\tilde{\boldsymbol{v}}+\boldsymbol{n}_{b}, $$ 其中 $E = [e_1, e_2, e_3, e_4]^\top \in \mathbb{R}^{4 \times 3}$ 为波束方向矩阵，$n_b = [n_1, n_2, n_3, n_4]^\top$ 为堆叠后的噪声向量。只要波束不共面（标准DVL配置均满足此条件），矩阵E的列秩即为3。因此，通过求解超定线性系统(20)的最小二乘解，即可由四个波束测量结果唯一确定三维速度 $^D\tilde{v}$： $$ ^{D}\tilde{\boldsymbol{v}}=\left(\boldsymbol{E}^{\top}\boldsymbol{E}\right)^{-1}\boldsymbol{E}^{\top}\boldsymbol{b}\tilde{\boldsymbol{v}}, $$ --- 其中矩阵逆 $(\boldsymbol{E}^{\top}\boldsymbol{E})^{-1}$ 仅取决于已知的波束几何参数，可离线预先计算。 由于我们假设式(15)中每条波束上的噪声相互独立且服从高斯分布，因此有 $$\begin{array}{r}{\boldsymbol{n}_{b}\sim\boldsymbol{\mathrm{N}}\big(\mathbf{0},\boldsymbol{\mathrm{{\Sigma}}}_{b}\big),\quad\boldsymbol{\Sigma}_{b}=\mathrm{diag}\big(\sigma_{1}^{2},\sigma_{2}^{2},\sigma_{3}^{2},\sigma_{4}^{2}\big).}\end{array}$$ 将式(20)代入式(21)，分离出真实速度和噪声项，可得 $$ {}^{D}\tilde{\mathbf{v}}={}^{D}\mathbf{v}_{\mathrm{t r u e}}+\boldsymbol{A}\boldsymbol{n}_{b},\quad\boldsymbol{A}=\left(\boldsymbol{E}^{\top}\boldsymbol{E}\right)^{-1}\boldsymbol{E}^{\top}, $$ 其中 $^D v_{true}$ 表示真实的DVL坐标系下的速度。由于 $n_b$ 是零均值高斯噪声，其线性映射结果 $An_b$ 也服从零均值高斯分布。因此，估计得到的DVL坐标系下速度仍服从高斯分布： $$ {}^{D}\tilde{\mathbf{v}}\sim\mathcal{N}\big({}^{D}\mathbf{v}_{\mathtt{t r u e}},\:\mathbf{\Sigma}_{D}\big),\quad\mathbf{\Sigma}_{D}=A\mathbf{\Sigma}_{b}A^{\top}. $$ 在波束方差相同 $\sigma_{i}^{2}=\sigma^{2}$ 的常见情况下，这可简化为 $$ \mathbf{\Sigma}_{D}=\sigma^{2}\big(\mathbf{E}^{\top}\mathbf{E}\big)^{-1}, $$ 这给出了DVL坐标系下速度测量协方差的便捷闭式表达式。 3) 用于ESKF状态更新的DVL残差：给定估计状态后，预测的DVL坐标系下速度计算如下： $$ ^{D}\hat{\pmb{v}}=\left(\hat{\pmb{R}}_{D}^{b}\right)^{\top}\left(\left(\hat{\pmb{R}}_{b}^{w}\right)^{\top}\hat{\pmb{v}}_{b}^{w}+\lfloor{^{b}\hat{\pmb{\omega}}}\rfloor_{\times}\hat{\pmb{p}}_{D}^{b}\right), $$ 其中 $^{b}\hat{\omega}$ 是机体坐标系中经过偏差校正后的角速度，$\hat{p}_{D}^{b}$ 是机体坐标系中表示的IMU与DVL之间的杆臂矢量，$\hat{R}_{D}^{b}$ 是从DVL坐标系到机体坐标系的旋转矩阵。因此，根据式(26)和式(21)，我们可以得到用于状态更新的DVL残差。 $$ r_{\mathrm{D V L}}=^{D}\tilde{\boldsymbol{v}}-^{D}\hat{\boldsymbol{v}}. $$ 随后将该残差关于误差状态线性化，再结合测量协方差 $\Sigma_{D}$ 用于标准的ESKF更新过程。 ### D. AWARE模块：可靠性评估 大多数基于融合的滤波器都假设每个传感器的测量噪声是固定且时不变的，但这在现实中很少成立：视觉质量会随着纹理、光照和运动而变化，而DVL测量在底部锁定不良、散射或水流干扰的情况下也会变差。如果忽略这些变化，某一模态产生的大量劣质测量结果可能会破坏整个估计结果，即便其他传感器仍能提供可靠数据。AWARE通过持续评估视觉和DVL的质量、动态调整其有效协方差，并暂时禁用严重退化的传感器来解决这一问题，从而防止任何单一故障源主导融合过程。 对于每一个传感器$s \in \{VIS, DVL\}$，AWARE都会维护一个可靠性指标$\sigma_s$，以及一个存储近期“不健康”事件的固定长度队列$Q_s$。每次测量时，都会计算出该传感器特定的质量评分$q_s \in [0,1]$（其中$q_{\mathrm{VIS}}$基于特征跟踪统计量和重投影误差，$q_{\mathrm{DVL}}$基于速度一致性和DVL数据）。 ```pseudo // Algorithm 1: AWARE Update for a Generic Sensor s. Input: sensor stream $\{(t, z_s)\}$, unhealthy events $Q_s$. Output: adapted scale $\sigma_s$. ``` 1 针对每一次测量$(t, z_s)$执行以下操作： 2 $q_s \leftarrow \text{QualityScore}_s(z_s)$ 3 若$enabled_s$为真，则 4 若$q_s \geq \tau_s$为真，则 5 若$R_{s}^{eff} \leftarrow R_s/\sigma_s^2$为真 6 $\left\lfloor \begin{array}{c} \text{ESKF\_UPDATE}(z_s, R_{s}^{eff}) \\ \text{else} \end{array} \right\rfloor$ 8 若$q_s$为不健康状态 9 若$q_s$等于$R_{s}^{eff} \leftarrow R_s/\sigma_s^2$ 10 若$q_s \leq \tau_s$为真，则 11 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 12 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 13 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 14 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 15 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 16 否则 17 若$q_s \geq \tau_s^{rec}$为真，则 18 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 19 若$q_s \leq R_{s}^{eff} \leq \tau_s$为真 残差）。这些评分会用于驱动协方差缩放和传感器门控决策，具体如算法1所总结。 ### E. 位姿先验视觉前端跟踪 我们的视觉前端会在图像金字塔上检测稀疏的Shi-Tomasi[^25]角点，并通过金字塔式Lucas-Kanade[^26]光流法对其进行跟踪；为提升在快速运动和模糊环境下的鲁棒性，跟踪初始化时会采用IMU预测的位姿先验。在立体匹配方面，候选对应点会被限制在极线上的一个小窗口内，这对于基线较长的场景至关重要——因为在此类场景中，无约束的光流法会因视差过大而无法正常工作。 ### IV. 实验 ### A. 实验设置与数据集 在本节中，我们在公开的Tank数据集[^2]上评估所提出的FAR-AVIO算法，该数据集包含了在水槽中采集的同步立体图像、IMU、DVL和深度测量数据。准确的真实值（GT）相机位姿由TankGT流程生成，该流程利用安装在水下结构上的AprilTag标记来实现定量基准测试，从而能在逼真的水下环境中进行评估。这八组序列被分为三种轨迹类型（Structure、HalfTank和WholeTank），根据载具速度、光照条件以及无纹理区域的大小，其难度等级被划分为简单/中等/困难。 ---
TABLEI
AVERAGE TRANSLATIONAL RMSE/STD (M) ON TANK SEQUENCES
SequenceFAR-AVIOFAR-AIOAQUA-SLAMUVA-SLAMSVIN2ORB-SLAM3VINS-Fusion
Structure Easy0.11 / 0.030.13 / 0.040.07 / 0.030.21 / 0.120.09 / 0.030.28 / 0.090.13 / 0.04
Structure Medium0.16 / 0.050.36 / 0.180.18 / 0.080.54 / 0.302.94 / 1.643.30 / 1.08NaN / NaN
Structure Hard0.13 / 0.040.14 / 0.040.50 / 0.240.50 / 0.233.26 / 1.432.73 / 1.45NaN / NaN
HalfTank Easy0.19 / 0.100.22 / 0.100.28 / 0.171.69 / 1.106.01 / 4.332.69 / 1.451.24 / 0.30
HalfTank Medium0.33 / 0.110.48 / 0.140.29 / 0.140.44 / 0.223.40 / 1.760.74 / 0.38NaN / NaN
HalfTank Hard0.25 / 0.100.33 / 0.130.36 / 0.220.58 / 0.3777.6 / 55.071.10 / 0.70NaN / NaN
WholeTank Medium0.34 / 0.150.53 / 0.230.52 / 0.281.34 / 0.730.72 / 0.411.18 / 0.7113.08 / 7.22
WholeTank Hard0.57 / 0.270.72 / 0.290.22 / 0.121.11 / 0.830.83 / 0.652.96 / 2.49NaN / NaN
Total RMSE2.082.912.426.4194.8514.98NaN
![Image](images/fe5d162cc267a2256df75863926cd7804bd077dbdc33f87f000c3d1309a7e3cf.jpg)
(a) HalfTank-Easy
![Image](images/cbe3de2722efdcf6c1b50bca93290073aeea7d5472fde7108ec33d20532a77da.jpg)
(b) HalfTank-Medium
![Image](images/e871c70df1c073f6e801f0cebd3a030451de787fcd44069d85a2eee8c248f856.jpg) ![Image](images/afa2c76f98efba827e3c657cd9b951399d44b4354b6e4f0541aed61dd5cb1b8a.jpg)
Fig. 4. Different baseline methods estimated trajectory comparisons.
(c) Structure-Medium
在所有方法中，FAR-AVIO的平均平移精度最佳，在单个序列上的排名始终位居第一或第二。在难度较高的序列（Structure-Hard、HalfTank-Hard、WholeTank-Medium）中，与AQUASLAM相比，FAR-AVIO可将平移RMSE降低多达75%（例如在Structure-Hard序列中，FAR-AVIO的误差为0.13米，而AQUASLAM的误差为0.50米）；与纯视觉惯性方法相比，误差更是降低了超过一个数量级。值得注意的是，即使完全禁用视觉模块，我们的系统FAR-AIO仍能在所有序列中正常运行，这证明了即使在视觉完全失效的情况下，声学-惯性融合框架依然具备很强的鲁棒性。图4展示了具有代表性的轨迹对比结果，对于出现发散现象的方法，仅展示其成功跟踪的部分。 相比之下，纯视觉-惯性方法（ORB-SLAM3、VINS-Fusion、SVIN2）在浑浊环境和视觉退化条件下常常会出现较大的漂移或跟踪失败现象，具体表现如下：
(d) Structure-Hard
图1展示了HalfTank-Easy序列中的典型视觉挑战及运行结果。 我们将FAR-AVIO与五种具有代表性的基线方法进行了对比测试：AQUA-SLAM、UVA-SLAM、SVIN2、ORB-SLAM3和VINS-Fusion。为确保公平性，所有方法均采用相同的相机/IMU内参和外参，并在立体惯性（或可用时采用立体惯性-DVL）配置下运行。估计轨迹通过[^27]中描述的方法与地面真值对齐，绝对平移误差（ATE）的平均均方根误差（RMSE）和标准差（STD）汇总于表I中。标记为NaN的条目表示在序列完成前出现了反复跟踪失败或发散现象。 ### B. 定位性能对比 ![Image](images/c2fc03d40b46c829698783b4e2e410b1eb7f610a5f2c71ca3b7c45dcf977640d.jpg) ![Image](images/8f6ec1f73d1c0db74c5ebdd1002090f1a911c44897433f9c584b6d80f932e91e.jpg)
Fig. 5. CPU load and memory usage of FAR-AVIO and baselines on Structure-Easy sequence.
米级误差和NaN条目。借助DVL的基线方法（UVA-SLAM、AQUA-SLAM）显著降低了漂移，在大多数序列中能达到亚米级精度，但在复杂环境下其误差仍比FAR-AVIO更大。 ### C. 运行时间与计算负载 我们在台式机CPU（AMD Ryzen 9 7950X，32 GB内存）和嵌入式平台（NVIDIA Jetson Orin NX，8 GB内存）上，对具备公开实现的方法（ORB-SLAM3、VINS-Fusion、SVIN2、FAR-AVIO）的运行时间和计算负载进行了评估。基于ORB-SLAM后端构建的AQUA-SLAM（以及UVA-SLAM）未纳入本次对比，但其计算成本预计与ORB-SLAM3相当甚至更高。图5显示，在所有基线方法中，FAR-AVIO的CPU利用率和内存占用始终最低。 在Orin NX平台上，表II中的模块级分解数据显示，FAR-AVIO处理一帧图像仅需28.28毫秒（$\approx$ 35 Hz）， ---
TABLE II
RONTIME COMPARISON BETWEEN VINS-FUSION AND FAR-AVIO ON THE EMBEDDED PLATFORM
ModuleVINS-FusionFAR-AVIO (Ours)
Time (ms)ShareTime (ms)Share
Visual frontend27.6945%20.2173%
Visual measure update33.7654%6.0821%
DVL measure update--0.782%
Others (I/O, etc.)0.201%1.214%
Total61.65100%28.28100%
相比VINS-Fusion（61.65毫秒，$2.2\times$ 16 Hz），速度提升了约$\approx$。主要优势来自后端：VINS-Fusion在视觉优化环节耗时33.76毫秒（占54%），而FAR-AVIO的视觉更新仅需6.08毫秒（占21%），额外的DVL更新也仅增加0.78毫秒（占2%）。因此，视觉前端成了FAR-AVIO的主要开销来源，这印证了所提出的基于滤波器的后端有效消除了优化瓶颈，非常适合嵌入式部署。 ### D. AWARE模块与外参校准的消融研究 我们开展了消融实验，以量化所提出的AWARE模块的贡献以及IMU-DVL外参的在线校准效果。该研究涵盖了真实的坦克序列，以及采用合成IMU [^16] 和DVL测量数据的纯数值模拟场景，其中外参的真值是已知的。 a) AWARE模块的效果：为了评估AWARE是否按预期工作，我们可视化了沿估计轨迹施加在视觉和DVL测量上的瞬时置信度尺度。对于两个具有代表性的序列——Structure–Easy（SE）和WholeTank–Hard（WH），我们绘制出轨迹，并根据对应的视觉和DVL $\Sigma_{scale} \in [0,1]$ 为各点着色；数值接近1表示置信度高（正常加权），而数值趋近于0则表明对该传感器的权重被大幅降低，详见图6(a)和图6(b)。在SE序列中，水质相对清澈且光照稳定，视觉前端很少出现长期性能退化现象。因此，几乎整个轨迹上视觉和DVL尺度都接近1，仅有微小波动（图6(a)）。这表明当所有传感器均正常工作时，AWARE不会引入不必要的权重调整，在简单序列中实际上会退化为标准的紧耦合VI-DVL融合方案。 相比之下，WH序列的挑战性要大得多：强浊度、光照不均，以及轨迹中大量图像特征微弱或缺失的区域，导致视觉跟踪长时间不可靠。在这些段落中，对应轨迹段上的视觉 $\Sigma_{scale}$ 显著降低，而DVL尺度仍接近1（图6(b)）。这种表现说明：当前端报告的跟踪质量较差时，AWARE会自动降低视觉更新的权重，同时更多地依赖DVL约束来稳定状态估计。随着视觉条件恢复，视觉尺度会平滑地回归至1，融合过程也会重新变为更均衡的VI-DVL加权模式。 ![Image](images/fcf62882188970b91e9974c02083b884f99bdce1f1411be132efeb3d33fc5591.jpg)
(a) Structure-Easy Sequence
![Image](images/6bb837e8502f426b7701c53462d330831f12dd14950949d3292f25458408ac3c.jpg) ![Image](images/1ca7d2f4f51696f5f54a2595190c7f1f159d8a61aaf204a98330cafa128ca6c7.jpg)
(b) WholeTank-Hard Sequence
Fig. 6. AWARE-estimated sigma scaling factors for visual and DVL measurements on two Tank sequences.
TABLE III
ABLATION OF THE AWARE MODULE ON TANK SEQUENCES
VariantSESMSHHEHMHH
FAR-AVIO (full)0.110.160.130.190.330.25
FAR-AIO0.240.210.140.39$F^{1}$F
FAR-AVIO w/o AWARE0.13FFFFF
$^{1}$ F表示跟踪失败或滤波器发散。 当视觉条件恢复后，视觉尺度会平滑地回归至1，融合过程也会重新变为更均衡的VI-DVL加权模式。 为了定量评估AWARE对定位性能的影响，我们比较了在Tank序列上启用与未启用AWARE的FAR-AVIO，以及仅融合IMU和DVL（无视觉测量）且启用AWARE的变体FAR-AIO。如表III所示，启用AWARE的FAR-AVIO在所有序列上均取得了最低的RMSE值，也是唯一能成功完成全部运行的变体；而FAR-AIO和未启用AWARE的FAR-AVIO在更具挑战性的场景中均出现了运行失败的情况。 b) IMU-DVL外部校准的效果：我们首先通过数值模拟验证了IMU-DVL外部校准的收敛性。图7展示了三种不同初始条件下外部误差随时间的演变情况：恒等变换（Identity），以及两种带有中等（Noise Middle）和小幅度（Noise Small）噪声的扰动初始外部参数。在所有情况下，估计出的IMU-DVL外部参数都会向真实值收敛，最终误差稳定在较小的残余水平。初始扰动越大，收敛过渡期越长且稳态误差略高，但即便在最终误差减小之后，校准过程依然保持稳定且具备收敛性。 --- ![Image](images/a926bd0ec9f97dc61605bfba6bee56b25bf61d870a4d6d671c8ec6c5d73239b0.jpg)
Fig. 7. Extrinsic of rotation part calibration convergence from different initializations in numerical simulation.
TABLE IV
ABLATION OF THE EXTRINSIC CALIBRATION MODULE ON TANK AND SIMULATE SEQUENCES
VariantSESHIdentityNM $^{2}$NS $^{2}$Average
FAR-AIO0.2420.2120.5740.1650.1240.263
w/o calib $^{1}$0.3230.2179.3723.5462.8488.152
$^{1}$ w/o calib表示未进行外部校准的FAR-AIO。 $^{2}$ NM代表中等水平噪声；NS代表小幅度噪声。 从粗略的恒等初始化出发即可实现这一结果，这证明了所提出的校准方案能够无需精心调校的初始猜测，就能可靠地从IMU-DVL运动数据中恢复外部参数。 我们进一步量化了其对轨迹精度的影响，如表四所示。真实的坦克序列和模拟序列均采用受扰动的外部参数进行初始化。在坦克数据上，校准可将均方根误差降低10–25%；在模拟实验中，当扰动较大时，未经校准的受损外部参数会导致均方根误差超过3–9米，而在线校准则能将精度恢复到亚米级（0.124–0.574米）。总体而言，启用校准后，均方根误差从8.152米降至0.263米，这证实了所提出的模块能够从嘈杂的初始估计中可靠地恢复出精确的外部参数。我们注意到，实验结果表明，车辆运动中的充分旋转激励对于可靠收敛至关重要；在近乎静止或纯平移运动的情况下，收敛速度可能较慢，此时提供一个合理的离线初始估计会更为妥当。 ### 五、结论 本文提出了FAR-AVIO——一种面向水下机器人的快速且稳健的基于舒尔补码的声-视觉-惯性融合里程计框架，该框架具备在线传感器校准功能。通过在真实序列和合成场景中的大量评估证实，与现有的水下及地面基准方法相比，FAR-AVIO在定位精度方面具有竞争力甚至更优的表现，同时它所需的CPU和内存资源显著更少，能够在嵌入式硬件上流畅地实时运行。 ## 参考文献 [^1]: M. Ferrera, V. Creuze, J. Moras, and P. Trouvé-Peloux, “AQUALOC: An underwater dataset for visual-inertial-pressure localization,” Int. J. Robot. Res., vol. 38, no. 14, pp. 1549–1559, 2019. [^2]: S. Xu et al., “Tank dataset: An underwater multi-sensor dataset for SLAM evaluation,” Int. J. Robot. Res., vol. 45, no. 4, pp. 541–551, 2025. [^3]: C. Campos, R. Elvira, J. J. G. Rodri guez, J. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021. [^4]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^5]: L. v. Stumberg and D. Cremers, “DM-VIO: Delayed marginalization visual-inertial odometry,” IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 1408–1415, Apr. 2022. [^6]: M. Ferrera, A. Eudes, J. Moras, M. Sanfourche, and G. Le Besnerais, "OV $^{2}$SLAM: A fully online and versatile visual SLAM for real-time applications," IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 1399–1406, Apr. 2021. [^7]: S. Boche, J. Jung, S. B. Laina, and S. Leutenegger, “OKVIS2-X: Open keyframe-based visual-inertial SLAM configurable with dense depth or LiDAR, and GNSS,” IEEE Trans. Robot., vol. 41, pp. 6064–6083, 2025. [^8]: C. Hu, S. Zhu, Y. Liang, and W. Song, “Tightly-coupled visual-inertial-pressure fusion using forward and backward IMU preintegration,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 6790–6797, Jul. 2022. [^9]: C. Hu, S. Zhu, Y. Liang, Z. Mu, and W. Song, “Visual-pressure fusion for underwater robot localization with online initialization,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 8426–8433, Aug. 2021. [^10]: S. Xu, K. Zhang, and S. Wang, “AQUA-SLAM: Tightly coupled underwater acoustic-visual-inertial slam with sensor calibration,” IEEE Trans. Robot., vol. 41, pp. 2785–2803, 2025. [^11]: S. Rahman, A. Quattrini Li, and I. Rekleitis, "SVIn2: A multi-sensor fusion-based underwater SLAM system," Int. J. Robot. Res., vol. 41, no. 11–12, pp. 1022–1042, Sep. 2022. [^12]: L. Zhao, M. Zhou, and B. Loose, “Tightly coupled visual-DVL-inertial odometry for robot-based ice-water boundary exploration,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2023, pp. 7127–7134. [^13]: E. Vargas et al., “Robust underwater visual SLAM fusing acoustic sensing,” in Proc. IEEE Int. Conf. Robot. Autom., 2021, pp. 2140–2146. [^14]: G. Sibley, L. Matthies, and G. Sukhatme, “Sliding window filter with application to planetary landing,” J. Field Robot., vol. 27, no. 5, pp. 587–608, Sep. 2010. [^15]: Y. Fan, T. Zhao, and G. Wang, “SchurVINS: Schur complement-based lightweight visual inertial navigation system,” in Proc. IEEE Int. Conf. Pattern Recognit., 2024, pp. 17964–17973. [^16]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in Proc. IEEE Int. Conf. Robot. Autom., 2020, pp. 4666–4672. [^17]: K. Sun et al., “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 965–972, Apr. 2018. [^18]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Autom., 2014, pp. 15–22. [^19]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization,” Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, 2015. [^20]: S. Xu et al., “Underwater visual acoustic SLAM with extrinsic calibration,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2021, pp. 7647–7652. [^21]: Y. Huang et al., “Tightly-coupled visual-DVL fusion for accurate localization of underwater robots,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2023, pp. 8090–8095. [^22]: A. Thoms, G. Earle, N. Charron, and S. Narasimhan, “Tightly coupled, graph-based DVL/IMU fusion and decoupled mapping for SLAM-Centric maritime infrastructure inspection,” IEEE J. Ocean. Eng., vol. 48, no. 3, pp. 663–676, Jul. 2023. [^23]: Y. Huang et al., “Visual-inertial-acoustic sensor fusion for accurate autonomous localization of underwater vehicles,” IEEE Trans. Cybern., vol. 55, no. 2, pp. 880–896, Feb. 2025. [^24]: J. Solà, “Quaternion kinematics for the error-state Kalman filter,” Institut de Robotica i Informatica Industrial, Universitat Politecnica de Catalunya, Barcelona, Spain, 2017. [^25]: J. Shi and C. Tomasi, “Good features to track,” in Proc. IEEE Int. Conf. Pattern Recognit., 1994, pp. 593–600. [^26]: B. D. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in Proc. Int. Joint Conf. Artif. Intell., 1981, pp. 674–679. [^27]: S. Umeyama, "Least-squares estimation of transformation parameters between two point patterns," IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, pp. 376–380, Apr. 1991.