# FAR-AVIO: Fast and Robust Schur-Complement Based Acoustic-Visual-Inertial Fusion Odometry With Sensor Calibration Hao Wei $^{ID}$, Graduate Student Member, IEEE, Peiji Wang, Qianhao Wang $^{ID}$, Yang Gu $^{ID}$, Graduate Student Member, IEEE, Tong Qin $^{ID}$, Senior Member, IEEE, Fei Gao $^{ID}$, Member, IEEE, and Yulin Si $^{ID}$, Member, IEEE Abstract—Underwater environments impose severe challenges to visual-inertial odometry systems, as strong light attenuation, marine snow and turbidity, together with weakly exciting motions, degrade inertial observability and cause frequent tracking failures over long-term operation. While tightly coupled acoustic-visual-inertial fusion, typically implemented through an acoustic Doppler Velocity Log (DVL) integrated with visual-inertial measurements, can provide accurate state estimation, the associated graph-based optimization is often computationally prohibitive for real-time deployment on resource-constrained platforms. Here we present FAR-AVIO, a Schur-Complement based, tightly coupled acoustic-visual-inertial odometry framework tailored for underwater robots. FAR-AVIO embeds a Schur complement formulation into an Extended Kalman Filter (EKF), enabling joint pose-landmark optimization for accuracy while maintaining constant-time updates by efficiently marginalizing landmark states. On top of this backbone, we introduce Adaptive Weight Adjustment and Reliability Evaluation (AWARE), an online sensor health module that continuously assesses the reliability of visual, inertial and DVL measurements and adaptively regulates their sigma weights, and we develop an efficient online calibration scheme that jointly estimates DVL-IMU extrinsics, without dedicated calibration manoeuvres. Numerical simulations and real-world underwater experiments consistently show that FAR-AVIO outperforms state-of-the-art underwater SLAM baselines in both localization accuracy and computational efficiency, enabling robust operation on low-power embedded platforms. Our implementation has been released as open source software. Index Terms—Acoustic-Visual-Inertial fusion, error state Kalman filter, underwater robotics. ### I. INTRODUCTION M ARINE robotics, such as Autonomous Underwater Vehicles (AUVs) and Remotely Operated Vehicles (ROVs), have become indispensable platforms for subsea infrastructure inspection, offshore energy maintenance, and ocean exploration $^{[1]}$, [^2]. For these missions, accurate and drift-bounded localization is essential, yet satellite navigation is unavailable in underwater environments and external positioning infrastructure is typically absent or severely constrained in practice. Cameras, as compact and low-cost payloads, are therefore increasingly adopted on underwater robots, and visual Simultaneous Localization and Mapping (SLAM) has become a natural candidate for precise underwater localization. State-of-the-art visual and visual-inertial SLAM systems, such as ORB-SLAM3 [^3], VINS-Mono [^4], DM-VIO [^5], OV2-SLAM [^6], OKVIS2-X [^7] have demonstrated impressive performance in ground and aerial scenarios with abundant visual texture and well-excited inertial motion. However, underwater environments present substantial challenges to these methods. Rapid light attenuation, poor illumination, and dense “marine snow” often cause visual degradation that persists for tens of seconds to minutes, exceeding the temporal horizon over which pure visual tracking remains reliable [^2]. Moreover, underwater robots frequently operate with gentle, quasi-static motions, leading to low accelerometer signal-to-noise ratios and difficult IMU initialization, as a result, conventional visual-inertial pipelines struggle to maintain consistent accuracy during prolonged visual degradation [^8], [^9]. To mitigate these limitations, numerous underwater SLAM and navigation frameworks have incorporated additional proprioceptive and exteroceptive sensors, such as DVL, depth/pressure sensors and sonar. Recent tightly coupled visual-inertial-DVL [^10] and sonar-based [^11] systems have demonstrated remarkable robustness and accuracy in challenging underwater scenarios. However, most of these approaches rely on large-scale graph-based optimization or factor-graph SLAM backends, which incur substantial computational overhead and are difficult to deploy in real time on resource-constrained embedded platforms. In contrast, existing filter-based multi-sensor fusion methods typically simplify visual and DVL information into low-dimensional pose or velocity measurements [^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) without fully exploiting point-level constraints or performing online calibration of multi-sensor extrinsics, which limits their ultimate accuracy and long-term consistency. - FAR-AVIO introduces an AWARE module, performing an online health score mechanism that dynamically adjusts sensor fusion sigma scale based on real-time reliability assessment, enabling robust operation under sensor degradation and failure conditions. These observations highlight a persistent gap between accuracy and efficiency in current underwater localization systems. To mitigate this gap, this letter presents FAR-AVIO, a Fast and Robust Acoustic-Visual-Inertial Odometry with online calibration and an AWARE module. FAR-AVIO is inspired by Schur-Complement based sliding-window filters [^14], [^15], and employs the Schur complement within an EKF framework, enabling joint pose-landmark optimization while leveraging landmark independence for efficient, constant-time updates. This design bridges the gap between optimization-based accuracy and filter-based efficiency. The main contributions of this work are as follows: - We propose the first Schur-Complement based tightly coupled Acoustic-Visual-Inertial odometry framework that rigorously models DVL measurements from doppler-shift principles and embeds them in filter based backend optimization, while jointly performing online DVL-IMU extrinsic calibration, thereby achieving high localization accuracy and real-time performance on resource-constrained underwater platforms without dedicated calibration procedures. - FAR-AVIO has been validated through extensive experiments including numerical simulations, and real-world underwater experiments, demonstrating superior accuracy and computational efficiency compared to state-of-the-art methods (see running result example in Fig. 1). To benefit the research community, we release the complete implementation as open-source software. The proposed system architecture is illustrated in Fig. 2, and the rest of the paper is organized as follows: Section II reviews related literature. Section III describes the proposed FAR-AVIO framework, including the Schur-Complement based visual update, DVL measurement update, online sensor calibration, and
Fig. 2. Proposed system architecture of FAR-AVIO. Input is from an underwater robot equipped with a stereo camera, an IMU, and a DVL.
the AWARE module. Section IV presents experimental evaluation on real-world underwater datasets, Section V concludes the paper and discusses future work. ### II. RELATED WORK We briefly discuss the related works in this section. Visual and visual-inertial SLAM have been extensively studied in terrestrial and aerial domains [^3], [^4], [^5], [^6], [^16], [^17], [^18], [^19]. These approaches provide a strong baseline when visual texture is abundant and inertial excitation is sufficient, but their performance degrades severely in underwater environments as discussed in the Section I. In the following, we focus on underwater multi-sensor fusion methods, with particular emphasis on DVL-based localization. Early underwater SLAM frameworks combined a DVL, stereo vision and gyroscopic measurements in a loosely integrated pipeline [^13], [^20], where DVL outputs served only as external velocity priors to the visual estimator. Although this configuration can deliver reasonable pose estimates in difficult underwater scenes, it neither exploits accelerometer data nor models gyroscope bias, which leads to drifting roll and pitch and ultimately limits the long-term consistency of the solution. To improve this, several tightly coupled visual-DVL and visual-inertial-DVL frameworks have been proposed. A visual-DVL fusion method in [^21] directly injects DVL velocities into a factor-graph back-end to jointly optimize camera poses and DVL measurements, but does not incorporate IMU data, restricting robustness under rapid motion and severe visual degradation. A filter-based tightly coupled visual-inertial-DVL odometry method [^12] jointly fuses all three modalities, yet assumes known, fixed extrinsics, making it sensitive to calibration drift or hardware changes. More recent work extends DVL fusion to richer multi-sensor SLAM systems. A graph-based LiDAR-VI-DVL framework for autonomous surface vehicles [^22] achieves accurate localization on water-surface trajectories, but is tailored to 2-D surface motion with strong LiDAR constraints and is not directly applicable to fully submerged 3-D trajectories. A tightly coupled visual-inertial-acoustic system [^23] and AQUA-SLAM [^10] further integrate DVL, camera and IMU in ORB-SLAM3-style backends with online extrinsic calibration, and the latter provides --- the Tank dataset [^2] for benchmarking. However, these factor-graph-based methods entail significant computational cost and are challenging to deploy on resource-constrained platforms for real-time applications. ### III. FILTER DESCRIPTION ### A. State Definition and Propagation Following the state formulation in [^24] and augmenting it with sensor extrinsic parameters, we define the system state as $$ \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} $$ where $p_b^w$, $v_b^w$, and $R_b^w$ denote the position, velocity, and orientation of the body frame $\{b\}$ expressed in the world frame $\{w\}$, respectively. The vectors $b_a$ and $b_g$ represent the accelerometer and gyroscope biases of the IMU. The terms $T_c^b$ and $T_D^b$ denote the extrinsic transformations from the body frame $\{b\}$ to the camera frame $\{c\}$ and the DVL frame $\{D\}$, respectively, each consisting of a rotation and translation (e.g., $T_c^b = \{R_c^b, R_c^b\}$). Accordingly, the associated error-state vector $\delta x_b$ is defined as $$ \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} $$ For non-rotational part, the standard additive error model $\boldsymbol{x} = \hat{\boldsymbol{x}} + \delta \boldsymbol{x}$ is adopted, where $\hat{\boldsymbol{x}}$ denotes the nominal state. For rotational components, perturbations are defined on SO(3) using the approximation $$ \boldsymbol{R}=\mathrm{E x p}(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\hat{\boldsymbol{R}}, $$ where $\delta\boldsymbol{\theta} \in \mathbb{R}^3$ is minimal rotation error representation, $\lfloor \cdot \rfloor \times$ denotes skew-symmetric operator, and $\text{Exp}(\cdot)$ is the matrix exponential operation. $\delta\boldsymbol{T} \in \mathbb{R}^6$ is the minimal perturbations of the camera and DVL extrinsic transformations, each stacking a translation and a rotation error state. The continuous-time dynamics of the nominal state are given by (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} $$ where $\tilde{a}$ and $\tilde{\omega}$ are the raw accelerometer and gyroscope measurements, respectively, and $\mathbf{g}^{w}$ is the gravity vector in the world frame. The error-state propagation can be derived by linearizing the (4), yielding $$ \delta\dot{\boldsymbol{x}}_{b}=\boldsymbol{F}\delta\boldsymbol{x}_{b}+\boldsymbol{G}\boldsymbol{n}_{b}, $$ where F is the state transition Jacobian, G is the noise input matrix. $n_{b}=[n_{a}^{\top}n_{aw}^{\top}n_{g}^{\top}n_{gw}^{\top}]^{\top}$ stacks the processes noise, $n_{a}$ and $n_{g}$ denote the Gaussian white noise of the accelerometer and gyroscope measurements, respectively, while $n_{aw}$ and $n_{gw}$ model the random-walk processes driving the accelerometer and gyroscope biases. ### B. Visual Measurement and Equivalent Residual Model To enable multi-view constraints, we maintain a fixed-size sliding window of N keyframes by cloning the current IMU pose when a new frame arrives; if it becomes a keyframe, the oldest frame is marginalized, otherwise the second-newest is discarded. For each landmark observed across these keyframes, assume that the j-th 3D landmark $\hat{\pmb{\xi}}_j^w \in \mathbb{R}^3$, expressed in the world frame w, is observed by the camera at keyframe i. The projection model can be written as $$ \hat{\boldsymbol{z}}_{i j}=\pi\Big(\hat{\boldsymbol{x}}_{b},\hat{\boldsymbol{\xi}}_{j}^{w}\Big), $$ where $\hat{z}_{ij} \in \mathbb{R}^2$ is the predicted pixel coordinate of the landmark in the image plane, $\pi(\cdot)$ denotes the camera projection function that maps the 3D landmark in the world frame to 2D pixel coordinates based on the current state estimate $\hat{x}_b$. Linearizing around the nominal state yields the reprojection residual $$ \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}, $$ where $r_{ij}$ and $z_{ij}$ are the reprojection residual and visual measurement from the frontend track result, respectively, $n_{ij}$ is the measurement noise, $H_{x,ij}$ and $H_{f,ij}$ are the Jacobians of the projection function with respect to the system state and the landmark position, and $\delta \mathcal{X}$ and $\delta \xi_{j}^{w}$ are the state and landmark position perturbations, respectively. Their explicit forms are given by $$ \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}$ denotes the Jacobian of the camera pixel projection function with respect to the 3D point in the camera frame, and its explicit form depends on the specific camera model. Stacking all observations across multiple keyframes in sliding window with (7), we obtain $$ \boldsymbol{r}=\boldsymbol{H}_{x}\delta\boldsymbol{\mathcal{X}}+\boldsymbol{H}_{f}\delta\boldsymbol{\xi}+\boldsymbol{n}, $$ where r and $[H_x H_f]$ are the stacked residuals and Jacobians, respectively. The stacked noise term is $n = [u, u, \ldots, u]^\top$ with covariance $\mathcal{R} = \text{diag}(u^2, u^2, \ldots, u^2)$. Then, we directly project the measurement model of (9) into the Jacobian space $[H_x H_f]^\top$ to construct an equivalent observation model as $$ \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'$ is the equivalent observation noise, the corresponding covariance is given by $$ \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]. $$ Equations (10) and (11) thus define an equivalent observation model and noise model in Jacobian space that fully preserve --- the information from all visual measurements, effectively eliminating the original measurement-dimension limitation. For clarity, (10) can be expanded as $$ \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]}\;. $$ Due to the limitation of the dimension in the system, the filter state does not include the landmark perturbation $\delta\xi$, a standard observation model must be constructed by marginalizing $\delta\xi$. To this end, adopt a Schur-Complement based elimination [^14] to eliminate the landmark state from the observation model in (12), yielding $$ \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} $$ where $n_1''$ is the resulting equivalent observation noise with covariance $\mathcal{R}_1''$. Equations (13a) and (13b) define an equivalent observation model and noise model that depend only on the error state $\delta \mathcal{X}$, fully preserving the information from all visual measurements while successfully marginalizing out the landmark state $\delta \xi$. The resulting equivalent residual can then be inserted into the standard EKF update procedure. After the EKF pose update, we perform a lightweight Gauss-Newton step to update each landmark's 3D position $\xi_j^w$ by minimizing its multi-view reprojection residuals across the sliding window. This decoupled point update keeps landmarks geometrically consistent with the updated state without expanding the filter dimension. ### C. DVL Measurement Model 1) Single-Beam Doppler Velocity: A DVL transducer emits a narrowband acoustic wave of known carrier frequency $f_t$ and receives the echo reflected by the seabed or water-borne scatterers. Let $f_r$ denote the received frequency and $\Delta f = f_r - f_t$ denote measured doppler shift. Under the standard narrowband, small-velocity assumption $|v_r| \ll c_s$, where $c_s$ denotes the speed of sound in water, the classic monostatic doppler relation yields the radial velocity $v_r$ along the acoustic beam as $$ v_{r}\approx-\frac{c_{s}}{2f_{t}}\Delta f. $$ Here the sign convention is chosen such that $v_r > 0$ corresponds to the vehicle moving towards the seabed along the beam direction. For each of the four DVL beams (indexed by $i = 1, \ldots, 4$, as shown in Fig. 3), let $\tilde{v}_i$ denote the scalar radial velocity obtained by converting its measured doppler shift via (14). Ideally, $\tilde{v}_i$ coincides with the true radial velocity $v_{r,i}$ along that beam, in practice, it is corrupted by noise and occasional outliers due to low signal-to-noise ratio or loss of bottom lock. We model the single-beam measurement as $$ \tilde{v}_{i}=v_{r,i}+n_{i},\quad n_{i}\sim\mathcal{N}(0,\sigma_{i}^{2}), $$ where $n_i$ denotes zero-mean Gaussian measurement noise with variance $\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 Velocity Measurement Model: The linear velocity at the DVL acoustic center, expressed in the DVL frame $\{D\}$, as $$ {}^{D}\tilde{\pmb{v}}=\left[{}^{D}\tilde{v}_{x},{}^{D}\tilde{v}_{y},{}^{D}\tilde{v}_{z}\right]^{\mathsf{T}}. $$ Following the DVL geometry in Fig. 3, the orientation of each beam $e_i$ with respect to the frame $\{D\}$ has been parameterized by a fixed tilt angle $\alpha$ from the horizontal plane $x_{DYD}$ and an azimuth angle $\beta_i$ about the $z_D$-axis. The azimuth $\beta_i$ is measured in the $x_{DYD}$-plane from the $x_D$-axis to the projection of the i-th beam. For a four-beam Janus configuration, the beams share the same tilt $\alpha$ and have azimuths $\beta_i$. The example $e_i$ of transducer 1 can be expressed as $$ \boldsymbol{e}_{1}=\left[\cos\beta_{1}\cos\alpha,\sin\beta_{1}\cos\alpha,\sin\alpha\right]^{\mathrm{T}}. $$ Then the true radial velocity measured by this beam is simply the projection of the 3D velocity onto the beam direction: $$ v_{r,i}=e_{i}^{\top}{}^{D}\tilde{\mathbf{v}}. $$ Substituting (18) into (15), the single-beam DVL measurement model can be written as $$ \tilde{v}_{i}=e_{i}^{\top}{}^{D}\tilde{\mathbf{v}}+n_{i}, $$ which states that each transducer provides a observation of the vehicle velocity component along its own acoustic axis. Stacking the four scalar beam measurements into a vector $^b\tilde{\boldsymbol{v}} = \left[ \tilde{v}_1, \tilde{v}_2, \tilde{v}_3, \tilde{v}_4 \right]^\top$ and collecting the beam direction vectors into a matrix, the single-beam model (19) can be written in compact form as $$ {}^{b}\tilde{\boldsymbol{v}}=\boldsymbol{E}^{D}\tilde{\boldsymbol{v}}+\boldsymbol{n}_{b}, $$ where $E = [e_1, e_2, e_3, e_4]^\top \in \mathbb{R}^{4 \times 3}$ is the beam direction matrix, and $n_b = [n_1, n_2, n_3, n_4]^\top$ is the stacked noise vector. The matrix E is full column rank (rank 3) as long as the beams are not coplanar, which holds for standard DVL configurations. Therefore, the 3D velocity $^D\tilde{v}$ can be uniquely determined from the four beam measurements by solving the overdetermined linear system (20) in a least-squares sense: $$ ^{D}\tilde{\boldsymbol{v}}=\left(\boldsymbol{E}^{\top}\boldsymbol{E}\right)^{-1}\boldsymbol{E}^{\top}\boldsymbol{b}\tilde{\boldsymbol{v}}, $$ --- where the matrix inverse $(\boldsymbol{E}^{\top}\boldsymbol{E})^{-1}$ depends only on the known beam geometry and can be pre-computed offline. Since we are assuming independent Gaussian noise on each beam in (15), we have $$\begin{array}{r}{\boldsymbol{n}_{b}\sim\boldsymbol{\mathrm{N}}\big(\mathbf{0},\boldsymbol{\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}$$ Substituting (20) into (21) and separating the true velocity and noise terms yields $$ {}^{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}, $$ where $^D v_{true}$ denotes the true DVL-frame velocity. Since $n_b$ is zero-mean Gaussian, its linear image $An_b$ is also zero-mean Gaussian. Consequently, the estimated DVL-frame velocity remains Gaussian: $$ {}^{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}. $$ In the common case of identical per-beam variance $\sigma_{i}^{2}=\sigma^{2}$, this simplifies to $$ \mathbf{\Sigma}_{D}=\sigma^{2}\big(\mathbf{E}^{\top}\mathbf{E}\big)^{-1}, $$ which provides a convenient closed-form expression for the covariance of the DVL-frame velocity measurement. 3) DVL Residual for ESKF State Update: Given the estimated state, the predicted DVL-frame velocity is computed as $$ ^{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), $$ where $^{b}\hat{\omega}$ is the bias-corrected angular velocity in the body frame, $\hat{p}_{D}^{b}$ is the IMU–DVL lever arm expressed in the body frame, and $\hat{R}_{D}^{b}$ is the rotation from DVL to body frame. Thus we can get the DVL residual for the state update, according to the (26) and (21) $$ r_{\mathrm{D V L}}=^{D}\tilde{\boldsymbol{v}}-^{D}\hat{\boldsymbol{v}}. $$ This residual is then linearized with respect to the error state and used in the standard ESKF update with measurement covariance $\Sigma_{D}$. ### D. AWARE Module: Reliability Evaluation Most fusion-based filters assume fixed, time-invariant measurement noise for each sensor, which is rarely valid in practice: visual quality changes with texture, illumination and motion, and DVL measurements degrade under poor bottom lock, scattering or flow disturbances. If such variations are ignored, bursts of bad measurements from one modality can corrupt the entire estimate even when other sensors remain reliable. AWARE addresses this by continuously assessing visual and DVL quality, adapting their effective covariances, and temporarily disabling severely degraded sensors, thus preventing any single faulty source from dominating the fusion. For each sensor $s \in \{VIS, DVL\}$, AWARE maintains a reliability scale $\sigma_s$ and a fixed-length queue $Q_s$ of recent “unhealthy” events. At every measurement, a sensor-specific quality score $q_s \in [0,1]$ is computed ($q_{\mathrm{VIS}}$ from feature tracking statistics and reprojection error, $q_{\mathrm{DVL}}$ from velocity consistency and 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 foreach measurement $(t, z_s)$ do 2 $q_s \leftarrow \text{QualityScore}_s(z_s)$ 3 if $enabled_s$ then 4 if $q_s \geq \tau_s$ then 5 if $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 if $q_s$ is unhealthy 9 if $q_s$ is $R_{s}^{eff} \leftarrow R_s/\sigma_s^2$ 10 if $q_s \leq \tau_s$ then 11 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 12 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 13 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 14 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 15 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 16 else 17 if $q_s \geq \tau_s^{rec}$ then 18 if $q_s \leq R_{s}^{eff} \leq \tau_s$ 19 if $q_s \leq R_{s}^{eff} \leq \tau_s$ residuals). These scores drive covariance scaling and sensor gating decisions, as summarized in Algorithm 1. ### E. Pose Prior Visual Frontend Tracking Our visual frontend detects sparse Shi–Tomasi [^25] corners on an image pyramid and tracks them via pyramidal Lucas-Kanade [^26] optical flow, initialized with IMU-predicted pose priors to improve robustness under fast motion and blur. For stereo matching, candidate correspondences are constrained to a small window along the epipolar line, which is critical for long-baseline setups where unconstrained optical flow fails due to large disparities. ### IV. EXPERIMENTS ### A. Experiment Settings and Datasets In this section, we evaluate the proposed FAR-AVIO on the public Tank dataset [^2], which provides synchronized stereo, IMU, DVL and depth measurements collected in a wave tank. Accurate ground-truth (GT) camera poses are generated by the TankGT pipeline using AprilTag markers mounted on the underwater structure, enabling quantitative benchmarking under realistic underwater conditions. The eight sequences are grouped into three trajectory types (Structure, HalfTank, and WholeTank) with varying difficulty levels (Easy/Medium/Hard) determined by vehicle speed, lighting and the amount of textureless area. ---
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 achieves the best average translational accuracy among all methods, consistently ranking first or second on individual sequences. On challenging sequences (Structure-Hard, HalfTank-Hard, WholeTank-Medium), FAR-AVIO reduces translational RMSE by up to 75% compared with AQUASLAM (e.g., 0.13 m vs. 0.50 m on Structure-Hard) and by over an order of magnitude compared with purely visual-inertial methods. Notably, FAR-AIO (our system with vision disabled) remains functional across all sequences, demonstrating the robustness of the acoustic-inertial fusion backbone even under complete visual failure. Fig. 4 visualizes representative trajectory comparisons, for methods that diverged, only the successfully tracked portion is shown. In contrast, pure visual-inertial methods (ORB-SLAM3, VINS-Fusion, SVIN2) frequently incur large drift or tracking failure under turbidity and visual degradation, as reflected by
(d) Structure-Hard
Fig. 1 illustrates typical visual challenges and running result in the HalfTank-Easy sequence. We have benchmarked FAR-AVIO against five representative baselines: AQUA-SLAM, UVA-SLAM, SVIN2, ORB-SLAM3, and VINS-Fusion. For fairness, all methods use the same camera/IMU intrinsic and extrinsic parameters, and are run in stereo-inertial (or stereo-inertial-DVL when available) configurations. Estimated trajectories are aligned with ground truth using the method described in [^27], the average root-mean-square error (RMSE) and standard deviation (STD) of the absolute translation error (ATE), summarized in Table I. Entries marked as NaN indicate repeated tracking failure or divergence before sequence completion. ### B. Localization Performance Comparison ![Image](images/c2fc03d40b46c829698783b4e2e410b1eb7f610a5f2c71ca3b7c45dcf977640d.jpg) ![Image](images/8f6ec1f73d1c0db74c5ebdd1002090f1a911c44897433f9c584b6d80f932e91e.jpg)
Fig. 5. CPU load and memory usage of FAR-AVIO and baselines on Structure-Easy sequence.
meter-level errors and NaN entries. DVL-aided baselines (UVA-SLAM, AQUA-SLAM) substantially reduce drift and achieve sub-meter accuracy on most sequences, yet still exhibit higher errors than FAR-AVIO on difficult settings. ### C. Runtime and Computational Load We evaluate runtime and computational load for methods with public implementations (ORB-SLAM3, VINS-Fusion, SVIN2, FAR-AVIO) on a desktop CPU (AMD Ryzen 9 7950X, 32 GB RAM) and an embedded platform (NVIDIA Jetson Orin NX, 8 GB RAM). AQUA-SLAM (and UVA-SLAM), which are built on the ORB-SLAM-based backend, are not included in this comparison, but their computational cost is expected to be comparable to or higher than ORB-SLAM3. Fig. 5 shows that FAR-AVIO consistently exhibits the lowest CPU utilization and memory footprint among all baselines. On the Orin NX, the per-module breakdown in Table II shows that FAR-AVIO processes frames in 28.28 ms ( $\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%
achieving about a $2.2\times$ speed-up over VINS-Fusion (61.65 ms, $\approx$ 16 Hz). The main gain comes from the backend: VINS-Fusion spends 33.76 ms (54%) in visual optimization, whereas FAR-AVIO visual update requires only 6.08 ms (21%), and the additional DVL update adds merely 0.78 ms (2%). As a result, the visual frontend becomes the dominant cost in FAR-AVIO, confirming that the proposed filter based backend effectively removes the optimization bottleneck and is well suited for embedded deployment. ### D. Ablation Study for AWARE Modular and Extrinsic Calibration We conduct ablation experiments to quantify the contribution of the proposed AWARE module and the online calibration of the IMU–DVL extrinsics. The study includes both real Tank sequences and purely numerical simulations with synthetic IMU [^16] and DVL measurements, where ground-truth extrinsics are known. a) Effect of the AWARE module: To assess whether AWARE behaves as intended, we visualize the instantaneous confidence scales applied to visual and DVL measurements along the estimated trajectory. For two representative sequences, Structure–Easy (SE) and WholeTank–Hard (WH), we plot the trajectory and color each point by the corresponding visual and DVL $\Sigma_{scale} \in [0,1]$, where values close to 1 indicate high confidence (nominal weighting) and values approaching 0 indicate strong down-weighting of that sensor, see Fig. 6(a) and Fig. 6(b), on the SE sequence, which features relatively clean water and stable lighting, the visual frontend rarely experiences long-term degradation. Accordingly, both the visual and DVL scales remain close to 1 along almost the entire trajectory, with only minor fluctuations (Fig. 6(a)). This indicates that AWARE does not introduce unnecessary re-weighting when all sensors operate nominally, and effectively reduces to a standard tightly coupled VI-DVL fusion scheme on easy sequences. In contrast, the WH sequence is considerably more challenging, strong turbidity, non-uniform illumination, and large portions of the trajectory with weak or missing image features lead to extended periods of unreliable visual tracking. In these segments, the visual $\Sigma_{scale}$ is significantly reduced along the corresponding parts of the trajectory, while the DVL scale remains close to 1 (Fig. 6(b)). This behavior shows that AWARE automatically down-weights visual updates when the frontend reports poor tracking quality, and simultaneously leans more on the DVL constraints to stabilize the state estimate. As visual ![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 means tracking failure or filter divergence. conditions recover, the visual scale smoothly returns towards 1, and the fusion reverts to a more balanced VI-DVL weighting. To quantitatively assess the impact of AWARE on localization performance, we compare FAR-AVIO with and without AWARE on the Tank sequences, as well as FAR-AIO, a variant that fuses only IMU and DVL (no visual measurements) with AWARE enabled. As summarized in Table III, FAR-AVIO with AWARE achieves the lowest RMSE on all sequences and is the only variant that successfully completes every run, while both FAR-AIO and FAR-AVIO without AWARE experience failures on the more challenging cases. b) Effect of IMU–DVL extrinsic calibration: We first validate the IMU-DVL extrinsic calibration convergence in a numerical simulation. Fig. 7 plots the evolution of the extrinsic errors over time for three different initializations: an identity transform (Identity), and two perturbed initial extrinsics with medium (Noise Middle) and small (Noise Small) noise levels. In all cases, the estimated IMU-DVL extrinsics converge toward the ground truth, with the final errors stabilizing at a small residual level. Larger initial perturbations understandably lead to longer convergence transients and slightly higher steady-state error, but the calibration remains stable and convergent even after the final errors are reduced. --- ![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 mean FAR-AIO without extrinsic calibration. $^{2}$ NM mean middle level noise; NS mean small level noise. from the coarse identity initialization. This demonstrates that the proposed calibration scheme can reliably recover extrinsics from IMU-DVL motion without requiring a carefully tuned initial guess. We further quantify the impact on trajectory accuracy, as shown in Table IV, both real Tank sequences and simulated sequences are initialized with perturbed extrinsics. On Tank data, calibration reduces RMSE by 10–25%, in simulation with larger perturbations, corrupted extrinsics cause RMSE to exceed 3–9 m without calibration, whereas online calibration recovers sub-meter accuracy (0.124–0.574 m). On average, enabling calibration reduces RMSE from 8.152 m to 0.263 m, confirming that the proposed module reliably recovers accurate extrinsics from noisy initial guesses. We note that, as observed in our experiments, sufficient rotational excitation in the vehicle motion is important for reliable convergence; under near-stationary or purely translational motion, convergence may be slow, and providing a reasonable offline initial estimate is advisable in such cases. ### V. CONCLUSION This letter presented FAR-AVIO, a fast and robust Schur-Complement based Acoustic-Visual-Inertial fusion odometry framework with online sensor calibration for underwater robots. Extensive evaluations on real world sequences and synthetic scenarios demonstrated that FAR-AVIO achieves competitive or superior localization accuracy compared with state-of-the-art underwater and terrestrial baselines, while requiring substantially lower CPU and memory resources and running comfortably in real time on embedded hardware. ## REFERENCES [^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.