1. Introduction
Unmanned surface vehicles (USVs) have gained prominence in ocean exploration, environmental monitoring, and maritime patrol due to their cost-effectiveness, maneuverability, and intelligent capabilities [
1,
2,
3]. However, individual USVs have limited adaptive capacity and struggle to respond effectively to unforeseen events. Consequently, USV clusters are often employed for the collaborative execution of marine tasks in complex and dynamic oceanic environments [
4,
5]. Collaborative localization technology is crucial for USV clusters, directly impacting navigation accuracy, formation control, and overall performance. Recent years have seen increased attention paid to USV cluster collaborative localization technology [
6,
7].
Previous research has explored various aspects of USV localization and collaboration. For instance, ref. [
8] developed an algorithm for USVs with self-localization capabilities to assist in positioning other USVs lacking this ability. The authors of ref. [
9] provided a comprehensive overview of USV collaborative techniques, including formation control and collision avoidance. The authors of ref. [
10] proposed a localization method for USV clusters in collaborative formation, although it required maintaining a relatively stable formation. The authors of ref. [
11] implemented a sequential fusion filter based on an unscented Kalman filter (UKF) for the real-time localization of individual USVs. The authors of ref. [
12] introduced an information fusion algorithm using adaptive fuzzy control to enhance multi-USV system localization accuracy. The authors of ref. [
13] presented a networked collaborative dynamic localization control scheme for multiple operating points, ensuring error index convergence within bounded regions. The authors of ref. [
14] proposed an integrated positioning and communication-assisted multi-USV network. However, most existing multi-USV collaborative localization methods rely on relatively fixed USV formations, with limited research on collaborative localization for distributed USV clusters without fixed formations.
This paper, therefore, aims to improve localization accuracy and consistency in distributed USV clusters through inter-USV information exchange. We develop a consensus algorithm to integrate localization information from each USV within the cluster.
For distributed network state estimation, three consensus-based information fusion structures exist: Consensus on Estimates (CE), Consensus on Measurement (CM), and Consensus on Information (CI) [
15]. CE performs consensus averaging on the state estimates of network nodes at different time points but does not transmit covariance matrix information, hindering state estimation accuracy improvement [
16,
17,
18,
19]. CM achieves consensus between local measurements and innovation covariance matrices by fusing information contribution vectors and associated information matrices. However, CM cannot guarantee stability with insufficient consensus steps [
20,
21]. CI achieves stability for arbitrary consensus steps by fusing information vectors and matrices but ignores correlations between local estimates, potentially reducing estimation accuracy [
21,
22,
23].
To address the limitations of existing consensus fusion methods, ref. [
21] introduced the Hybrid Consensus on Measurement and Consensus on Information (HCMCI) fusion method, which combines CM and CI to overcome their respective drawbacks while preserving their advantages. The HCMCI fusion algorithm has since been further developed. The authors of ref. [
24] applied HCMCI to target tracking in camera networks. The authors of ref. [
25] proposed a hybrid distributed algorithm for consensus tracking in small-scale unmanned aerial vehicle networks based on HCMCI.
In practical engineering applications, most systems are inherently nonlinear, necessitating numerical implementations for nonlinear systems [
26,
27]. These implementations generally fall into two categories: the direct approximation of nonlinear functions and approximation using probability density functions. The classical Extended Kalman Filter (EKF), proposed in [
28], directly expands nonlinear systems using Taylor series. Building on this, ref. [
21] introduced the Extended Kalman Hybrid Consensus Filter (EKHCF) for nonlinear systems. The authors of ref. [
29] presented the UKF, which reduces errors associated with higher-order Taylor terms, offering improved filtering accuracy compared to the EKF. The authors of ref. [
30] proposed the Particle Filter (PF) based on Monte Carlo methods, capable of handling nonlinear models in multidimensional state spaces, albeit with drawbacks such as particle degradation and high computational complexity. The authors of ref. [
31] introduced the cubature Kalman filter (CKF) based on the third-degree spherical–radial cubature rule, offering moderate computational requirements and superior filtering accuracy compared to the UKF.
The CKF is widely used in complex nonlinear systems. The authors of ref. [
32] propose an indirect fuzzy robust CKF based on a multi-input multi-output fuzzy reasoning system to address the issue of measurement uncertainty in nonlinear systems. To address the issue of inaccurate motion noise statistics caused by dynamic interference in integrated navigation systems, a closed-loop feedback covariance control method based on the CKF is proposed in [
33] to solve the aforementioned problem. The authors of ref. [
34] propose a CKF-based INS/CNS integrated nonlinear filtering method, which effectively mitigates the adverse effects of non-additive noise and dynamic model uncertainty on inertial measurements. The authors of ref. [
35] adopt the principle of multi-model interaction to design a CKF algorithm that combines adaptability and robustness, aiming to solve the tightly coupled integration problem of GNSS/INS. The authors of ref. [
36] propose a robust CKF with a scaling factor to mitigate the impact of GNSS observation uncertainty on INS/GNSS integrated navigation systems.
In light of these developments, we propose a Cubature Kalman Hybrid Consensus Filter (CKHCF) based on a CKF and HCMCI to address the collaborative localization problem for widely distributed USV clusters. In this approach, each USV within the cluster initially performs local filtering, then exchanges two sets of information pairs with all neighboring USVs. Corresponding fusion methods are applied to these information pairs, and the position state based on global measurements is calculated through the fused information. Our key contributions are as follows:
- (1)
The design of the CKHCF algorithm based on the CKF and HCMCI for distributed USV cluster collaborative localization.
- (2)
The development of a measurement compensation technique to mitigate the impact of random measurement delays.
- (3)
A proof of the algorithm’s consistency and boundedness for any given time and number of consensus steps using mathematical induction.
- (4)
A demonstration of the algorithm’s effectiveness through comprehensive simulated experiments.
The remainder of this paper is structured as follows:
Section 2 presents the problem formulation.
Section 3 details the design and derivation of the CKHCF algorithm for distributed USV cluster collaborative localization.
Section 4 provides a proof of the proposed algorithm’s consistency and boundedness using mathematical induction.
Section 5 describes a series of simulation experiments and analyzes the results to validate the algorithm’s effectiveness.
Section 6 concludes this paper.
Notation: The symbols “−1” and “T” denote the matrix inverse and transpose operations, respectively, denotes the Euclidean norm, and is equal to its previous neighbor. is the expectation of a random variable, and denotes a diagonal matrix with elements .
2. Problem Formulation
Since USVs operate solely on the water surface, the positioning coordinate system is simplified to a two-dimensional plane coordinate system.
The local Cartesian coordinate system is chosen as the navigation coordinate system. As depicted in
Figure 1, the angle between the carrier coordinate system and the navigation coordinate system represents the heading angle of the USV.
The two-dimensional kinematics model of USV
can be expressed as
where
is the forward velocity of USV
at time
,
is the heading angular velocity of USV
at time
,
is the course angle of USV
at time
,
is the dead reckoning epoch, and
is the number of USVs.
This paper aims to enhance the positioning accuracy of each USV by calculating and fusing the position information of different USVs. Consequently, only the location state of USVs will be analyzed [
37]. The state vector of the system can be defined as
where
and
represent the position of USV
at time
in the navigation coordinate system.
From Equations (1) and (2), the nonlinear dynamic model of the system can be expressed as
From Equation (3), the state equation of the USV cooperative localization system can be expressed as
where
is the state vector, and
is the system noise.
The measurement information of each USV in the collaborative localization system includes its own positional information, as well as the distance and relative angle information between it and other USVs within the communication range.
The measurement information of the USV cluster collaborative localization system with measurement delay can be expressed as
where
is the ideal measurement,
is the actual measurement,
is the measurement noise,
is the measured condition, and
and
represent the distance and relative angle between USV
and USV
.
Different from the measurement equation proposed in [
8], we also included measurements of the relative angle between two USVs to ensure the observability of the system when their motion directions are the same during consecutive observation periods.
Assumption 1. and are Gaussian white noise with zero mean and covariances and , respectively. They satisfy the followingwhere and are Kronecker delta functions. Assumption 2. The random sequence consists of independent Bernoulli random variables that take on the values 1 and 0 withwhere is the probability of the USV measurement delay at time . Assumption 3. The initial state vector is independent of , , and and satisfies the following equation.where and are the initial error covariance matrix and information matrix. The objective of this paper is to design a collaborative localization algorithm for distributed USV clusters with random measurement delays.
3. USV Cluster Collaborative Localization Algorithm
In this section, we design a CKHCF collaborative localization algorithm for a large-scale distributed USV cluster with random measurement delays.
In a large-scale distributed USV cluster, each USV within the cluster can only exchange information with its adjacent USVs due to limitations such as distance and the lack of global communication capability.
The communication structure of a cluster network consisting of USVs can be represented by a topology graph with a USV set and an edge set . If information can be exchanged between USVs and , then they are neighbors, and we define the neighboring USVs of USV as . The graph is strongly connected.
Figure 2 shows the communication network topology for the USV cluster with seven USVs. Taking USV 4 as an example, two consensus fusion steps are required to acquire global information. In the first consensus fusion step, the position information from USVs 2, 3, and 5 is obtained. In the second consensus fusion step, the position information containing USVs 1, 6, and 7 is obtained.
In Assumption 1, for the sake of derivation convenience, we assume that the USV cluster collaborative localization system is a noise-independent system, as shown in (7), where system noise is unrelated to measurement noise, the measurement noise of each USV is independent, and all noises are independent of their previous time step. In Assumption 2, we assume that the probability of a measurement delay in a USV cluster is . Equation (8) can be obtained from probability statistics. In Assumption 3, we assume that the initial state vector is independent of the system noise, measurement noise, and measured condition in the filtering process, and its error covariance matrix is the initial matrix of error covariance. The following theorems and conclusions are derived based on Assumptions 1–3.
Theorem 1. For Systems (4), (5), and (6) under the conditions of Assumptions 1–3, we have the first-stage USV cluster collaborative localization algorithm CKHCF with local filter pairs and .
Proof of Theorem 1. In the first stage, each USV independently conducts local filtering based on its own measurements. □
where
is an information pair composed of local filter information state vector
and information matrix
,
is another information pair composed of local filter information contribution state vector
and its associated information matrix
,
is the error of predicted measurement,
and
are the predicted information vector and information matrix, and
is the cross-covariance matrix at time
.
The predicted measurement error
can be expressed as
where
is the predicted state, and
is the predicted ideal measurement.
The predicted state
can be expressed as
where
and
are the state vector and information matrix based on global measurements at time
.
The predicted information vector
and information matrix
can be expressed as
The cross-covariance
can be expressed as
The local filter information matrix
and information vector
are defined as
where
is the local filter state error covariance matrix of USV
at time
.
From the Kalman Gaussian filter, the local iteration of information vector and information matrix can be calculated as (11).
Then, the local iteration of information state contribution vector and associated information matrix are defined as (13).
The predicted measurement can be calculated as
The predicted measurement error
can be calculated as
The cross-covariance
can be calculated as
From Equation (21), the information state vector
can be calculated as (18), and the predicted information matrix
can be calculated as
where
and
will be defined in Theorem 2.
Theorem 2. For Systems (4), (5), and (6) with assumptions 1–3, the second-stage USV cluster cooperative localization algorithm CKHCF can be expressed as follows. For Systems (4), (5), and (6) under the conditions of Assumptions 1–3, we have the first-stage USV cluster collaborative localization algorithm CKHCF with mean and information covariance matrix .
Proof of Theorem 2. In the second stage, all USVs transmit information pairs and , , ( denotes the consensus fusion steps, is the number of consensus fusion steps) to their neighboring USVs and receive two sets of information pairs from all their neighboring USVs. Subsequently, the state vectors within the information pairs are calculated using measured distances and relative angles and then fused with their own filter results. □
As shown in
Figure 1, USV
recalculates the information vector of the received information through measurement. The recalculated information vector can be calculated as
The recalculated state vectors are substituted into the information pairs received by USV and denoted as and , .
The fusion of two information pairs can be calculated as
where
,
is the fusion weight, and
.
The information vector and information matrix are updated as
where
is the consensus scalar weight.
Take , which represents the cardinality of the set N. When consensus step is large enough, the consensus weights are chosen which can provide and ; therefore, the accuracy of this distributed algorithm converges to that of a centralized algorithm.
Then, the state vector after information fusion
can be calculated as
The numerical implementation of , , , and is as follows
The covariance is factorized
The cubature points are evaluated (
)
where the unit cubature point is defined as
where
is the n-dimensional unit vector with the
element.
The propagated cubature points are evaluated (
)
,
, and
are estimated
The prediction covariance is factorized
The prediction cubature points are evaluated (
)
The propagated prediction cubature points are evaluated (
)
The prediction measurement is estimated
The cross-covariance matrix is estimated
The collaborative localization algorithm for a USV cluster, based on CKHCF and applicable to a wide range of distributions, can be summarized as follows (Algorithm 1).
Algorithm 1 CKHCF |
Given initial values |
Step 1. (First stage) Each USV performs local filtering. |
Measurements are obtained for each USVs. |
Calculate by Equation (41). |
Calculate by Equation (15). |
Calculate by Equation (48). |
Calculate the predicted information pair by Equations (18) and (19). |
Calculate the information pair by Equations (13) and (14). |
Step 2. (Second stage) Information pairs exchange and fusion. |
For l = 0, 1, …, L − 1, implement the following consensus steps in parallel. |
(1) Each USV transmits information pairs |
(2) |
(3) by Equation (28). |
(4) by Equations (30)–(33). |
Information update and state vector settlement. |
by Equations (34) and (35). |
by Equation (36). |
Step 3. Let t = t + 1, return to step 1. |
Remark 1. When the USV cluster is distributed within a limited area, each USV has global communication capability. In this case, the USVs in the cluster are all neighbors and can be considered as a special case with a consensus step of 1.
4. Stability Analysis
In this section, we demonstrate the stability of the proposed algorithm from two aspects: consistency and boundedness.
4.1. System Linearization Approximation
In order to facilitate analysis, the nonlinear system is linearized.
The method of statistical linear error propagation is first used to construct the state transition matrix
and measurement matrix
. According to [
15],
and
can be calculated as
where
can be calculated by Equation (48), and
can be calculated as
Two unknown instrument diagonal matrices and are defined, which are introduced to compensate for the higher-order terms of the Taylor expansion.
Therefore, Systems (4) and (5) can be approximated by linearization as
Based on linear systems (52) and (53), the information filter can be rewritten as
4.2. Proof of Consensus
Lemma 1. Consider a random vector , and let and be the unbiased estimates and error covariance of , respectively. When and satisfy the following equation, can be considered consistent [38]. Lemma 2. Define the function to be monotonic and non-decreasing. Function is determined by the following formula.
Theorem 3. When the initial error covariance satisfiesthe algorithm CKHCF can maintain consistency in any consensus step at any time. Proof of Theorem 3. Assume that at time
, the following equation holds,
From the choice of weight
, it can be inferred that
. We can obtain
Using the initialization algorithm, from Equation (62), we can obtain
Since the covariance intersection fusion rule is consistent, the following can be obtained
From Equation (65) and Lemma 2, we can obtain
According to mathematical induction, when the initial error covariance satisfies (60), the following inequalities hold true
In conclusion, the Proof of Theorem 3 is completed. □
4.3. Proof of Boundedness
Theorem 4. If the initial error covariance matrix is bounded, and there is a positive real constant , , , and that satisfy and , we can obtainwhere , and , where is the maximum eigenvalue of . Moreover, when .
Proof of Theorem 4. From Equation (57) and Lemma 2, we can obtain
So, we only need to prove that is bounded. We use mathematical induction to prove this conclusion.
For , from Equation (69), we can obtain . Assume at . Next, we only need to prove that the above conclusion is still true at time .
From Equations (55) and (57), we can obtain
In conclusion, the Proof of Theorem 4 is completed. □
5. Simulation
A simulation experiment is designed with a cluster of seven USVs to validate the effectiveness of the algorithm proposed in this paper. The state equation can be modeled as follows:
The measurement equation of the system can be represented by Equation (5), and the communication network topology of the USV cluster is shown in
Figure 2.
The initial state and error covariance , of USVs are set as , , , , , , , , , , , , , and .
The system noise settings for the seven USVs are set as , which are Gaussian white noise with a mean of zero, and the covariances are set as , , , , , , and .
The measurement noise settings for the seven USVs are set as , which are Gaussian white noise with a mean of zero. The position measurement noise covariances are the same as the setting, the distance measurement noise covariances are set as 0.5, and the relative angle measurement noise covariances are set as 1°.
The probabilities of each USV measurement delay are set as , , , , , , and .
The root mean-squared error (RMSE) is defined as
where
is the USV
state at time
, and
denotes the number of iterations of the Monte Carlo simulation.
Figure 3 and
Figure 4 show the position state
and
RMSEs of the CKHCF for USVs 1–7. As can be seen from the figures, USVs with diverse initial parameters can maintain localization consistency through information exchange and fusion within the cluster.
Figure 5 and
Figure 6 show the position state
and
RMSEs of the local filter and network filter for USV 4. As can be seen from the figures, the state RMSEs obtained by USV 4 through collaborative localization in the USV cluster network can be significantly reduced compared to those obtained by USV 4 through local filtering. This demonstrates that the collaborative localization network of the USV cluster can greatly improve the localization accuracy of the USVs within the cluster.
Figure 7 and
Figure 8 show the position state
and
RMSEs of the CKHCF with random delay compensation, as well as without compensation for USV 4. As can be seen from the figures, random delay can effectively enhance the localization accuracy of the USV within the cluster.
Figure 9 and
Figure 10 show the position state
and
RMSEs of the CKHCF, unscented Kalman hybrid consensus filter (UKHCF), and EKHCF for USV 4. As can be seen from the figures, the EKHCF has relatively higher state RMSEs, UKHCF has relatively lower state RMSEs, and CKHCF has the lowest state RMSEs. This is because the EKHCF neglects higher-order terms during linear approximation, leading to reduced filtering accuracy. On the other hand, the UKHCF and CKHCF utilize an approximate probability density function method, resulting in higher filtering accuracy. However, the UKF loses some statistical properties of Sigma points related to the posterior distribution of nonlinear functions, thereby reducing system estimation accuracy.
Figure 11 and
Figure 12 show the position state
and
RMSEs of the CKHCF, cubature Kalman consensus information filter (CKCIF), and cubature Kalman consensus measurement filter (CKCMF) for USV 4. As can be seen from the figures, the CKHCF algorithm exhibits the best estimation accuracy. This is because the CKCIF algorithm ignores the correlation between local estimations, while the CKCMF algorithm is unstable when there are fewer consensus steps. The CKHCF algorithm integrates the estimation and contribution of information from each USV separately, updating the information pairs using the fused results. This approach not only combines the advantages of the CKCIF and CKCMF but also overcomes their shortcomings.
6. Conclusions
In this paper, we design a collaborative localization algorithm CKHCF that is suitable for USV cluster systems with random measurement delays. Each USV exchanges two sets of information with all its neighbors, and after calculating the received information, the fusion of all information is used to obtain positioning based on global measurements. The proposed algorithm was proven to maintain consistency and boundedness at all times and in every consensus step through mathematical induction. In future research, we will focus on investigating how to utilize existing CKF algorithms [
32,
33,
34,
35,
36] to mitigate measurement interference caused by factors such as wind, waves, and ocean currents.