Next Article in Journal
Dynamic Effects in Nucleation of Receptor Clusters
Previous Article in Journal
Interactive Multiobjective Procedure for Mixed Problems and Its Application to Capacity Planning
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

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

1
School of Information Science and Engineering, Harbin Institute of Technology, Weihai 264209, China
2
College of Underwater Acoustic Engineering, Harbin Engineering University, Harbin 150001, China
3
School of Information and Communication Engineering, Dalian University of Technology, Dalian 116081, China
*
Authors to whom correspondence should be addressed.
Submission received: 3 September 2021 / Revised: 19 September 2021 / Accepted: 22 September 2021 / Published: 24 September 2021

Abstract

:
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.

1. 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 Kalman Filter (EKF) [8], EKF with measurement rough estimation algorithm [9], passive-tracking based on EKF [10], EKF based belief propagation [11], Multi-Hypothesis Extended Kalman Filter (MHEKF) [12], Particle Filtering (PF) [13], and Optimal Bayesian Kalman filter [14]. Among these methods, Cubature Kalman Filter (CKF) has a specific advantage in nonlinear estimation problems [15] and is a promising scheme in CL. Though CKF performs well in nonlinear estimation accuracy, it lacks system robustness when applied in CL. Actually, filters can improve its adaptation for measurement outliers to improve algorithm robustness. Reference [16] considered measurement noise as a heavy-tailed mixture (HTM) distribution and a Normal-Gamma-inverse Wishart distribution. Student’s t-based EKF algorithm [17] and Expectation Maximization (EM) [18] show an improved robustness against outliers found in the process and measurement noises, respectively. There have been some adaptive methods proposed to maintain filters’ Gaussian feature. The authors [19] presented an Adaptive Extended Kalman Filter (AEKF) based on the EM method. An adaptive filter using a forgetting factor has been presented to calculate noise matrix [20]. A measurement information detection based on Adaptive Neuro Fuzzy Inference System (ANFIS) is designed in [21]. In view of the previous scholarly studies, an Adaptive Cubature Kalman Filter (ACKF) is proposed in this paper, aiming to improve the robustness of CL method.
One disadvantage is that only current measurement information is used in filter methods. Graph optimization theory can be an ideal integrated scheme for filter in CL, benefiting from its combined use of historical information. Factor Graph (FG) is a promising graph optimization method, and sum-product algorithm is the core supporting to deal with message passing [22,23]. There have been a wide variety of algorithms proposed related to sum-product, including artificial intelligence, signal processing, digital communications, and positioning and navigation. A factor-graph and sum-product-based cooperative positioning algorithm by local function estimation is proposed in the literature [24]. Sun et al. proposed a CL algorithm based on a hybrid topology architecture using relative measurement graph method [25]. In the literature [26], Gaussian parameterizations of messages are combined in the framework of factor graph. Factor graph can merge different available asynchronous sensor information efficiently and accurately [27] and can be readily modified by adding corresponding factor nodes and edges [28]. Considering the nonlinear filtering capacity of CKF and efficient utilization of historical information in distributed topology of factor graph, a Factor Graph and Cubature Kalman Filter (FGCKF)-integrated algorithm is proposed in this paper, which aims to improve the positioning accuracy and promote in single-transponder-aided Cooperative Localization (STCL) of underwater multi-AUVs operation.
In the proposed FGCKF in STCL, outliers of multidimensional information are considered, including acoustic transmission delay, sound velocity, and motion velocity, etc. ACKF is supposed to overcome outliers of transmission delay. As for the outliers of sound velocity, an estimating method for sum-product based Effective Sound Velocity (ESV) [29] is designed. Based on historical factor graph list, ESV can be updated with transmission delay simultaneously and can further be treated as a random variable participating in the filtering process in ACKF. Motion velocity of AUV can also contain outliers during long time operation in a deep-sea environment. Though current velocity is believed to revise accurate motion velocity by some scholars [30,31], the building and updating of the current velocity model is very difficult. Maximum Correntropy Criterion (MCC) has recently been successfully applied in robust regression and filtering [32] and active noise control [33]. In FGCKF, MCC is adopted to deal with the outlier problem of motion velocity.
The contributions of this paper are as follows. A factor graph and CKF-integrated algorithm is proposed in single-transponder-aided Cooperative Localization in underwater multi-AUVs operations. It aims not only to improve the positioning accuracy but also to enhance the robustness greatly in CL schemes. Furthermore, in the proposed FGCKF, an Adaptive CKF method is presented to focus on the adjustment of the measurement covariance matrix for acoustic transmission delay. ESV estimation based on a sum product 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.

2. 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.

3. FGCKF in STCL

3.1. 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
x k = [ x k , y k , z k , v k , θ k , v e , k ] T
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 k = f ( x k 1 ) + w k
z k = h ( x k ) + δ k
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.
f = x k = x k 1 + v k · t · cos θ k y k = y k 1 + v k · t · sin θ k z k = z k v k = v k 1 + w k v θ k = θ k 1 + w k θ v e , k = g ( z k ) ;
h = τ b , k = ( x k x b ) 2 + ( y k y b ) 2 + ( z k z b ) 2 v e , k τ l , k = ( x k x l , k ) 2 + ( y k y l , k ) 2 + ( z k z l , k ) 2 v e , k
where t is time interval. w k v 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:
f ( x 1 , , x k | z 1 , , z k ) = i = 1 k f ( x i | x i 1 ) f ( z i | x i )
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.
f ( x i | x i 1 ) = N ( f ( x i 1 ) , Q i )
f ( z i | x i ) = N ( h ( x i ) , R i )
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 .
μ k | k 1 ( x k 1 ) = μ k 1 | k 1 ( x k 1 ) f ( x k | x k 1 ) d x k 1   = μ k 1 | k 1 ( x k 1 ) N ( f ( x k 1 ) , Q k )   N ( m k | k 1 , P k | k 1 )
μ k | k ( x k ) = μ k | k 1 ( x k ) f ( z k | x k )   = N ( m k | k 1 , P k | k 1 ) N ( h ( x k ) , R k )   N ( m k | k , P k | 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.
P k 1 = S k 1 S k 1 T
x i , k 1 = S k 1 ξ i + x k 1 i = 1 , , 2 n
x i , k | k 1 = f ( x i , k 1 )
x k | k 1 = 1 2 n i = 1 2 n x i , k | k 1
P k | k 1 = 1 2 n i = 1 2 n x i , k | k 1 x i , k | k 1 T x k | k 1 x k | k 1 T + Q k
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.
P k | k 1 = S k | k 1 S k | k 1 T
x i , k | k 1 = S k | k 1 ξ i + x k | k 1 i = 1 , , 2 n
z i , k | k 1 = h ( x i , k | k 1 )
z k | k 1 = 1 2 n i = 1 2 n z i , k | k 1
P z , k | k 1 = 1 2 n i = 1 2 n z i , k | k 1 z i , k | k 1 T z k | k 1 z k | k 1 T + R k
P x z , k | k 1 = 1 2 n i = 1 2 n x i , k | k 1 z i , k | k 1 T x k | k 1 z k | k 1 T
K k = P x z , k | k 1 P z , k | k 1 1
x k = x k | k 1 + K k ( z k z k | k 1 )
P k = P k | k 1 K k P z , k | k 1 K k T
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 x z 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 z 1 and h z 2 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 ( X k , Y k , Z k ) is obtained. Assuming location of transponder is ( X b , Y b , Z b ) , location of master AUV is ( X l , Y l , Z l ) , and then τ c in time k is expressed as
τ c , k b = ( X k X b ) 2 + ( Y k Y b ) 2 + ( Z k Z b ) 2 v e τ c , k l = ( X k X l , k ) 2 + ( Y k Y l , k ) 2 + ( Z k Z l , k ) 2 v e
where v e indicates sound velocity. A delay error value γ is designed based on the difference between τ c and τ m ,
γ k = | τ m , k τ c , k | ,
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.
γ ¯ = 1 k i = 1 k γ i
σ γ = 1 k 1 i = 1 k ( γ i γ ¯ ) 2
Two sets of mean and variance of error value γ can be achieved based on τ b and τ l , namely γ ¯ 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.
1 Σ γ 2 = 1 σ γ , b 2 + 1 σ γ , l 2
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
τ k + 1 = τ m , k + 1 d k d k = Σ γ k > k c 0 k k c
where k c is the adjustment threshold, while k k c , Σ γ is considered not valuable enough to be the adjustment pattern of τ .

3.2. 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
R k = R k 1 / α
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].
α = 1 s v β 0 β 0 s v β 1 s v β 1 β 0 β 0 < s v < β 1 10 10 s v β 1 ,
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
s v = ϵ k σ k ,
where σ k is variance factor obtained by m e d i a n ( 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:
β 1 = m a x ( | s ^ i | ) / m e d i a n ( s i ) / 0.6745 ,
where s ^ 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.

3.3. 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-product-based 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
p ( x k , v e , k | z k ) ( x k | x k 1 , v e , k 1 ) p ( v e , k | v e , k 1 ) p ( z k | x k 1 )
In the factor graph of FGCKF, the prediction message is shown as
μ ( x k | x k 1 ) = f ( x k | x k 1 , v e , k 1 ) μ ( x k 1 ) μ ( v e , k 1 )
The message of current state can be updated by
μ ( x k ) = f ( z k | x k 1 ) μ ( x k | x k 1 )
and the message of current ESV is expressed as μ ( v e , k ) .
μ ( v e , k ) = g ( v e , k | x k ) μ ( x k )
Function node g is defined as
g ( v e , k | x k ) = x k x b τ k
where x b presents location state of single transponder. Based on principle of the sum-product algorithm, the message of current ESV is updated further.
μ ( v e , k ) = μ ( v e , k 1 ) μ ( v e , k )
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.

3.4. 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
V ( x , y ) = Ξ [ κ ( x , y ) ] = κ ( x , y ) d F x y ( x , y ) ,
where Ξ denotes the expectation operator, F x y ( 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
κ ( x , y ) = G σ ( x y ) = G σ ( e ) = 1 2 π σ exp ( e 2 2 σ 2 ) ,
where σ > 0 is the kernel bandwidth.
The first step is to revise the velocity. The model can be expressed by the following equation,
v k = v ^ k 1 + η
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.
J = G σ ( v k v k ) + G σ ( v k v k m )
where J represents correntropy of the velocity, v k m 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
J v k = ( G σ ( v k v k ) + G σ ( v k v k m ) ) v k   = ( 1 2 π σ ( exp ( ( v k v k ) 2 2 σ 2 ) + exp ( ( v k v k m ) 2 2 σ 2 ) ) ) v k   = 0
v ^ k = v k + ( v k m v k ) exp ( ( v k v k m ) 2 2 σ 2 ) / exp ( ( v k v k ) 2 2 σ 2 ) =   v k + ( v k m v k ) exp ( ( v k v k m ) 2 2 σ 2 )
The outliers of motion velocity can be restrained efficiently using the MCC-based method.

3.5. 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.
Algorithm 1 Flowchart of proposed FGCKF in STCL.
Imput: The initial state x 0 and P 0 , time delay τ b , 1 : k and τ l , 1 : k and position of transponder and leader ( x b , y b , z b ) and ( x l , y l , z l ) 1 : k
Output: The estimation x 1 : k and P 1 : k
 1: for i = 1:k do
 2: Calculate the velocity in Equation (46)
 3: Calculate prediction state using Equations (11)–(15)
 4: Revise measurement using Equation (30)
 5: Calculate measurement noise covariance in Equations (31)–(34)
 6: Calculate update state by Equations (16)–(24)
 7: Estimate sound velocity using Equations (39) and (40)
 8: Calculate measurement residual in Equations (25)–(29)
 9: end for
10: return x 1 : k and P 1 : k

4. Simulations and Experiments

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

4.1. 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 E r r o r = ( 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.

4.1.1. 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.

4.1.2. 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.

4.1.3. 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.

4.2. 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 Figure 9 and Figure 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.

5. 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.

Author Contributions

Conceptualization, W.Z.; data curation, H.Z.; formal analysis, L.L.; project administration, D.Z.; validation, D.Z.; investigation, H.Z.; writing—original draft, W.Z. All authors have read and agreed to the published version of the manuscript.

Funding

This work was supported partly by National Natural Science Foundation of China (Grant No. 62101158) and National Natural Science Foundation of China (Grant No. 62171075).

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Simetti, E.; Indiveri, G.; Pascoal, A.M. Wimust: A cooperative marine robotic system for autonomous geotechnical surveys. J. Field Robot. 2021, 38, 268–288. [Google Scholar] [CrossRef]
  2. Willners, J.S.; Toohey, L.; Petillot, Y.R. Sampling-Based Path Planning for Cooperative Autonomous Maritime Vehicles to Reduce Uncertainty in Range-Only Localization. IEEE Robot. Autom. Lett. 2019, 4, 3987–3994. [Google Scholar] [CrossRef]
  3. Zhao, W.; Zhao, H.; Liu, L. A Single Beacon-aided Cooperative Localization Algorithm based on Maximum Correntropy Criterion. In Proceedings of the 2021 International Wireless Communications and Mobile Computing (IWCMC), Harbin, China, 28 June–2 July 2021; pp. 1835–1839. [Google Scholar]
  4. Nguyen, V.T.; Jeong, Y.; Shin, H.; Win, M. Least Square Cooperative Localization. IEEE Trans. Veh. Technol. 2015, 64, 1318–1330. [Google Scholar] [CrossRef]
  5. Russell, J.S.; Ye, M.; Anderson, B.D.O.; Hmam, H.; Sarunic, P. Cooperative Localization of a GPS-Denied UAV Using Direction-of-Arrival Measurements. IEEE Trans. Aerosp. Electron. Syst. 2020, 56, 1966–1978. [Google Scholar] [CrossRef] [Green Version]
  6. Yan, J.; Zhang, X.; Luo, X.; Wang, Y.; Chen, C.; Guan, X. Asynchronous Localization with Mobility Prediction for Underwater Acoustic Sensor Networks. IEEE Trans. Veh. Technol. 2018, 67, 2543–2556. [Google Scholar] [CrossRef]
  7. Kuhlman, M.J.; Jones, D.; Sofge, D.A.; Hollinger, G.A.; Gupta, S.K. Collaborating Underwater Vehicles Conducting Large-Scale Geospatial Tasks. IEEE J. Ocean. Eng. 2021, 46, 785–807. [Google Scholar] [CrossRef]
  8. Wang, S.; Chen, L.; Gu, D.; Hu, H. Cooperative localization of AUVs using moving horizon estimation. IEEE/CAA J. Autom. Sin. 2014, 1, 68–76. [Google Scholar]
  9. Lu, J.; Chen, X.; Luo, M.; Zhou, Y. Cooperative localization for multiple AUVs based on the rough estimation of the measurements. Appl. Soft Comput. 2020, 91, 106197. [Google Scholar] [CrossRef]
  10. Liao, Y.; Xie, J.; Wang, Z.; Shen, X. Multisensor Estimation Fusion with Gaussian Process for Nonlinear Dynamic Systems. Entropy 2019, 21, 1126. [Google Scholar] [CrossRef] [Green Version]
  11. Xiong, J.; Xiong, Z.; Cheong, J.W.; Xu, J.; Yu, Y.; Dempster, A.G. Cooperative positioning for low-cost close formation flight based on relative estimation and belief propagation. Aerosp. Sci. Technol. 2020, 106, 106068. [Google Scholar] [CrossRef]
  12. Chakraborty, A.; Brink, K.M.; Sharma, R. Cooperative Relative Localization Using Range Measurements without a Priori Information. IEEE Access 2020, 8, 205669–205684. [Google Scholar] [CrossRef]
  13. Fallon, M.F.; Papadopoulos, G.; Leonard, J.J.; Patrikalakis, N.M. Cooperative AUV Navigation using a Single Maneuvering Surface Craft. Int. J. Robot. Res. 2010, 29, 1461–1474. [Google Scholar] [CrossRef] [Green Version]
  14. Dehghannasiri, R.; Esfahani, M.S.; Qian, X.; Dougherty, E.R. Optimal Bayesian Kalman Filtering with Prior Update. IEEE Trans. Signal Process. 2018, 66, 1982–1996. [Google Scholar] [CrossRef]
  15. Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar] [CrossRef] [Green Version]
  16. Bai, M.; Huang, Y.; Zhang, Y.; Chen, F. A Novel Heavy-Tailed Mixture Distribution Based Robust Kalman Filter for Cooperative Localization. IEEE Trans. Ind. Inform. 2021, 17, 3671–3681. [Google Scholar] [CrossRef]
  17. Xu, B.; Guo, Y.; Wang, L.; Zhang, J. A Novel Robust Gaussian Approximate Smoother Based on EM for Cooperative Localization with Sensor Fault and Outliers. IEEE Trans. Instrum. Meas. 2021, 70, 1–14. [Google Scholar]
  18. Li, Q.; Ben, Y.; Naqvi, S.M.; Neasham, J.A.; Chambers, J.A. Robust Student’s t-Based Cooperative Navigation for Autonomous Underwater Vehicles. IEEE Trans. Instrum. Meas. 2018, 67, 1762–1777. [Google Scholar] [CrossRef] [Green Version]
  19. Huang, Y.; Zhang, Y.; Xu, B.; Wu, Z.; Chambers, J.A. A New Adaptive Extended Kalman Filter for Cooperative Localization. IEEE Trans. Aerosp. Electron. Syst. 2018, 54, 353–368. [Google Scholar] [CrossRef]
  20. Huang, X.; Wang, J. Real-Time Estimation of Center of Gravity Position for Lightweight Vehicles Using Combined AKF-EKF Method. IEEE Trans. Veh. Technol. 2014, 63, 4221–4231. [Google Scholar] [CrossRef]
  21. Xu, B.; Li, S.; Razzaqi, A.A.; Guo, Y.; Wang, L. A Novel Measurement Information Anomaly Detection Method for Cooperative Localization. IEEE Trans. Instrum. Meas. 2021, 70, 1–18. [Google Scholar]
  22. Oliveira, H.; Dias, S.S.; Bruno, M.G.D.S. Cooperative Terrain Navigation Using Hybrid GMM/SMC Message Passing on Factor Graphs. IEEE Trans. Aerosp. Electron. Syst. 2020, 56, 3958–3970. [Google Scholar] [CrossRef]
  23. Kschischang, F.; Frey, B.J.; Loeliger, H.-A. Factor graphs and the sum-product algorithm. IEEE Trans. Inf. Theory 2001, 47, 498–519. [Google Scholar] [CrossRef] [Green Version]
  24. Fan, S.; Zhang, Y.; Yu, C.; Zhu, M.; Yu, F. An Advanced Cooperative Positioning Algorithm Based on Improved Factor Graph and Sum-Product Theory for Multiple AUVs. IEEE Access 2019, 7, 67006–67017. [Google Scholar] [CrossRef]
  25. Sun, Q.; Tian, Y.; Diao, M. Cooperative Localization Algorithm Based on Hybrid Topology Architecture for Multiple Mobile Robot System. IEEE Internet Things J. 2018, 5, 4753–4763. [Google Scholar] [CrossRef]
  26. Zhang, L.; Tang, C.; Chen, P.; Zhang, Y. Gaussian Parameterized Information Aided Distributed Cooperative Underwater Positioning Algorithm. IEEE Access 2020, 8, 64634–64645. [Google Scholar] [CrossRef]
  27. Xu, J.; Yang, G.; Sun, Y.; Picek, S. A Multi-Sensor Information Fusion Method Based on Factor Graph for Integrated Navigation System. IEEE Access 2021, 9, 12044–12054. [Google Scholar] [CrossRef]
  28. Yuan, W.; Wu, N.; Guo, Q.; Huang, X.; Li, Y.; Hanzo, L. TOA-Based Passive Localization Constructed Over Factor Graphs: A Unified Framework. IEEE Trans. Commun. 2019, 67, 6952–6965. [Google Scholar] [CrossRef] [Green Version]
  29. Qin, H.D.; Yu, X.; Zhu, Z.B.; Deng, Z.C. A variational bayesian approximation based adaptive single transponder navigation method with unknown ESV. Ocean Eng. 2020, 209, 107484. [Google Scholar] [CrossRef]
  30. Kim, J. Cooperative Localization and Unknown Currents Estimation Using Multiple Autonomous Underwater Vehicles. IEEE Robot. Autom. Lett. 2020, 5, 2365–2371. [Google Scholar] [CrossRef]
  31. Yan, J.; Guo, D.; Luo, X.; Guan, X. AUV-Aided Localization for Underwater Acoustic Sensor Networks with Current Field Estimation. IEEE Trans. Veh. Technol. 2020, 69, 8855–8870. [Google Scholar] [CrossRef]
  32. Chen, B.; Xing, L.; Zhao, H.; Zheng, N.; Principe, J.C. Generalized Correntropy for Robust Adaptive Filtering. IEEE Trans. Signal Process. 2016, 64, 3376–3387. [Google Scholar] [CrossRef] [Green Version]
  33. Zhu, Y.; Zhao, H.; Zeng, X.; Chen, B. Robust Generalized Maximum Correntropy Criterion Algorithms for Active Noise Control. IEEE/ACM Trans. Audio Speech Lang. Process. 2020, 28, 1282–1292. [Google Scholar] [CrossRef]
  34. Thomson, D.J.M.; Dosso, S.E.; Barclay, D.R. Modeling AUV Localization Error in a Long Baseline Acoustic Positioning System. IEEE J. Ocean. Eng. 2018, 43, 955–968. [Google Scholar] [CrossRef]
  35. Li, X.; Lu, B.; Ali, W.; Jin, H. Passive Tracking of Multiple Underwater Targets in Incomplete Detection and Clutter Environment. Entropy 2021, 23, 1082. [Google Scholar] [CrossRef] [PubMed]
  36. Yang, Y.; Song, L.; Xu, T. Robust estimator for correlated observations based on bifactor equivalent weights. J. Geod. 2002, 76, 353–358. [Google Scholar] [CrossRef]
Figure 1. Illustration of framework of STCL.
Figure 1. Illustration of framework of STCL.
Entropy 23 01244 g001
Figure 2. Message passing mode of FGCKF.
Figure 2. Message passing mode of FGCKF.
Entropy 23 01244 g002
Figure 3. Sum-product based ESV in FGCKF.
Figure 3. Sum-product based ESV in FGCKF.
Entropy 23 01244 g003
Figure 4. Performance comparison of ACKF and CKF with great outliers of transmission delay.
Figure 4. Performance comparison of ACKF and CKF with great outliers of transmission delay.
Entropy 23 01244 g004
Figure 5. Performance comparison of various constant sound velocity and ESV.
Figure 5. Performance comparison of various constant sound velocity and ESV.
Entropy 23 01244 g005
Figure 6. Performance comparison of proposed FGCKF, CKF and EKF.
Figure 6. Performance comparison of proposed FGCKF, CKF and EKF.
Entropy 23 01244 g006
Figure 7. Performance comparison of proposed FGCKF, CKF and EKF with great outliers.
Figure 7. Performance comparison of proposed FGCKF, CKF and EKF with great outliers.
Entropy 23 01244 g007
Figure 8. Performance comparison of proposed FGCKF, CKF, and EKF in different types of motion trajectory.
Figure 8. Performance comparison of proposed FGCKF, CKF, and EKF in different types of motion trajectory.
Entropy 23 01244 g008
Figure 9. Performance analysis of proposed FGCKF with field data in motionless master test.
Figure 9. Performance analysis of proposed FGCKF with field data in motionless master test.
Entropy 23 01244 g009
Figure 10. Performance analysis of proposed FGCKF with field data in moving master test.
Figure 10. Performance analysis of proposed FGCKF with field data in moving master test.
Entropy 23 01244 g010
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Share and Cite

MDPI and ACS Style

Zhao, W.; Zhao, H.; Zou, D.; Liu, L. A Novel Factor Graph and Cubature Kalman Filter Integrated Algorithm for Single-Transponder-Aided Cooperative Localization. Entropy 2021, 23, 1244. https://0-doi-org.brum.beds.ac.uk/10.3390/e23101244

AMA Style

Zhao W, Zhao H, Zou D, Liu L. A Novel Factor Graph and Cubature Kalman Filter Integrated Algorithm for Single-Transponder-Aided Cooperative Localization. Entropy. 2021; 23(10):1244. https://0-doi-org.brum.beds.ac.uk/10.3390/e23101244

Chicago/Turabian Style

Zhao, Wanlong, Huifeng Zhao, Deyue Zou, and Lu Liu. 2021. "A Novel Factor Graph and Cubature Kalman Filter Integrated Algorithm for Single-Transponder-Aided Cooperative Localization" Entropy 23, no. 10: 1244. https://0-doi-org.brum.beds.ac.uk/10.3390/e23101244

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop