Evaluation of UKF-Based Fusion Strategies for Autonomous Underwater Vehicles Multisensor Navigation

In the underwater domain, guaranteeing accurate navigation for an autonomous underwater vehicle (AUV) is a complex but fundamental task to be achieved. As a matter of fact, only by ensuring a correct AUV localization, it is possible to accomplish surveillance, monitoring, and inspection missions. Most of the navigation filters for AUVs are based on Bayesian estimators, such as the Kalman filter, the extended Kalman filter (EKF), or the unscented Kalman filter (UKF), and employ different instruments, including the Doppler velocity log to perform the localization task. Recently, the use of payload sensors, such as cameras or forward-looking SONARs, in navigation-aiding has arisen as an interesting research field in the attempt to reduce the localization error drift. Such sensors, if used simultaneously, can provide multiple observations, which can be combined in a Kalman filtering framework to increase navigation robustness against noise sources. Navigation techniques that employ multiple devices can provide a high improvement of the estimation quality, but they can also cause an increase in terms of computational load. Consequently, strategies that can represent a tradeoff between these two conflicting goals have to be investigated. In this contribution, two different frameworks have been implemented and compared: on the one hand, a centralized iterative UKF-based navigation approach and, on the other hand, a sensor fusion framework with parallel local UKFs. The sequential (or iterated) UKF, where the correction step is iteratively performed for each available measurement, belongs to the first class of filters. The federated and the consensus-based decentralized UKFs can be categorized as the second class and they differ in the employed fusion strategy. Experimental navigation data obtained during sea trials performed at Vulcano Island, Messina, Italy has been used for offline validation. The results analysis focuses on both the navigation quality and the filter robustness against the reduction of the available measurements.

accurate underwater localization and navigation for autonomous underwater vehicles (AUVs) has been a challenge. The principal difficulty stems from the unavailability of the traditional global localization sensors, such as the global positioning system (GPS), due to the reduced propagation of radio frequency signals in the underwater domain. To reach a satisfactory navigation accuracy, most of the navigation filters for AUVs exploit Bayesian estimators, such as the Kalman filter (KF) or its variants applied to nonlinear systems, as the extended Kalman filter (EKF) [1] or the unscented Kalman filter (UKF) [2]. As reported in [3], three different categories can be defined to collect the navigation and localization techniques, as the dead reckoning (DR) strategies, the transponder-based strategies, or the geophysical data based strategies. DR-based estimation requires employing accurate and expensive sensors, as the Doppler velocity log (DVL), to guarantee a precise position estimation. In acoustic transponders based approaches, the employment of long baseline and ultrashort baseline, located in a fixed known position, is required to find the AUV position correctly. Furthermore, period resurfacing operations are commonly performed to obtain a GPS fix and correct the navigation drift. Geophysical data based algorithms work on optical, acoustic, and/or magnetic information extrapolated from the surrounding environment. Usually, the strategies based on exteroceptive sensors, such as optical cameras or SONARs, employ simultaneous localization and mapping (SLAM) or payload-based odometry approaches to increase the estimation quality.
In industrial and research applications, the most exploited underwater navigation approaches are based on the use of a DVL and an attitude heading reference system, which are respectively employed for the linear and the angular component of the pose vector estimation. The DVL sensor provides, when the bottom-lock is possible, highly precise linear velocity estimates. When exploiting this sensor, the measurement error sources are related to external noises such as the presence of bubbles or other reflecting objects. In the case where the DVL is not present or cannot provide the correct linear speed measurements, sensors like optical cameras or forward-looking SONARs (FLSs), which are usually employed for the surrounding environment inspection, can be used for navigation aiding. In particular, due to the decreasing cost of optical cameras, visual odometry (VO) strategies have been widely investigated as a replacement or support to the DVL in underwater navigation. However, since the water reduces the camera field of view and complicates This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ identifying good reliable image features, a strongly textured environment is required to apply VO strategies effectively. FLSs, which have been often discarded for navigation purposes, have recently emerged as a novel instrument for acoustic-inertial based solutions for its robustness against the underwater environment. As a matter of fact, FLSs can penetrate water for long ranges in almost every scenario, but, on the contrary, the acquired images are impaired by a superimposed noise, which heavily reduces the image resolution. Consequently, the navigation system robustness depends on various factors, such as the surrounding environment properties or the presence of noise sources, which strongly influence the decision about the most suitable navigation strategy.
By following the considerations reported in [4], in the context of the presented work, the presented sensor fusion strategies have been combined with an UKF-based navigation framework. The main advantages of the UKF approach against the EKF one can be easily summarized. The UKF approach does not require Jacobians' calculation and is a derivative-free algorithm, which can be useful with nondifferentiable AUV models where a combination of sign functions and jump discontinuities is employed. The main advantages of the UKF approach against the EKF one can be easily summarized. The UKF approach does not require Jacobians' calculation. Moreover, it is a derivative-free algorithm, which can be useful with nondifferentiable AUV models where a combination of sign functions and jump discontinuities are employed. Last, concerning AUV localization, it has been employed in the authors' previous works [5] and [6], showing superiority with respect to the EKF. When the DVL, FLS, and camera are operative, multiple speed measurements are available as input to the UKF, thanks to DVL-based acquired data and the FLS and camera-based odometries. A standard centralized UKF may result in a high computational load when all the available measurements are passed centrally in one step to generate an estimate and its covariance matrix. Furthermore, the lack of robustness when there are spurious data in any sensor is negatively influenced by a central Kalman filtering strategy.
Possible alternative strategies have been studied to overcome these disadvantages and their results are compared in the present work. The proposed solutions follow two different ways to reach a compromise between combination of the acquired data, time consumption, and measurement outlier rejection. While on one side, an iterative filtering approach is performed, where the correction step is sequentially executed for each speed measurement, on the other, a parallel filtering strategy, composed of several local filters and one master filter dedicated to the fusion algorithm, is applied. For the latter, it is necessary to highlight that, depending on the used fusion criterion and the master algorithm framework design, the available parallel filtering strategies can be numerous and can drive different results.
Three different approaches will be described in depth in the present work, and their navigation results will be compared. One iterative filtering strategy, which, in the following, will be referred to as sequential UKF, will be compared with two parallel filtering strategies, the federated UKF and the consensus-based decentralized UKF, which differ from each other due to the employed fusion strategy.
The main contributions of the present work concern the development and comparison of UKF-based sensor fusion strategies for AUV navigation to overcome the limitation of the position estimation filter described in [5] and [7]. As reported in Section V, the employed sensor fusion strategies have been largely studied from a theoretical point of view and their stability and convergence have been proved. However, their exploitation for accurate underwater navigation is still a novel field of research. To the authors' best knowledge, these strategies have been tested with simulations and their properties have been outlined focusing on linear systems and linear KF. Instead, in the context of this work, the proposed solutions have been adapted to be combined with an UKF framework and the navigation results have been obtained by testing the algorithms with prerecorded real data. The offline validation of the developed sensor fusion strategies has been performed by employing experimental navigation and payload data recorded in Vulcano Island, Messina, Italy in June 2019 in the context of the European project EUMarineRobots (EUMR) [8].
The rest of this article is organized as follows. The state of the art in multisensor underwater navigation and Kalman filtering sensor fusion techniques are detailed in Section II-B, whereas Section III is dedicated to the employed strategies for speed measurement acquisition and computation in the context of this work. Section IV outlines the navigation filter properties. Section V treats the proposed navigation strategies, whereas offline validation and results comparison are reported in Section VII. Finally, Section VIII concludes this article.

A. Main Contributions
This article investigates the employment of different sensor fusion strategies for speed measurement fusion in the AUV navigation context. In particular, in the marine robotics realm, handling redundant observations (e.g., speed information from different sources) still represents an open problem for AUV navigation applications. Concerning the above-mentioned field, to the authors' best knowledge, conclusive studies are not present yet. Indeed, for the majority of the Bayesian estimators employed (e.g., EKF or UKF), a measurement vector containing all the available data provided by the on-board sensors might not be the better strategy. In fact, increasing the measurement vector dimension causes the rise of the computational time requested for each filter iteration due to its effect on the filter matrices' size to be inverted and multiplied between each other [9], [10]. In this context, centralized and decentralized fusion strategies have been developed and analyzed (both in terms of localization accuracy and computational burden). In particular, by means of offline validation with real data acquired with an AUV, sequential (or iterated), federated, and consensus-based decentralized techniques have been subject of investigation and have been compared with more traditional strategies, as a centralized standard UKF and the UKF proposed in [5] and [7], which will be referred in the following as reduced UKF. In the context of the presented work, the proposed sensor fusion strategies have been combined with an UKF-based navigation framework. Standard UKF for AUVs navigation has been already accounted in the authors' previous works [5] and [6]. In conclusion, the main contributions of this article are as follows: 1) the simultaneous employment of different sensing devices for AUV speed measurement, namely a DVL, an optical camera, and an FLS; 2) to the authors' best knowledge, a first comparative study among different data fusion techniques (both centralized and decentralized) for AUV navigation in the presence of redundant speed observations; 3) an offline validation of the hereby presented techniques by means of navigation data acquired during autonomous missions performed with an AUV.

B. Related Works
AUV localization is a challenging task due to all the difficulties introduced by the underwater environment, such as the lack of GPS and the reduced visibility. Consequently, the development of sensor fusion strategies, capable of exploiting all the measurement information acquired by the AUV during its mission, has been an active and interesting research field. Focusing on underwater navigation applications, visual inertial odometry (VIO) strategies have been appropriately investigated as a combination of inertial and optical data to overcome both sensors' limitations. This choice can be highly fruitful, thanks to the complementarity of the employed sensor properties. While cameras suffer highly rapid movements and textureless environments, inertial sensors, such as the inertial measurement units (IMUs), are independent of the observed scene and can be a useful support to the image-based navigation data. VIO strategies for underwater navigation have been addressed in [11] and [12].
Following the approaches introduced in the VIO field, acoustic inertial odometry has been explored in the last few years but still is in a more embryonic stage; some promising results can be found in [13]- [15], whereas structure from motion and SLAM have been tackled in [16] and [17], respectively. Finally, a SONAR visual inertial SLAM strategy has been proposed in [18] and [19].
To the authors' best knowledge, in the AUV navigation context, fusion strategy for redundant information as in the presence of acoustic, optical, and inertial sensors is still a niche and open problem; preliminary works (outside the SLAM realm) can be found in [20]. Broadly speaking, in the presence of redundant information, a first categorization can be made between centralized and decentralized methods. Historically, the former solutions represent the standard approach, and they advocate the usage of a single central filter devoted to estimation. A thorough review goes beyond the goal of this work, but a detailed discussion can be found [21]. As far as this work is concerned, the sequential (iterated) [22] and both the standard and the reduced UKFs have been subject of investigation. When compared with centralized solutions, decentralized methods significantly reduce the computational effort and increase the fault-tolerant capability of the overall navigation system [23]- [26]. Within the decentralized filtering realm, the federated filters introduced by Carlson [23] and [24] represent one of the most famous solutions. Besides, information Kalman filters are fused in a central processor unit in [27] and [28]. Moreover, consensus-based works [29] and [30], which typically involve a sensor network constituting several AUV platforms [31], [32], are considered here in a single vehicle context.

A. DVL-Based Dead Reckoning
Standard DR strategies usually estimate the AUV position by integrating the linear velocity measured through highly accurate instruments, such as DVLs. Assuming a discrete-time system, the mathematical equation that can be extracted from the previously introduced statement is reported in the following: where η 1,k and η 1,k−1 are, respectively, the current and previous position estimation output of the DR navigation system expressed in the North-East-down (NED) reference system, R n b (η 2 ) represents the rotation matrix between the NED and the body-fixed frames, ν 1,k−1 contains the body-fixed frame linear velocities, and ΔT is the fixed sampling time.

B. Visual Odometry
VO is a navigation technique that employs one or more cameras rigidly attached to a vehicle for its motion estimation. Briefly, a VO algorithm updates the estimated vehicle position by analyzing the induced motion on the images acquired from the cameras. For its nature, a VO strategy works well only if particular conditions are verified, such as a scene uniformly illuminated and with a high number of features. As input for the UKF algorithms, it has been used the linear speed computed through the mono (i.e., single camera employed) VO strategy proposed in [33], where complete information can be found. For the sake of this study, the algorithm returns the transformation matrix reconstructed from the displacements of their features, which can be employed, supposing the absolute image acquisition times are known, to compute the body speed estimation.

C. Acoustic Odometry
FLS-based DR strategies have been carefully investigated. These strategies, also called acoustic odometry (AO), can be defined as a navigation technique that employs an FLS rigidly attached to a vehicle for motion estimation. The authors' previous work is thoroughly detailed in [14], [15], and [34]. For the sake of this study, the algorithm is able to compute the FLS-based body speed estimation.

IV. NAVIGATION FILTER FRAMEWORK
The complete pose estimation filter works by following two parallel structures, where the first part, employed for attitude estimation, is employed as input in the second part, which is dedicated to position estimation. In particular, the attitude estimation filter is based on a compass, IMU, and fibre optic gyroscope (FOG) data, and it estimates the roll, pitch, and yaw angles and their derivatives to send the orientation values to the filter in charge of estimating the vehicle position. Diving deep into the details is beyond the purpose of this article; further information about the attitude estimation filter can be found in [35] and [36]. A UKF-based estimator with a mixed kinematic-dynamic vehicle model is employed as core of the position estimation filter. In particular, by taking into account only the longitudinal dynamics, the processing unit's computational cost is strongly reduced. An exhaustive analysis of the position estimation filter is detailed in [5] and [7]. Before describing the proposed sensor fusion strategies, it is necessary to briefly define the employed AUV state-space model and the sensor model equations.

A. AUV State-Space Model
The kinematic-dynamic model formulation is based on the standard notation from the Society of Naval Architects and Marine Engineering [37], where the state of the AUV, which is considered as a rigid body, is represented using two reference systems: 1) a local Earth-fixed reference frame, with axes pointing to North, East, and down (NED frame); 2) a body reference frame with axes' origin located on the vehicle center of gravity (body frame). The AUV pose can be reported with the vector η = [ n η 1 η 2 ] , where n η 1 is the AUV position with respect to the NED frame and η 2 is its orientation expressed in terms of roll, pitch, and yaw Euler angles. The vector ν = [ b ν 1 b ν 2 ] contains the linear and angular velocities of the AUV expressed in the body-fixed reference frame. The AUV kinematic law is reported in the following equation: where R n b (η 2 ) is the rotation matrix between the body and the NED frame and T b n (η 2 ) is the Euler matrix. According to [37], the AUV complete dynamic model is where M is the mass matrix, C(ν) is the Coriolis and centrifugal matrix, D(ν) is the damping matrix, the vector g(η) contains the buoyancy and gravity effects, and τ (ν, u) takes into account the thrust action on the AUV, which is dependent on the velocity ν and the rotational speed of each motor u ∈ R m , where m is the number of motors. According to the assumptions exploited in [5], the dynamic model presented in (4) can be strongly simplified, leading to the following equation for the dynamics along the surge axis: where τ 1x (ν, u) is the component of τ (ν, u) along the surgeaxis of the vehicle, m is the AUV dry mass, and D x (ν) is the damping term along the surge-axis. In particular, D x (ν) can be represented as follows: where C D is the surge-axis drag coefficient that depends on the parameters involved in the longitudinal drag. By following the previously introduced notation, the kinematic-dynamic model is described with the following state variables: with x ∈ R 6 . Considering the discrete state evolution, a mixed kinematic-dynamic model is employed to describe the AUV behavior and it can be represented as reported in the following: where k is the current iteration step, ΔT is the fixed sampling time of the filter, and w k is the additive process noise (assumed zero-mean Gaussian white noise).

B. Sensor Modeling
The position filter can use GPS fixes (available before the vehicle dives or during resurfacings) depth measurements from a depth sensor, as well as linear speed estimations which can be provided from multiple sensors, as the DVL, the FLS through AO strategies and the camera trough VO approaches. Considering that this work's focus is on the redundant velocity measurements, only the noise sources that affect the sensors involved in linear speed measurements will be described in the follows.
1) A DVL measurement can be modeled as in the following equation: where the measured velocity b v DVL is the sum of the true body-fixed velocity of the sensor bias b DVL and of the measurement noise DVL .

2) A 2-D FLS can provide a series of azimuth θ and range
R measurements with a scalar value that represents the intensity of the returned echo. Considering a scene point [ X Y Z ] , the imaging projection model for an FLS (Fig. 1), the following equation can be obtained: where m R is the measured range and R is its real value, m θ is the measured azimuth and θ is its real value, andb FLS and˜ FLS are, respectively, the additional bias term and the measurement noise for both the range and the azimuth angle. Considering that the FLS is used to obtain linear speed estimations and that it is not easy to predict the final noise statistics, a model with a measured value that is the sum of a true one, a bias term, and a noise can be adopted where the measured velocity b v FLS is assumed to be the sum of the true value b ν 1 of the bias b FLS and of the measurement noise FLS . 3) A monocular camera can provide a 2-D image, where, to each pixel located in [ x y ] on the image plane, it is possible to associate a scalar value that represents the lighting value. Considering a scene point [ X Y Z ] , the pinhole camera projection model ( Fig. 2) with focal length f , the following equation can be retrieved: where [ m x m y ] is the measured pixel position and [ x y ] is its real value, andb CAM and˜ CAM are, respectively, the additional bias term and the measurement noise for the x and y positions. Considering that the camera is used to obtain linear speed estimations and that it is convenient to standardize the noise modeling, a model with a measured value that is the sum of a true one, a bias term, and a noise can be employed where the measured velocity b v CAM is assumed to be the sum of the true value b ν 1 , of the bias b CAM , and of the measurement noise CAM . At the kth instant, the measurement vector y k ∈ R 5 is where n P GPS is the position in the NED frame obtained with the GPS and d n DS is the measured depth. As explained in Section III, acoustic and visual (camera is configured as bottom-looking) odometry provides a 2-D speed estimation vector, which belongs to the xy-plane of the body reference frame. To guarantee uniformity, only the body-fixed frame surge and sway velocities provided by the DVL are taken into account for the sensor fusion approaches. For the sake of completeness, it is necessary to highlight that the depth may be independently estimated using the DVL itself or a distinct depth sensor. In the context of this work, since the current navigation system exploits a depth sensor to estimate the depth, the proposed algorithms take into account just the AUV motion along the xy-plane. In conclusion, the measurement equation can be expressed as follows: where matrix H k is time-variant and it contains only 1 or 0 elements according to the presence of the corresponding measurement at the current iteration time and where GPS is the error introduced by the GPS accuracy and DS is the depth sensor noise. Several speed measurements can be discarded by following this framework due to their redundancy at each filter iteration. As shown in (14), only one speed measurement is employed in the vector y k and, if multiple ones are available, only one of them is chosen by following the chronological acquisition order (Fig. 3). As a matter of fact, measurement redundancy is not handled and every new speed measurement, regardless of the sensor that bought it, is overwritten on the previous one. This approach can cause the loss of helpful information and, consequently, a growth of the uncertainty on the AUV state knowledge, which results in an increase of the covariance matrix.

V. FILTERING STRATEGIES
The presented work's main contribution is the evaluation and comparison of the possible strategies for speed measurements fusion in an UKF-based navigation system. Handling several available measurements can be done by following multiple ways, but, necessarily, some of them are not compliant for robotics application, due to the limitations imposed by the required computational cost that compromise the real-time action. As a matter of fact, a standard UKF filter with a measurement vector containing all the available data provided by the on-board sensors is not the better strategy to be followed. Increasing the measurement vector dimension causes the rise of the computational time requested for each filter iteration due to its effect on the filter matrices' size to be inverted and multiplied between each other [9].
The first proposed solution to overcome this issue is an iterative filtering strategy. While the prediction step is not modified by the iterative approach, the correction step is performed one or more times depending on the available measurements. This approach is desirable when a tradeoff between the computational load and the reduction of discarded measurements is required. This approach will be accurately described in Section V-A, where the sequential UKF strategy is presented (Fig. 4).
The second proposed solution is a parallel filtering strategy. The position estimation filter is divided into two separate structures: The first one is a group of local filters, where each of them is dedicated to each single speed measurement sources, and the second one is a master filter, which deals with the fusion of the estimates and covariances provided by the local filters to compute as output a global optimal estimate and covariance. This approach guarantees, on the one hand, that all the acquired or computed speed measurements are employed and, on the  other hand, that the local estimates and covariances can be fused into a global estimate and covariance according to a chosen criterion imposed by the master filter. To guarantee the local filter parallelism and to avoid that one of them diverges due to the lack of measures, a reset function is always applied at the end of each master filter iteration. The reset procedure guarantees that the local filters' a priori estimates and covariances are accorded to the master filter a posteriori estimate and covariance, and it is necessary for the filter convergence. This approach will be deeply analyzed in the following sections, where the federated and the decentralized UKF strategies are explained. Even the global behavior of these strategies is similar; the internal functioning is markedly different. While, in the federated UKF approach (Section V-B), the state estimations are fused and weighted through the state covariances, in the decentralized UKF (Section V-C), the state estimations and covariances are iteratively forced to converge to a common value depending on the accordance between each other (Figs. 5 and 6).
For the sake of completeness, even the correction step in the sequential UKF is iteratively performed N times, where N is equal to the available velocity measurements, and all the data coming from the sensors are provided as input directly into a single filter. This approach is usually referred to as centralized Kalman filtering. On the contrary, the federated UKF, as the decentralized UKF, is a decentralized KF, where the global filtering job is divided among a bank of subfilters, each of which is operating on a separate subset of the complete measurement suite. In both the federated and the decentralized UKFs, the following four fundamental assumptions are taken into account for the filter framework development.
1) The state vector x is the same for all local filters and the master filter. 2) There is no information sharing among the local filters.
3) The errors in each measurement vector are mutually uncorrelated, and, thus, the global matrix R is block diagonal. 4) None of the measurement vectors are fed to the master filter directly. It is necessary to note that there are no constraints on the dimensionality of each measurement vector passed to the local filters. This missing constraint is very helpful in a filter for underwater navigation. The measurement's availability is not guaranteed at each iteration due to unpredictable external conditions, such as bubbles, an untextured environment, or acoustic noise. The main properties of the analyzed filters, which will be described in the following subsections, are summarized in Table I.

A. Sequential UKF
The sequential (or iterated) UKF approach aims to minimize the dimension of the innovation covariance matrix. This can be an important advantage for applications where the computational load is limited by the available hardware, such as in mobile underwater robotics, since such matrix is inverted during every correction step. By considering the measurement (15) and supposing that all the measurements (provided by GPS, depth sensor, DVL, FLS, and camera) are available, the measurement vector y k can be expressed as a vector belonging to the R 9 space. Suppose that instead of employing the entire measurement vector y k ∈ R 9 , N m measurement vectors y (i) k ∈ {R 1 , R 2 } are passed to the filter, where N m is the number of the available measurements, i = 1, 2, . . ., N m and k is the current iteration step. The measurement vectors that are compared are the following: The employed symbols have been defined in the previous sections. Instead of processing all the measurements at time k as a greater dimension single vector, a KF that can elaborate smaller dimension multiple vectors has been implemented.
Turning to the filter framework, it is necessary to underline that the sequential UKF prediction step is performed as in the standard UKF. The main changes can be observed in the correction stage, where the on-board sensors' measurements are employed. As for the measurement vector y (i) k , further in this section, the notation reported here will be employed:x (i) k|k is the optimal state estimate after the ith measurement has been processed at time k and P (i) k|k is the correspondent covariance matrix. From these definitions, it is possible to retrieve that the following equalities can be employed for correction step initialization: wherex (0) k|k−1 is the estimate after zero measurements have been processed and P (0) k|k−1 is its covariance matrix. The estimatex (i) k|k and the covariance P (i) k|k are computed by using the traditional UKF correction equations, which are repeated N m times. The updated estimate and covariance after each iteration are employed as initialization for the subsequent one. After all N m measurement vectors are processed, the corrected estimate and covariance can be retrieved as follows: The sequential UKF filter is summarized in Algorithm 1 and Fig. 7.

B. Federated UKF
The federated UKF scheme employs the principle of information sharing among the local filters handled by the master filter. As introduced before, the federated UKF approach is based on a double-step data processing architecture, in which a master filter subsequently combines the outputs of the sensor-related local filters. In the federated UKF approach, the ith local filter is assumed to implement the full-order state vector and, at step k, is assumed to have the availability of its respective prior estimatê x i,k|k−1 and its associated error covariance P i,k|k−1 . The input measurement vector provided to the ith filter at time k is y i,k , and, as explained in (21), the measurement equation of the local filter is where l i,k is a zero-mean random variable with covariance R i,k . By assuming that the local filters do not have access to each others' measurements, they form their respective corrected estimates and covariances according to the traditional UKF equations. It is important to note that the local estimates are optimal for the local provided measurements, but not with respect to all the available measurements, due to the local filter independence hypothesis.
Turning the attention to the master filter, which is an UKF dedicated to the computation of an optimal global estimate of the state vector x, the following notation is employed:x m,k|k−1 is the optimal estimate of x conditioned on all the measurement vectors provided to the local filters up to but not including y i,k , and P m,k|k−1 is the associated covariance matrix. The optimal global estimate and the corresponding error covariance, which are reported in the information form of the KF, can be retrieved as follows: where the incorrelation of the measurement vectors' hypothesis is employed. However, (22) and (23) have to be modified in light of one of the master filter properties, which implies that the master filter does not have direct access to the measurement vectors y i,k . As a consequence, (22) and (23) have to be rewritten in terms of the local filter estimates and covariances. It results that x m,k|k = P m,k|k P −1 m,k|k−1x m,k|k−1 The federated UKF filter is summarized in Algorithm 2 and Fig. 8, where the correction step is performed by following the information Kalman filtering approach, which is based on the definition of the information matrix Ω m,k|k = P −1 m,k|k and the vectorφ m,k|k = P −1 m,k|kx m,k|k . A reset procedure is applied to guarantee the information transfer from the master filter to the local filters. At the end of each iteration, the local filter estimates and covariances are updated with the global estimate and covariance. This last step is performed by using the following equations: for i = 1, 2, . . ., N and with γ i such that N i=1 γ −1 i = 1. As no information to weigh the local filters are available, the γ i factors have been set equal for all the local filters [38]. As the local filters can do their own local projections and repeat the cycle at step k + 1, the master filter projects the global estimate and covariance to obtain the a priori information for the following iteration. Thus, the multiple filters' architecture permits complete autonomy of local filters and it yields to local optimal estimates with respect to the available local measurements. Furthermore, the master filter achieves optimality in the global estimate. This solution permits to maintain the independence of the local filters between each other and, at the same time, to achieve the global optimal solution.

C. Consensus-Based Decentralized UKF
Distributed Kalman filtering algorithms are generally employed for sensor networks, where each node can share the information flow only with its neighbors on the network. Distributed Kalman filtering strategies for fully connected sensor network, or consensus-based decentralized Kalman filtering strategies, involve state estimation using a set of local KFs that communicate with all other nodes. The distributed and decentralized KFs generally use a static or dynamic consensus algorithm, which permits the fusion of estimate and covariance data obtained by each node. The distributed filtering theory, which has been largely employed for multiagent system applications, has been adapted here to work with a sensor network composed of the sensors devoted to speed measurement.
Within the consensus-based decentralized KF approach, the information form of a single central filter for a sensor network observing a process (i.e., the AUV localization process) boils down to a proper number of local filters that collectively calculate the same state estimate. The sensor network dedicated to the speed measurement can be represented as a simple fully connected triangular graph G (N , E), where N = {DVL, CAM, FLS} is the node set composed of the DVL, the camera, and the FLS and E = {{DVL, CAM}, {DVL, FLS}, {CAM, FLS}} is the edge set. In the rest of this subsection, the generic edge that connects the ith and the jth sensor, chosen from the set composed of the DVL, the FLS, and the camera, will be referred as {i, j}, where {i, j} ∈ E, i ∈ N and j ∈ N . The employed consensus algorithm is reported in the information form and is based on the following discrete-time iterative equation: where is the discretization step, which has to, necessarily, be sufficiently reduced to guarantee the algorithm convergence, a ij is the consensus coefficient between each sensor couple {i, j}, k|k and Ω i,k|k = P −1 i,k|k are, respectively, the information vector and the information matrix after the correction step of the ith filter and after the tth iteration of the consensus algorithm. Equations (28) and (29) can be manipulated to obtain an equivalent structure, where the autonomous and the consensus parts of the algorithm are highlighted: The discrete-time collective dynamics of the network driven by (30) and (31) can be written as follows: with P = I − L Perron matrix of a graph G with parameter , whose component can be retrieved with The matrix L is the weighted Laplacian matrix. The consensus coefficient a ij is a weight coefficient that is updated after each consensus iteration, which has been designed to consider not only the covariance matrix but also the accordance between each local state estimation for the global one computation. Practically, each a ij coefficient is defined through the following equation: It is immediate to note that the more the estimatesx i andx j are similar, more the coefficient a ij is near to one, which is the upper limit for the weight factor. The value of the coefficient a ij influences the estimate convergence to a common value, by weighting more estimates with a reduced covariance and a high consensus coefficient.
In the consensus-based decentralized UKF approach, as in the federated UKF, the ith local filter state vector is full-order and, at step k, is assumed to have the availability of its respective corrected estimatex i,k|k and its associated error covariance P i,k|k , which are employed in the master filter. The input measurement vector provided to the ith filter at time k is y i,k and the measurement equation of the local filter is, as in the federated UKF where l i,k is a zero mean random variable with covariance R i,k . Two stop criteria have been chosen for the consensus-based decentralized UKF. The algorithm could work until: 1) it reaches a fixed maximum number of iterations N iter ; 2) the mean distance between the state vector estimates provided by each node of the graph is lower than a fixed threshold γ. In the first case, while the global covariance matrix is retrieved by computing the inverse matrix of the sum of all the local information matrices, the global state estimate is retrieved by computing a covariance-based weighted mean. Equations (38) and (39) summarize the previous sentence In the second stop condition, which is the most frequent due to the convergence speed of the consensus algorithm, the global state estimate and its covariance matrix can be set equal to one of the correspondent values taken from one local filter. The chosen local filter employed during this step is not important because all the local estimates and covariances converged to the same values under a threshold, whose value has been a priori set with the aim of guaranteeing at least a fifth-order convergence. The reset procedure is not necessary for the consensus-based UKF. The consensus algorithm guarantees the convergence of all the local estimates and covariances, which are employed as initialization data for the following step. The consensus-based decentralized UKF filter is summarized in Algorithm 3 and Fig. 9. An example of the consensus step application is provided, with the aim of proving the convergence of the designed algorithm, and its convergence step is shown. Fig. 10 shows the state convergence for a network with three 2-D states.

VI. SIMULATION RESULTS
To highlight the properties of the proposed filters, some experiments in a simulated environment have been performed. The main goal of these simulations is to evaluate the consistency of the analyzed strategies and how they propagate the estimated covariances. The position filter was fed with GPS position measurements during the simulations, when the vehicle was higher than a fixed depth, depth measurements, and multiple speed measurements. To increase adherence to the real dataset, the speed measurements have been published at a different fixed rate for each sensor (i.e., DVL, VO, and AO). Following the frequency values reported in Table V, which will define an approximate availability rate for the speed data in the real-world experiments, the DVL measurements have been published with a 5-Hz rate, the camera-based and FLS-based speed estimation, respectively, at 2 and 1 Hz. To focus the attention only on the filter capabilities, instead of applying the VO and AO algorithms to estimate the camera and FLS-based velocities, all the speed measurements have been generated as random variables defined as where b ν 1 is the speed true value and N (0, δ) is the uncertainty defined as a normal distribution with zero-mean and covariance δ = [ 0.1 0.05 ] . The surge speed has been set equal to 0.5 m/s, and the covariance values have been chosen as a consequence.
The proposed strategies have been tested on a vehicle whose dynamic behavior has been simulated by using the equations defined in [37] and that has traveled a rectangular path at a fixed depth of 2 m. For each filter, a Monte Carlo simulation with 100 iterations has been performed. The position errors and the estimated 3σ bounds along the East and North directions are reported in Figs. 11-15. It can be noted that all the filters, except the consensus-based distributed UKF (e.g., reduced UKF,     standard UKF, sequential UKF, and federated UKF), have a similar 3σ bound propagation. The consensus-based distributed UKF 3σ bounds have the same behavior as the other filters, but its divergence is reduced. This can be related to the particular fusion strategy employed in the filter, which is dependent on the accordance between each local estimation. All the 3σ bounds continuously diverge when the vehicle is under the sea surface and no position measurements are available. It correctly represents the behavior of the AUV, which has no access to any new absolute position information until it resurfaces. Furthermore, the estimated resurfacing position has been compared with the theoretical first GPS fix and its 3σ bound. It is necessary to note that the resurfacing positions estimated in all the Monte Carlo simulations fall inside the 3σ bound, guaranteeing acceptable estimations. Furthermore, it is possible to compare the 3σ bound estimation obtained from the filters and the 3σ bound estimation obtained from the simulated data. The latter has been evaluated by computing the best normal distribution approximating the estimated resurfacing positions with respect to the theoretical ones. All the filters are consistent with their estimations, but the best results from this point of view can be obtained with the consensus-based decentralized UKF. Indeed, this filter provides data-based and filter-based 3σ bounds similar estimations. Besides that, it is necessary to note that, in all cases, the filter-based 3σ bounds estimations are larger than the data-based ones, and, consequently, all the filters have a conservative behavior.
Turning to the speed errors and the related 3σ bounds, it can be noted from Figs. 16 and 17 that the estimation errors remain, for all the time, within the bounds. It is necessary to highlight that only the output of one iteration of the Monte Carlo simulation has been reported for the speed estimations.

A. FeelHippo AUV Main Features
FeelHippo AUV [39], shown in Fig. 18, is a light-weight, compact, and high-functional vehicle developed and built by the Department of Industrial Engineering of the University of Florence. The vehicle, whose approximate dimensions are 600 × 640 × 500 mm, can perform up to 30-m depth. Its dry mass is 35 kg and the maximum permissible longitudinal speed is 1 m/s. The hardware and electronics are contained inside the vehicle's central core, which is a Plexiglass cylinder with 5-mm thickness. The principal electronic devices and sensors directly employed for the navigation tasks are as follows: 1) U-blox 7P precision GPS; 2) Orientus advanced navigation attitude heading reference system; 3) KVH DSP 1760 single-axis high-precision fiber optic gyroscope; 4) Nortek DVL1000 DVL, measuring linear velocity and acting as depth sensor; 5) Teledyne BlueView M900 2-D FLS; 6) two Microsoft Lifecam Cinema forward-and bottomlooking cameras;

7)
Intel i-7-based LP-175-Commel motherboard (used for on-board processing). Furthermore, FeelHippo AUV is equipped with devices dedicated to communication with the sea-surface through Wi-Fi or radio. The main properties of FeelHippo AUV are listed in Table II.

B. Navigation Filter Design Choice
Before describing the results obtained through the application of the presented UKF-based sensor fusion strategies, it is necessary to summarize here the main properties of the filter and the fulfilled choices. In the state transfer function, the nonlinear mixed kinematic-dynamic model proposed in (8) is employed. The focus of the presented work is the optimal fusion of the data coming from the on-board sensors. The measurement equation needs to be investigated more in detail. As introduced in Section IV, since the navigation filter works at a fixed sampling time of ΔT = 0.1 s or, in other words, at an operative frequency of 10 Hz, due to the various sampling rates of the available sensors and due to the various working frequencies of the odometry strategies, the structure of the measurement matrix H k might change during each iteration and might be different for the parallel local filters. This issue has been solved in both the iterative and the parallel filtering approaches. In the sequential UKF, the number of performed correction steps is fixed by the available measurements, e.g., speed, depth, and position measurements. Accordingly, the H k structure is set for each filter iteration and its shape is determined by the presence of GPS and depth sensor measures and at least one speed vector. In the decentralized and federated UKFs, the measurement matrix H k can be different in each local filter due to the associated speed measurement availability. This condition, which can easily occur, does not influence the position filter behavior because the local filters do not exchange information between each other and because the information transfer from all the local filters to the master filter is achieved through the state estimates and their covariances that have the same fixed size in both the local and master filters.
The discrete-time system and measurement equations are given as follows: where it is necessary to again underline that while the state transition equation is nonlinear, the measurement one is linear with a H k matrix, composed of only zeros and ones, that maps the state vector into the measurement vector. The matrix R k depends on the actual dimension and composition of the measurement vector. This property is necessarily determined by the measurement data availability during the in-progress iteration.
Considering the presence of all the measurements that can be provided by the onboard sensors, the matrix R k can be defined as follows: where R GPS , R DS , R DVL , R CAM , and R FLS are, respectively, the measurement covariances related to GPS, depth sensor, DVL, camera, and FLS. Regarding the speed sources, it is finally necessary to highlight that the measurements are provided according to the sensor acquisition and elaboration rate and, for the odometry strategies, to the local environment properties. Due to these constraints, visual and acoustic odometries can, respectively, acquire images with a 10-and 2-Hz rate, but speed measurements are computed with a lower rate. In particular, while the AO can work with almost all the available acoustic images, the VO algorithm's maximum work frequency is approximately 4 Hz. Finally, the DVL measurements have been guaranteed with a 5-Hz rate.

C. Offline Validation
The presented navigation strategies have been tested and validated offline by employing experimental data recorded in Vulcano Island, Messina, Italy, in June 2019, during two autonomous underwater missions performed in the framework of the European project EUMarineRobots. The underwater mission was performed at a constant advance speed of 0.5 m/s with a fixed depth of 2.5 m and an altitude from the sea bottom  between 2 and 4 m. In both missions, all the payloads reported in Section VII-A were switched on and the vehicle, during its autonomous navigation along a preprogrammed path, acquired both acoustic and optical images. GPS readings were collected before FeelHippo AUV dove, and after it resurfaced, they have been employed as ground truth to compute the resurfacing error and to compare the proposed strategies. As reference path (RP), the UKF-based estimation, presented in [5] and [7], is employed. Of course, considering that some measurements are discarded, this RP cannot be employed as ground truth, but it can represent a reference whose navigation performance has to be overcome by the proposed novel approaches. The resurfacing error value, representing an estimation of the navigation drift, has been analyzed to assess the possible improvements of the presented strategies. Along the path, only the waypoints employed to plan the autonomous underwater mission can be used as a reference to evaluate the goodness of the navigation strategies.
Furthermore, to provide a RP estimation, a UKF that centrally employs all the available measurements has been tested. Considering that a centralized filter that uses, at the same time, all the available measurements provides the optimal estimate, the obtained trajectory can be considered as ground-truth.
The first autonomous mission lasted around 260 s covering approximately 100 m. From Fig. 24 and Table III, it is easily noticeable that all the proposed strategies increase the navigation estimation quality, analyzing the results from the GPS resurfacing error and the planned path error point of view. The path employed for the second autonomous mission is approximately 150-m long and it has been covered in about 390 s. As for the other mission, the proposed strategies led to an improvement of the navigation performance, which can be retrieved from Fig. 25 and Table IV.   To evaluate the agreement between estimation errors and estimated uncertainty, the 3σ bounds of the estimated trajectories are presented. Both the analyzed missions present the same structure. In the beginning, the vehicle is on the surface and GPS fixes bound the position terms of the state covariance. Subsequently, due to the absence of position measurements, the position terms of the state covariance increase until the vehicle resurfaces. When above the surface, GPS fixes are available again and the covariance gets smaller due to GPS corrections. This is summarized in Figs. 19-23, where, concerning the first autonomous mission, the 3σ bounds for the filter and the GPS are presented. In all the analyzed cases, the position provided by the filter (with its confidence bounds) appears to guarantee a reasonable prediction of the vehicle's true position when it resurfaces. As stated by the vendor [40], the employed GPS has an expected accuracy on the order of meters and the 2-D Fig. 22. On the left and in the center, the position estimation for mission #1 with its 3σ bound obtained with the sequential UKF algorithm, respectively, along the East and North directions. On the right, the bound of the last position under the sea surface and the first GPS fix measurement with its accuracy bound.  error can be represented as a 2-D Gaussian distribution whose components are independently distributed.
Finally, it is essential to evaluate the robustness of the proposed strategies against reducing the available measurements. In the previously presented results, reported in Figs. 24 and 25, all the available speed measurements have been passed to the navigation filters. A large amount of inertial, acoustic, and optical data necessarily determines the good performance of the employed navigation strategy, reducing the importance of the particular technique used to merge the measurements. To highlight the importance of a fusion-based filter as a compromise between reduced computational load and navigation robustness, the proposed UKF-based algorithms have been applied to two different subsets of the data acquired during the first mission; one reduced by 50% and the other one reduced by 75%. The selection of the measurements passed to the filters has been performed by applying a statistical selection function to each provided speed measurement. The choice has been based on the Bernoulli distribution test, whose probability mass function is reported in the following equation: where q represents the probability to get b = 1 or, equivalently, a true and 1 − q represents the probability to get b = 0 or, equivalently, a false in a double-choice experiment. In the following results, the coefficient q of the Bernoulli distribution has been set to three different values, namely q = 1, which provides the RP for the algorithm robustness analysis and whose results have been reported in Fig. 24, and q = 0.5 and q = 0.25, which impose the employment of a half and a quarter of the available measurements (Fig. 27). To give a quantitative evaluation of the robustness of the proposed algorithms, the navigation performance of the strategies applied to the reduced dataset has been compared to the one obtained with the full dataset. The employed metrics is defined in the following equations: where e i ∈ R + represents, at the instant i ∈ N, the navigation error, η RP 1,i and η TS 1,i denote, with respect to the NED reference system, the vehicle position according to the RP and to the current tested solution (TS), respectively. As introduced before, while the RP is represented by the estimated trajectory retrieved by applying one of the proposed strategies with a selection test with q = 1, the tested path is obtained by estimating the vehicle position with the same strategy of the RP but with a selection coefficient reduced to q = 0.5 or q = 0.25. In addition to this, e k ∈ R + denotes the mean of e i for i = 1, . . ., k.
Regarding the computational burden, the CPU burden and the execution time of the filters have been the subject of the analysis. It is worth highlighting that the simulations have been performed on laptop PC i7-9750H CPU@4.5 GHz with 16-GB RAM. Concerning the CPU burden, the output of the command top, which is a task manager program available in many Unix-like operating systems, has been recorded to store the data. Regarding the execution time, the sum of the requested time to perform both the prediction and the correction steps has been taken into account. The results can be found in Fig. 30 for what concerns the CPU burden and in Fig. 31 for the execution time.

D. Analysis of the Results
When compared to the reduced UKF, i.e., the filter currently on board FeelHippo AUV, the obtained results, due to the employment of a more significant number of speed measurements, shows an advantage in terms of navigation estimation quality (see Figs. 24 and 25 and Tables III and IV). Moreover, the obtained results from the proposed filters are comparable with the standard UKF, which centrally employs all the measurements  simultaneously. As a matter of fact, the iterative and parallel fusion strategies guarantee a solid tradeoff between localization accuracy and computational cost. More in detail, the sequential UKF strategy provides the minor resurfacing error and the best adherence to both missions' reference trajectories. Furthermore, this strategy guarantees the best adherence to the standard UKF estimated trajectory, which can be considered as the benchmark path. Concerning the federated and the consensus-based decentralized UKF, these strategies guarantee a performance increase with respect to the reduced UKF strategy, thanks to their better exploitation of the available measurements in a framework based on parallel local filtering.
As far as a comparison of the dataset analysis is concerned, the reduction of the navigation drift in the second mission with respect to the first one, despite the greater length of the route, can be related to the surrounding environment properties (see Figs. 26 and 28 and Table V). As a matter of fact, the presence of a more varied and textured environment is a significant advantage for VO/AO. Regarding robustness against the lack of measurements, trivially, the reduction of measurements leads to an increase in navigation error. However, the proposed strategies guarantee better robustness against these issues providing results comparable with the standard UKF. By observing Fig. 29, which are in the presence of 50% and 75% measurement reduction, it can be noted that both the decentralized and the centralized strategies are similarly insensitive to measurement reduction. Only the reduced UKF, due to its strategy based on the last acquired speed measurement in chronological order, is significantly negatively affected by the measurement reduction (see Table VI).
Concerning the computational burden, it can be noted that the sequential UKF has a CPU burden and an execution time similar to the reduced UKF. Still, it can provide a lower resurfacing error, which is comparable with the standard UKF. It is, therefore, possible to assert that the sequential UKF can represent a solid tradeoff between computational complexity and estimation performance. The proposed consensus-based decentralized UKF has the highest CPU burden and execution time due to the particular adopted fusion strategy. Finally, it is necessary to highlight that the federated UKF, despite it being a decentralized strategy, has a mean execution time slightly higher than the standard UKF. This last statement highlights the non-negligible burden requested by a centralized strategy that, at the same time, processes all the available measurements.
Finally, it is necessary to summarize the pros and cons of each filtering strategy proposed to overcome the limitations of the reduced UKF. The reduced UKF, despite it requiring the lowest CPU burden and execution time, provides estimated trajectories whose estimation errors tend to diverge faster than the other filters. Furthermore, this strategy is susceptible to the lack of measurements, which can be limited when they are obtained from payload sensors that acquire data from the surrounding environment. Consequently, the obtained results can be summarized in the following sentences.
1) The standard UKF centrally fuses, at the same time, all the available measurements and it provides in both missions the lowest resurfacing error. It requests a computational  burden higher than the other centralized filters, i.e., sequential UKF and reduced UKF, but similar or lower than the decentralized strategies.
2) The sequential UKF can be the best solution to guarantee a solid tradeoff between an increase of estimation properties and a reduction of computational load, which can be one of the major limitations in mobile robotics. Reducing the available measurements, the sequential UKF remains much better than the reduced UKF, but it provides the worst results if compared with the other filters.
3) The consensus-based decentralized UKF provides the lowest divergence of the estimated covariance when the vehicle is under the sea surface and no position measurements are available. As illustrated by the Monte Carlo simulations, it is possible to highlight that the filter provides the best results from the consistency point of view and that the position 3σ bound contains the position estimation errors. On the contrary, the employed fusion strategy requires a non-negligible execution time and CPU burden, which could limit its applications when the hardware is not adapted. 4) The federated UKF is a decentralized strategy with a requested execution time comparable with the standard UKF. From the results obtained by reducing the measurement availability, the federated UKF is the most insensitive strategy to the lack of measurements.

VIII. CONCLUSION
This article shows and compares a set of novel UKF-based sensor fusion strategies for autonomous underwater navigation. Increasing the navigation performance is a complex but essential target to be achieved, because it guarantees that the AUV can correctly perform its mission and accomplish its tasks. The proposed strategies have been adapted to be inserted in an UKF-based framework and tested on a real dataset acquired at field during an autonomous underwater mission performed in Vulcano Island, Messina, Italy. To the authors' best knowledge, the fusion of inertial, acoustic, and optical data in an UKF algorithm and the application of the presented sensor fusion strategies to AUV navigation is novel. To increase the reliability of the proposed techniques, they have been tested on two different sets of data, leading to common results and conclusions. The centralized strategy, as the sequential UKF, guarantees the best improvements in terms of estimation quality, which can be retrieved by analyzing the resurfacing error and the adherence to the reference input trajectory. Decentralized strategies provide an increase of robustness against measurement reduction. Both consensus-based decentralized and federated UKFs, thanks to a parallel local filtering approach, are less affected by the lack of measurements than the centralized algorithms.
The obtained results are promising for mobile robotics applications, where providing robust and accurate navigation but guaranteeing a reduced computational cost is fundamental due to the limitations concerning dimensions, weight, and embedded computational power. Consequently, the proposed strategies can represent a noteworthy tradeoff toward the improvement of both navigation accuracy and robustness, while taking into account the low-computational load requirement.
Nonetheless, further progress still needs to be made. The proposed strategies need to be implemented and tested on-board the vehicle during an underwater mission. This will be an important test to practically evaluate the reductions of computational cost. Moreover, the integration of the visual and acoustic measurements within the attitude estimator could represent another subject to be investigated.