A Novel Factor Graph and Cubature Kalman Filter Integrated Algorithm for Single-Transponder-Aided Cooperative Localization

Cooperative localization (CL) of underwater multi-AUVs is vital for numerous underwater operations. Single-transponder-aided cooperative localization (STCL) is regarded as a promising scheme for multi-AUVs CL, benefiting from the fact that an accurate reference is adopted. To improve the positioning accuracy and robustness of STCL, a novel Factor Graph and Cubature Kalman Filter (FGCKF)-integrated algorithm is proposed in this paper. In the proposed FGCKF, historical information can be efficiently used in measurement updating to overcome uncertain observation environments, which greatly helps to improve the performance of filtering progress. Furthermore, Adaptive CKF, sum product, and Maximum Correntropy Criterion (MCC) methods are designed to deal with outliers of acoustic transmission delay, sound velocity, and motion velocity, respectively. Simulations and experiments are conducted, and it is verified that the proposed FGCKF algorithm can improve positioning accuracy and robustness greatly than traditional filtering methods.


Introduction
Multi-AUVs are widely used in underwater operations, which has become a significant trend for environment and oceanography research to improve operation efficiency and diversity concurrently [1]. Cooperative localization (CL) of multi-AUVs is an efficient technology in deep-sea areas, especially where Global Navigation Satellite System (GNSS) is unavailable due to the attenuation of wireless signal. Generally, navigation aid is necessary for CL systems to provide exact position information for revising system deviation. Autonomous Surface Vehicle (ASV) and surface buoy are common navigation aids [2]; however, these surface units are affected by ocean currents, as well as lack of crypticity. In this paper, a single transponder is adopted as the navigation aid [3], which is deployed and calibrated at the seafloor in advance.
One of the core problems is the positioning algorithm in CL. There have been many algorithms proposed to realize underwater multi-AUVs CL. Least square (LS) is a typical CL algorithm, including square-range LS with Gaussian variational message passing [4], LS combining rank-relaxed semidefinite programming and maximum likelihood estimation [5], and iterative LS estimators [6]. LS focuses on dealing with a multilateral positioning method, which is not suitable for single-transponder-aided CL environments. Some other methods have been studied; for example, an iterative greedy algorithm has been proposed for collaborating underwater vehicles tasks [7]. In absolute terms, Bayesian filters provide more ideas to cope with CL problem, and a great number of algorithms based on Bayesian have been explored, such as Moving Horizon Estimation (MHE) integrated with Extended algorithm is described to improve accuracy of ranging measurements. An MCC based reconstruction method is adopted to overcome the uncertainty problem of motion velocity.
The rest of this paper is organized as follows. Section 2 presents the framework of STCL. The proposed FGCKF is described in detail in Section 3. In Section 4, simulations and experiments are conducted. Finally, conclusions are given in Section 5.

Framework of STCL
Generally speaking, there are two modes of multi-AUVs CL technology, one is the master-slave mode, and the other is parallel mode. Benefiting from saving positioning equipment and navigation cost, the master-slave mode is attracting more and more attention. In master-slave mode, the main AUV, which can position itself with high-accuracy navigation equipment, is considered the master AUV. Other AUVs that reach their locations using the master AUV are named slave AUVs. Ranging and location information are transmitted by acoustic communication links between master and slave.
In traditional master-slave CL, a master AUV, equipped with high-accuracy Inertial Navigation System (INS) and GNSS, needs to rise to sea surface to update global location periodically. Even if it avoids rising to the surface, a surface unit, such as an ASV or buoy, must be present as a navigation aid. However, when multi-AUVs operate in a deep sea environment, it is a time-consuming and energy-intensive process for the master to rise to the sea surface; meanwhile, it is hard to build steady communication link between surface unit and deep sea master. In this paper, a novel Single Transponder aid CL (STCL) scheme is designed. Unlike traditional the acoustic technology Long Base-line (LBL) positioning system [34], where at least three transponders need to be deployed and calibrated at the seabed or lakebed in advance, only one transponder is adopted in STCL. Although it is difficult to calculate accurate locations using one series of acoustic transmission delays, it can play an efficient supporting role to aid CL. As the water depths vary greatly in different environments, it will lead a significant error for ranging measuring, which will be solved by an ESV estimating method detailed in next Section. The framework of STCL is shown in Figure 1. In this STCL framework, all multi-AUVs can achieve ranging information between AUV and a single transponder by periodic acoustic impulse response. The master AUV is equipped with high-accuracy INS, which can provide accurate motion-velocity information, and hence the master can be positioned, merging velocity information and single transponder ranging information precisely. However, taking the cost into consideration, slave AUVs are equipped with Doppler Velocity Log (DVL) instead of INS. The results is that only a rough motion velocity can be achieved even with certain outliers. This presents a prime challenge to STCL, namely how to position slave AUVs by making full use of master-slave communication link, master ranging, single transponder ranging, rough motion velocity, etc.
To solve the main problem in STCL, a factor graph and CKF integrated algorithm is proposed and shown in detail in the following section.

The Proposed FGCKF Algorithm
Considering the STCL as a discrete time system, we define a state vector at time k to describe the slave AUV dynamic state as where x k , y k , z k denote the AUVs three-dimensional coordinates, v k and v e,k present motion velocity and sound velocity, respectively. θ k indicates heading direction. State equation and measurement equation are built as follows. x where f (·) and h(·) are nonlinear transition functions for state and measurement updating. z k represents the measurement vector. w k ∼ N(0, Q k ), δ k ∼ N(0, R k ) indicate state noise and measurement noise, respectively. Q k and R k denote the noise covariances of w k and δ k .
In the framework of STCK, the state transition function and measurement transition function are as follows.
where t is time interval. w v k and w θ k represent process noise of v and θ, respectively. g(·) represents the relationship between sound velocity and depth. τ b,k presents transmission delay between slave and single transponder, τ l,k presents transmission delay between slave and master. (x b , y b , z b ) is single transponder location, and (x l,k , y l,k , z l,k ) is master location.
The model of Kalman filter can be written as conditional probability density function f (x 1 , · · · , x k |z 1 , · · · , z k ), in which x k is determined by x k−1 and z k , satisfying hidden Markov model condition. Further the function can be defined as: The state and measurement variables are assumed to satisfy Gaussian distribution [35]; hence the state prediction Probability Density Function and measurement transition Probability Density Function are described as follows.
To make a better state information fusing, a factor graph is employed in STCL. A factor graph (FG) is a kind of bi-directional graph for information transfer that can cope with complex multivariate global functions. Moreover, FG is strong in plug-and-play and asynchronous fusion, which is valuable applying in STCL.
There are two category nodes in FG, including variable node and function node. More specifically, it graphically represents the factorization structure of global function as the product of local functions. Messages are transmitted following the sum-product in FG. At time slot k, the message from state node x k−1 and measurement node z k are fused on node x k .
where m k|k is equal to x k . Furthermore, based on Equations (9) and (10), the Cubature Kalman Filter (CKF) is adopted to finish updating of state information.
CKF depends on transforming a set of cubature points using state and measurement models to realize nonlinear estimation. The key to propagate the cubature points is the Cholesky decomposition of error covariance. The following equations show how to use cubature points to calculate the mean and variance of the state.
where P k−1 and P k|k−1 denote error covariance and predicted error covariance at time k − 1, respectively. S k−1 presents Cholesky decomposition result. x i,k|k−1 indicates propagating cubature points. x k|k−1 is the predicted state. The set of points ξ i adopts √ n [1] i . Cubature points are also propagated to accomplish the estimation summarized as follows.
where z i,k|k−1 is cubature points of observation, z k|k−1 is predicted measurement vector, P z is autocorrelation covariance matrix, P xz is cross-correlation covariance matrix, and K is Kalman gain. Based on Equations (16)-(24), the state vector and error covariance are updated. The fusion result is achieved by certain iterations. CKF utilizes cubature points to update the state, which makes the system mean and variance reach the third-order accuracy. Compared with EKF in first-order accuracy, it can improve the updating accuracy greatly.
As mentioned above, the state evolves compared to the former state at time k − 1 and current measurement at time k. However, there is a large amount of historical measuring information ignored in the filtering process. It is known that historical information is efficiently used by message passing in the FG framework. Therefore, the message passing mode in FGCKF is designed, and its procedure is shown in Figure 2. In Figure 2, x k , x k−1 represent state nodes in time k and time k − 1, respectively. z 1 indicates measuring transmission delay vector between slave and transponder, and z 2 indicates measuring transmission delay vector between slave and master. f x denotes state prediction node. h z1 and h z2 present observation functions of z 1 and z 2 . d is set to deal with observation residuals. The blue arrows represent the messages sent out by state node x, and the blue arrows represent the messages sent into state node x.
In the message passing of transmission delay, there exists certain noises of measurement error. Let τ present transmission delay, and τ b , τ l denote transmission delay from single transponder and master AUV, respectively. Besides the direct measurement variable τ m , transmission delay can also be calculated in the former process of CKF, expressed as τ c .
During every update step in CKF, a real-time position of AUV ( where v e indicates sound velocity. A delay error value γ is designed based on the difference between τ c and τ m , Then the delay error vector γ γ γ = [γ 1 , · · · , γ k−1 , γ k ] is built, whose variance can represent the measurement error magnitude. We use γ γ γ b to represent measurement error value related to transponder and γ γ γ l to represent measurement error value related to master AUV. The mean and variance of γ γ γ b andγ γ γ l can be calculated at time k as follows.
Two sets of mean and variance of error value γ can be achieved based on τ b and τ l , namelȳ γ b ,σ γ,b andγ l ,σ γ,l . Based on the sum product rule, this information is fused using the Gaussian product rule, which is shown as follows.
Based on the fused variance Σ 2 γ , vector d is designed to adjust measurement value. The transmission delay τ k+1 in time k + 1 is defined as where k c is the adjustment threshold, while k ≤ k c , Σ γ is considered not valuable enough to be the adjustment pattern of τ.

Adaptive CKF
As mentioned in Section 3.1, measurement noise matrix R of acoustic transmission delay influences Kalman gain, which determines the proportion of trust between observation variable and state transferring variable. When there exists outliers of transmission delay, the calculation of Kalman gain will deteriorate. In this section, an Adaptive CKF (ACKF) is designed to resist delay outliers by renewing measurement noise matrix R. The details are shown in the following.
Firstly, an adaptive scaling factor α is introduced. The measurement noise matrix R k in time k is expressed as By adaptive changing scaling factor α, it is enabled to adjust the system noise covariance matrix for online tuning of the predicted state covariance. When meeting the high precision measurement, the weight of measurement in the state estimation is enhanced. Otherwise, the weight of low precision measurement is decreased. A screening model is established to classify the observation residual, which is analogous to the IGG III equivalent weight function model [36].
where β 0 and β 1 are screening standard parameters and s v is normalized residual. Let k = z k − h(x k|k−1 ) be the measurement residual; s v is expressed as where σ k is variance factor obtained by median(s i )/0.6745. s i is the set of all residual values. Variance factor is mainly used to standardize residual, which is related to the detection and discrimination of errors. Moreover, β 0 and β 1 are usually regarded as constant experienced values. Based on traditional practical experience, β 0 adopts 1.5-2.0, and β 1 adopts 3.0-8.5. Considering complex underwater environment, we set β 1 to be alterable as follows: whereŝ i indicates the set of residual values without outliers. When there are more samples participated into the adaptive changing process, the alternative β 1 will be more suitable for enhancing accuracy of scaling factor.

Sum-Product Based ESV
Sound velocity (SV) is a vital element for estimation of acoustic range. However, SV changes in various ways following the changing environments. Achieving an Effective Sound Velocity (ESV) is an essential part of FGCKF in STCL. In this section, a sum-productbased ESV calculating method is proposed.
Assuming that ESV is v e,k at time slot k, the posterior probability of state variable related to ESV can be given by In the factor graph of FGCKF, the prediction message is shown as The message of current state can be updated by and the message of current ESV is expressed as µ(v − e,k ).
Function node g is defined as where x b presents location state of single transponder. Based on principle of the sumproduct algorithm, the message of current ESV is updated further. Figure 3 gives the existing method of sum-product-based ESV in the FGCKF procedure. Benefiting from the continuous renewal of ESV, the accuracy of ranging estimation can be improved greatly.

MCC-Based Velocity Uncertainty
Motion velocity is significant information in slave localization in STCL. However, motion velocity cannot be measured very accurately using DVL, even with many outliers, which we call Velocity Uncertainty (VU). In this section, a Maximum Correntropy Criterion (MCC) method is adopted to solve the VU problem.
Correntropy is a generalized similarity measure between two random variables. V(x, y) represents correntropy between variable x and y, defined as where Ξ denotes the expectation operator, F xy (x, y) is joint distribution function, and κ denotes a shift-variant kernel. In this paper, the kernel function adopts a Gaussian kernel for the noise is assumed as Gaussian random noise, shown as where σ > 0 is the kernel bandwidth. The first step is to revise the velocity. The model can be expressed by the follow- where η is Gaussian noise, which is subjected to N(0, Φ), v − k is the prediction of velocity, and v k−1 represents the optimal estimate of velocity. Here the maximum correntropy is used as the cost function.
where J represents correntropy of the velocity, v m k is measured velocity which is obtained by DVL. Because of great VU, previous state and measurement are both used for velocity estimation. The optimal velocity is the one that makes the correntropy J reaches maximum. Furthermore, the partial derivatives is used for the optimal solution. The optimal v satisfies The outliers of motion velocity can be restrained efficiently using the MCC-based method.

Flow-Chart of FGCKF
By combining Adaptive CKF, Sum-product based ESV, MCC-based Velocity Uncertainty together, the proposed FGCKF algorithm is extended to be more complete and robust. The algorithm flowchart of FGCKF is summarized in Algorithm 1.

Simulations and Experiments
In this section, both simulations and field experiments are conducted to verify the performance of the proposed scheme.

Simulation Results
We developed a cooperative localization simulation platform in Matlab, in which a single transponder is fixed in a position, and a master AUV and a slave AUV are deployed in this simulation scenes and move with virtual velocity and heading information. The sampling rates of AUVs and transponder are both set as 1 Hz. The simulations are all modeled under three dimension circumstance, and positioning error is calculated by Error = (x − x r ) 2 + (y − y r ) 2 + (z − z r ) 2 . Since the depth information z can be measured accurately by pressure sensor in actual operation, simulations focus principally on horizontal coordinates x, y. The simulations are conducted in three parts, namely performance analysis of ACKF, performance analysis of ESV, and performance analysis of integrated FGCKF.

Performance Analysis of ACKF
The first simulation is done to evaluate the robustness of ACKF. The sound velocity is set to be a constant as 1500 m/s. The single transponder is located at (100, −50, −100). The master slave moves with known trajectory coordinates. The positioned slave trajectory is compared with the preplanned trajectory (real trajectory) to calculate positioning error. In the ranging estimation, two parts of big noise are added artificially to represent the outliers of the acoustic transmission delay. Figure 4 gives the comparison of positioning trajectory and positioning error of ACKF and CKF, respectively.  From Figure 4a,b, it can be seen that ACKF performs better than CKF when facing unknown measurement noise. ACKF performs well in meeting the problem of measurement outliers, which indicates that ACKF improves the algorithm robustness significantly.

Performance Analysis of ESV
Normally, sound velocity is set as a constant such as 1500 m/s. However, sound velocity varies, especially with changing depth, which will influence CL greatly. In this simulation, we use a sound velocity profile measured in the South China Sea to simulate different sound velocity at different depths, and four different sound velocities are chosen to compare with the proposed sum-product-based ESV. The basic simulation condition is the same as Performance Analysis of ACKF. The comparison results of various constant sound velocities and ESVs can be seen in Figure 5. It is clear that though the position error of constant sound velocity is sometimes lower than ESV, ESV performs much better in general. As a result, the CL benefits from the ESV, and the positioning accuracy is obviously improved. This encourages us to further investigate the features of ESV in the future.

Performance Analysis of Integrated FGCKF
In this section, the performance of the proposed integrated FGCKF is verified by comparing it with EKF and CKF. A single transponder is deployed at (400, −100, −100). Figure 6 shows the simulation result of FGCKF. From Figure 6a, it can be seen that the positioning trajectory of the proposed FGCKF is much closer to the real trajectory than CKF and EKF. Figure 6b shows that the average positioning error of FGCKF is the smallest among three methods. From this simulation, it can be concluded that the proposed FGCKF algorithm can efficiently improve positioning accuracy of STCL.   Furthermore, to evaluate the robustness of the proposed method, outliers of measurements, including transmission delay and motion velocity, are joined in this simulation. The outliers related simulation result is shown in Figure 7. It is clear that the positioning trajectory and positioning error of EKF and CKF make mutations when measurements meet outliers. Fortunately, the proposed FGCKF performs much better with obvious mutations with no matter positioning trajectory or positioning errors. From this simulation, it can be concluded that the proposed FGCKF improves robustness of STCL effectively.
Besides the wave-like trajectory used in former simulations, three typical motion trajectories are considered, including straight-line trajectory, comb-type trajectory, and circular trajectory. Figure 8 describes the performance of FGCKF, CKF, and EKF with different motion trajectories. It can be seen that for strong linearity trajectories, including straight-line trajectory and comb-type trajectory, there is no obvious difference between CKF and EKF, and FGCKF performs much better than others. For non-linearity trajectories, including wave-like trajectory and circular trajectory, EKF performs worst, FGCKF and CKF perform better than EKF, and FGCKF is better than CKF.
Based on the above simulation results, it can be concluded that the proposed FGCKF can not only improve positioning accuracy but also effectively enhance robustness of STCL.

Field Data
In this section, the performance of FGCKF is verified by field data, which were collected using a lake experiment. The single transponder was deployed in the bottom of lake at (−65.7, −98.4, −64.5) in a local coordinate system. Furthermore, two AUVs were employed, where one served as the master and the other one as the slave. Both master and slave are equipped with acoustic transmitter and receiver, which help to realize ranging measuring between AUVs and transponder. The time interval of acoustic transmission delay gathering is 2 s. The slave AUV moved in a circular shape to imitate actual operations. Moreover, slave and master were both equipped with GPS to collect the true positions. The slave was equipped with a DVL to measure motion velocity information and a compass for heading information. Two different tests were conducted, including motionless master AUV and moving master AUV. On the condition of moving master AUV, it moved in a straight line at a constant velocity.
The test results are shown in Figures 9 and 10. From the tests, it can be seen that FGCKF and CKF maintain low errors most of the time, while FGCKF has lower errors than CKF. In particular, when encountering sharp measurement noise, CKF produces larger position errors, and FGCKF maintains positioning results stable, benefiting from its robustness.
From both simulation and experimental results, it can be seen that the proposed FGCKF can ensure accuracy and robustness concurrently in STCL.

Conclusions
In this paper, a novel factor graph and cubature Kalman filter-integrated algorithm is proposed for a single-transponder-aided cooperative localization underwater framework in master-slave AUV mode. In the proposed FGCKF, an adaptive CKF is presented in which adaptive measurement noise matrix is designed to overcome outliers of acoustic transmission delay. Traditional static sound velocity is replaced by sum-product-based ESV to make full use of historical sound velocity information in a factor graph. The outliers of the rough motion velocity measured by DVL in the slave AUV are restrained by the MCC method. Both simulation and experiment were conducted to verify the efficient performance of the proposed FGCKF, which lays a good foundation for future study of the positioning accuracy and robustness of FGCKF in STCL environments.