Cubature Kalman Hybrid Consensus Filter for Collaborative Localization of Unmanned Surface Vehicle Cluster with Random Measurement Delay

Abstract

This paper addresses the collaborative localization problem for unmanned surface vehicle (USV) clusters with random measurement delays. We propose a Cubature Kalman Hybrid Consensus Filter (CKHCF) based on the cubature Kalman filter (CKF) for widely distributed USV clusters lacking global communication capabilities. In this approach, each USV exchanges two pairs of information with all its neighbors and recalculates the received localization data based on distance and relative angle measurements. The recalculated information is then fused with the locally filtered data and updated to obtain localization information based on global measurements. To mitigate the impact of random measurement delays, we employ one-step prediction to compensate for delayed measurements. We present the derivation of the CKHCF algorithm and prove its consistency and boundedness using mathematical induction. Finally, we validate the effectiveness of the proposed algorithm through simulation experiments.

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, $\Vert ·\Vert$ denotes the Euclidean norm, and $(·)$ is equal to its previous neighbor. $E[·]$ is the expectation of a random variable, and $diag\{c_1,c_2,···,c_n\}$ denotes a diagonal matrix with elements $\{c_1,c_2,···,c_n\}$.

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.

Figure 1. USV two-dimensional plane collaborative localization system.

The two-dimensional kinematics model of USV $i$ can be expressed as

$$\{\begin{array}{l}{p}_{x,t}^i=p_{x,t-1}^i+v_t^i{\tau }_t\sin ({\phi }_t^i)\\ p_{y,t}^i=p_{y.t-1}^i+v_t^i{\tau }_t\cos ({\phi }_t^i)\\ {\phi }_t^i={\phi }_{t-1}^i+{\tau }_t{\omega }_t^i,\hspace{1em}i\in N\end{array}$$

(1)

where $a_t^i$ is the forward velocity of USV $i$ at time $t$, ${\omega }_t^i$ is the heading angular velocity of USV $i$ at time $t$, ${\phi }_t^i$ is the course angle of USV $i$ at time $t$, ${\tau }_t$ is the dead reckoning epoch, and $N$ 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

$$x_t^i={[p_{x,t}^i p_{y,t}^i]}^T$$

(2)

where $p_{x,t}^i$ and $p_{y,t}^i$ represent the position of USV $i$ at time $t$ in the navigation coordinate system.

From Equations (1) and (2), the nonlinear dynamic model of the system can be expressed as

$$f^{\prime }(x_t^i)=\left(\begin{array}{c}{\dot{p}}_{x,t}^i\\ {\dot{p}}_{y,t}^i\end{array}\right)=\left(\begin{array}{c}{v}_t^i\sin ({\phi }_t^i)\\ v_t^i\cos ({\phi }_t^i)\end{array}\right)$$

(3)

From Equation (3), the state equation of the USV cooperative localization system can be expressed as

$$x_t^i=f(x_{t-1}^i)+w_t^i$$

(4)

where $x_t^i$ is the state vector, and $w_t^i$ 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

$$\begin{array}{ll}{z}_t^i& =\left(\begin{array}{c}{p}_{x,t}^i\\ p_{y,t}^i\\ \sqrt{{(p_{x,t}^j-p_{x,t}^i)}^2+{(p_{y,t}^j-p_{y,t}^i)}^2}\\ \arctan \left({\displaystyle \frac{p_{y,t}^j-p_{y,t}^i}{p_{x,t}^j-p_{x,t}^i}}\right)\end{array}\right)+v_t^i\\ & =\left(\begin{array}{c}{p}_{x,t}^i\\ p_{y,t}^i\\ d_t^{i,j}\\ {\theta }_t^{i,j}\end{array}\right)+v_t^i=h(x_t^i)+v_t^i,\hspace{1em}j\in N,j\ne i\end{array}$$

(5)

$$y_t^i=(1-{\gamma }_t^i)z_t^i+{\gamma }_t^i{\widehat{z}}_{t|t-1}^i, t>1; y_t^i=z_t^i$$

(6)

where $z_t^i$ is the ideal measurement, $y_t^i$ is the actual measurement, $v_t^i$ is the measurement noise, ${\gamma }_t^i$ is the measured condition, and $d_t^{i,j}$ and ${\theta }_t^{i,j}$ represent the distance and relative angle between USV $i$ and USV $j$.

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. 

$w_t^i$ and $v_t^i$ are Gaussian white noise with zero mean and covariances $Q_t^i$ and $R_t^i$, respectively. They satisfy the following

$$E\left[\left(\begin{array}{c}{w}_t^i\\ v_t^i\end{array}\right)\left(\begin{array}{cc}{w}_k^{l,T}& v_k^{l,T}\end{array}\right)\right]=\left[\begin{array}{cc}{Q}_t^i& 0\\ 0& R_t^i\end{array}\right]{\delta }_{ij}{\delta }_{tk}$$

(7)

where ${\delta }_{ij}$ and ${\delta }_{tk}$ are Kronecker delta functions.

Assumption 2. 

The random sequence ${\gamma }_k^i$ consists of independent Bernoulli random variables that take on the values 1 and 0 with

$$\begin{array}{l}p({\gamma }_t^i=1)=E[{\gamma }_t^i]=p_t^i\\ p({\gamma }_t^i=0)=1-E[{\gamma }_t^i]=1-p_t^i\\ E[{({\gamma }_t^i-p_t^i)}^2]=(1-p_t^i)p_t^i\end{array}$$

(8)

where $p_t^i$ is the probability of the USV $i$ measurement delay at time $t$.

Assumption 3. 

The initial state vector ${\widehat{x}}_0^i$ is independent of $w_t^i$, $v_t^i$, and ${\gamma }_t^i$ and satisfies the following equation.

$$E[x_0^i]={\widehat{x}}_0^i$$

(9)

$$E[(x_0^i-{\widehat{x}}_0^i){(x_0^i-{\widehat{x}}_0^i)}^T]=P_0^i={\zeta }_0^{i,-1}$$

(10)

where $P_0^i$ and ${\zeta }_0^i$ 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 $G=(V,E)$ with a USV set $V=\{1,2,\cdots ,N\}$ and an edge set $E\subseteq V\times V$. If information can be exchanged between USVs $i$ and $j$, then they are neighbors, and we define the neighboring USVs of USV $i$ as $N_i=\left\{j\right|(i,j)\in E\}$. 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.

Figure 2. Communication network topology with seven USVs.

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 ${\gamma }_t^i$. 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 $({\widehat{\varsigma }}_{t,0}^i,{\zeta }_{t,0}^i)$ and $({\widehat{\eta }}_{t,0}^i,{\mu }_{t,0}^i)$.

Proof of Theorem 1. 

In the first stage, each USV independently conducts local filtering based on its own measurements. □

$${\widehat{\varsigma }}_{t,0}^i={\widehat{\varsigma }}_{t|t-1,0}^i+{\zeta }_{t|t-1,0}^i{P}_{xy,t|t-1}^i{(R_t^i)}^{-1}[{\tilde{y}}_{t|t-1}^i+{(P_{xy,t|t-1}^i)}^T{\zeta }_{t|t-1,0}^i{\widehat{x}}_{t|t-1}^i]$$

(11)

$${\zeta }_{t,0}^i={\zeta }_{t|t-1,0}^i+{\zeta }_{t|t-1,0}^i{P}_{xy,t|t-1}^i{(R_t^i)}^{-1}{(P_{xy,t|t-1}^i)}^T{\zeta }_{t|t-1,0}^i$$

(12)

$${\widehat{\eta }}_{t,0}^i={\zeta }_{t|t-1,0}^i{P}_{xy,t|t-1}^i{(R_t^i)}^{-1}[{\tilde{y}}_{t|t-1}^i+{(P_{xy,t|t-1}^i)}^T{\zeta }_{t|t-1,0}^i{\widehat{x}}_{t|t-1}^i]$$

(13)

$${\mu }_{t,0}^i={\zeta }_{t|t-1,0}^i{P}_{xy,t|t-1}^i{(R_t^i)}^{-1}{(P_{xy,t|t-1}^i)}^T{\zeta }_{t|t-1,0}^i$$

(14)

where $({\widehat{\varsigma }}_{t,0}^i,{\zeta }_{t,0}^i)$ is an information pair composed of local filter information state vector ${\widehat{\varsigma }}_{t,0}^i$ and information matrix ${\zeta }_{t,0}^i$, $({\widehat{\eta }}_{t,0}^i,{\mu }_{t,0}^i)$ is another information pair composed of local filter information contribution state vector ${\widehat{\eta }}_{t,0}^i$ and its associated information matrix ${\mu }_{t,0}^i$, ${\tilde{y}}_{t|t-1}^i$ is the error of predicted measurement, ${\widehat{\varsigma }}_{t|t-1,0}^i$ and ${\zeta }_{t|t-1,0}^i$ are the predicted information vector and information matrix, and $P_{xy,t|t-1}^i$ is the cross-covariance matrix at time $t-1$.

The predicted measurement error ${\tilde{y}}_{t|t-1}^i$ can be expressed as

$${\tilde{y}}_{t|t-1}^i=(1-{\gamma }_t^i)z_t^i+({\gamma }_t^i-1){\widehat{z}}_{t|t-1}^i$$

(15)

$${\widehat{z}}_{t|t-1}^i={\displaystyle \int h^i(x_t^i)N(x_t^i;{\widehat{x}}_{t|t-1}^i,{\zeta }_{t|t-1,0}^{i,-1})d{x}_t^i}$$

(16)

where ${\widehat{x}}_{t|t-1}^i$ is the predicted state, and ${\widehat{z}}_{t|t-1}^i$ is the predicted ideal measurement.

The predicted state ${\widehat{x}}_{t|t-1}^i$ can be expressed as

$${\widehat{x}}_{t|t-1}^i={\displaystyle \int f(x_{t-1}^i)N(x_{t-1}^i;{\widehat{x}}_{t-1,L}^i,{\zeta }_{t-1,L}^{i,-1})d{x}_{t-1}^i}$$

(17)

where ${\widehat{x}}_{t-1,L}^i$ and ${\zeta }_{t-1,L}^{i,-1}$ are the state vector and information matrix based on global measurements at time $t-1$.

The predicted information vector ${\widehat{\varsigma }}_{t|t-1,0}^i$ and information matrix ${\zeta }_{t|t-1,0}^i$ can be expressed as

$${\zeta }_{t|t-1,0}^i={({\displaystyle \int f(x_{t-1}^i)}{(·)}^{T}N(x_{t-1}^i;{\widehat{x}}_{t-1,L}^i,{\zeta }_{t-1,L}^{i,-1})d{x}_{t-1}^i-{\widehat{x}}_{t|t-1}^i{\widehat{x}}_{t|t-1}^{i,T}+Q_t^i)}^{-1}$$

(18)

$${\widehat{\varsigma }}_{t|t-1,0}^i={\zeta }_{t|t-1,0}^i{\widehat{x}}_{t|t-1}^i$$

(19)

The cross-covariance $P_{xy,t|t-1}^i$ can be expressed as

$$P_{xy,t|t-1}^i=(1-p_t^i){\displaystyle \int x_t^i}{(h^i(x_t^i))}^{T}N(x_t^i;{\widehat{x}}_{t|t-1}^i,{\zeta }_{t|t-1,0}^{i,-1})d{x}_t^i-(1-p_t^i){\widehat{x}}_{t|t-1}^i{\widehat{z}}_{t|t-1}^{i,T}$$

(20)

The local filter information matrix ${\zeta }_{t,0}^i$ and information vector ${\widehat{\varsigma }}_{t,0}^i$ are defined as

$${\zeta }_{t,0}^i={(P_{t,0}^i)}^{-1}$$

(21)

$${\widehat{\varsigma }}_{t,0}^i={(P_{t,0}^i)}^{-1}{\widehat{x}}_{t,0}^i={\zeta }_{t,0}^i{\widehat{x}}_{t,0}^i$$

(22)

where $P_{t,0}^i$ is the local filter state error covariance matrix of USV $i$ at time $t$.

From the Kalman Gaussian filter, the local iteration of information vector ${\widehat{\varsigma }}_{t,0}^i$ and information matrix ${\zeta }_{t,0}^i$ can be calculated as (11).

Then, the local iteration of information state contribution vector ${\eta }_{t,0}^i$ and associated information matrix ${\mu }_{t,0}^i$ are defined as (13).

The predicted measurement can be calculated as

$${\widehat{y}}_{t|t-1}^i=E[(1-{\gamma }_t^i)z_t^i+{\gamma }_t^i{\widehat{z}}_{t|t-1}^i]={\widehat{z}}_{t|t-1}^i$$

(23)

$${\widehat{z}}_{t|t-1}^i=E[z_t^i]={\displaystyle \int h^i(x_t^i)N(x_t^i;{\widehat{x}}_{t|t-1}^i,{\zeta }_{t-1,L}^{i,-1})d{x}_t^i}$$

(24)

The predicted measurement error ${\tilde{y}}_{t|t-1}^i$ can be calculated as

$$\begin{array}{ll}{\tilde{y}}_{k+1|k}^i& =y_{k+1}^i-{\widehat{y}}_{k+1|k}^i\\ & =(1-{\gamma }_t^i)z_t^i+{\gamma }_t^i{\widehat{z}}_{t|t-1}^i-{\widehat{z}}_{t|t-1}^i\\ & =(1-{\gamma }_t^i)z_t^i+({\gamma }_t^i-1){\widehat{z}}_{t|t-1}^i\end{array}$$

(25)

The cross-covariance $P_{xy,t|t-1}^i$ can be calculated as

$$\begin{array}{ll}{P}_{t|t-1}^{x^i{y}^i}& =E[(x_t^i-{\widehat{x}}_{t|t-1}^i){(y_t^i-{\widehat{y}}_{t|t-1}^i)}^T]\\ & =E[(x_t^i-{\widehat{x}}_{t|t-1}^i){((1-{\gamma }_t^i)z_t^i+({\gamma }_t^i-1){\widehat{z}}_{t|t-1}^i)}^T]\\ & =(1-p_t^i)E[x_t^i{(h^i(x_t^i))}^T]-(1-p_t^i){\widehat{x}}_{t|t-1}^{i}E[z_t^{i,T}]\\ & \hspace{1em}-(1-p_t^i)E[x_t^i]{\widehat{z}}_{t|t-1}^{i,T}+(1-p_t^i){\widehat{x}}_{t|t-1}^i{\widehat{z}}_{t|t-1}^{i,T}\\ & =(1-p_t^i){\displaystyle \int x_t^i}{(h^i(x_t^i))}^{T}N(x_t^i;{\widehat{x}}_{t|t-1}^i,{\zeta }_{t|t-1,0}^{i,-1})d{x}_t^i-(1-p_t^i){\widehat{x}}_{t|t-1}^i{\widehat{z}}_{t|t-1}^{i,T}\end{array}$$

(26)

From Equation (21), the information state vector ${\widehat{\varsigma }}_{t|t-1}^i$ can be calculated as (18), and the predicted information matrix ${\zeta }_{t|t-1}^i$ can be calculated as

$$\begin{array}{ll}{\zeta }_{t|t-1}^i& =P_{t|t-1}^{i,-1}=E^{-1}[(x_t^i-{\widehat{x}}_{t|t-1}^i){(x_t^i-{\widehat{x}}_{t|t-1}^i)}^T]\\ & ={(E[x_t^i{x}_t^{i,T}]-E[x_t^i]{\widehat{x}}_{t|t-1}^{i,T}-{\widehat{x}}_{t|t-1}^{i}E[x_t^{i,T}]+{\widehat{x}}_{t|t-1}^i{\widehat{x}}_{t|t-1}^{i,T})}^{-1}\\ & ={({\displaystyle \int f(x_{t-1}^i)}{(·)}^{T}N(x_{t-1}^i;{\widehat{x}}_{t-1,L}^i,{\zeta }_{t-1,L}^{i,-1})d{x}_k-{\widehat{x}}_{t|t-1}^i{\widehat{x}}_{t|t-1}^{i,T}+Q_t^i)}^{-1}\end{array}$$

(27)

where ${\widehat{x}}_{t-1,L}^i$ and ${\zeta }_{t-1,L}^{i,-1}$ 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 ${\widehat{x}}_{t,L}^i$ and information covariance matrix ${\zeta }_{t,L}^i$.

Proof of Theorem 2. 

In the second stage, all USVs transmit information pairs $({\widehat{\varsigma }}_{t|t-1,l}^i,{\zeta }_{t|t-1,l}^i)$ and $({\eta }_{t,l}^i,{\mu }_{t,l}^i)$, $i\in N$, $l=0,1,\cdots ,L-1$ ($l$ denotes the consensus fusion steps, $L$ 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 $i$ recalculates the information vector of the received information through measurement. The recalculated information vector can be calculated as

$${\widehat{\varsigma }}_{t|t-1,l}^{i,j}={\widehat{\varsigma }}_{t|t-1,l}^j-{\zeta }_{t|t-1,l}^j\left[\begin{array}{c}{d}_t^{i,j}\sin ({\theta }_t^{i,j})\\ d_t^{i,j}\cos ({\theta }_t^{i,j})\end{array}\right]$$

(28)

$${\widehat{\eta }}_{t,l}^{i,j}={\zeta }_{t|t-1,l}^j{P}_{xy,t|t-1}^j{(R_t^j)}^{-1}{P}_{xy,t|t-1}^{j,T}{\zeta }_{t|t-1,l}^j\left[\begin{array}{c}{d}_t^{i,j}\sin ({\theta }_t^{i,j})\\ d_t^{i,j}\cos ({\theta }_t^{i,j})\end{array}\right]$$

(29)

The recalculated state vectors are substituted into the information pairs received by USV $i$ and denoted as $({\widehat{\varsigma }}_{t|t-1,l}^j,{\zeta }_{t|t-1,l}^j)$ and $({\widehat{\eta }}_{t,l}^j,{\mu }_{t,l}^j)$, $j\in N_i$.

The fusion of two information pairs can be calculated as

$${\widehat{\varsigma }}_{t|t-1,l+1}^i={\displaystyle {\sum }_{j\in N_i}{G}^{i,j}{\widehat{\varsigma }}_{t|t-1,l}^{i,j}}$$

(30)

$${\zeta }_{t|t-1,l+1}^i={\displaystyle {\sum }_{j\in N_i}{G}^{i,j}{\zeta }_{t|t-1,l}^j}$$

(31)

$${\widehat{\eta }}_{t,l+1}^i={\displaystyle {\sum }_{j\in N_i}{G}^{i,j}{\widehat{\eta }}_{t,l}^{i,j}}$$

(32)

$${\mu }_{t,l+1}^i={\displaystyle {\sum }_{j\in N_i}{G}^{i,j}{\mu }_{t,l}^j}$$

(33)

where $G^{i,j}$, $j\in N_i$ is the fusion weight, and ${\displaystyle {\sum }_{j\in N_i}{G}^{i,j}=1}$.

The information vector and information matrix are updated as

$${\widehat{\varsigma }}_{t,L}^i={\widehat{\varsigma }}_{t|t-1,L}^i+{\lambda }_t^i{\widehat{\eta }}_{t,L}^i$$

(34)

$${\zeta }_{t,L}^i={\zeta }_{t|t-1,L}^i+{\lambda }_t^i{\mu }_{t,L}^i$$

(35)

where ${\lambda }_t^i$ is the consensus scalar weight.

Take ${\lambda }_t^i=\left|N\right|$, which represents the cardinality of the set N. When consensus step $L$ is large enough, the consensus weights are chosen $G^{i,j}\to 1/\left|N\right|$ which can provide ${\widehat{\eta }}_{t,L}^i\to {\displaystyle \frac{1}{N}}{\displaystyle {\sum }_{t=1}^N{\widehat{\eta }}_t^i}$ and ${\mu }_{t,L}^i\to {\displaystyle \frac{1}{N}}{\displaystyle {\sum }_{t=1}^N{\mu }_t^i}$; therefore, the accuracy of this distributed algorithm converges to that of a centralized algorithm.

Then, the state vector after information fusion ${\widehat{x}}_{t,L}^i$ can be calculated as

$${\widehat{x}}_{t,L}^i={({\zeta }_{t,L}^i)}^{-1}{\widehat{\varsigma }}_{t,L}^i$$

(36)

The numerical implementation of ${\widehat{x}}_{t|t-1}^i$, ${\widehat{\varsigma }}_{t|t-1,0}^i$, ${\zeta }_{t|t-1,0}^i$, and $P_{xy,t|t-1}^i$ is as follows

• The covariance is factorized

  $${({\zeta }_{t-1,L}^i)}^{-1}=S_{t-1}^i{(S_{t-1}^i)}^T$$

  (37)
• The cubature points are evaluated ($k=1,2,\cdots ,2m$)

  $$X_{k,t-1}^i={\widehat{x}}_{t-1,L}^i+S_{t-1}^i{\gamma }_k$$

  (38)

  where the unit cubature point is defined as

  $$u_k=\{\begin{array}{ll}\sqrt{m}{e}_k,& k=1,2,\cdots ,m\\ -\sqrt{m}{e}_{k-m},& k=m+1,\cdots ,2m\end{array}$$

  (39)

  where $e_k$ is the n-dimensional unit vector with the $kth$ element.
• The propagated cubature points are evaluated ($k=1,2,\cdots ,2m$)

  $$X_{k,t|t-1}^i=f(X_{k,t-1}^i)$$

  (40)
• ${\widehat{x}}_{t|t-1}^i$, ${\widehat{\varsigma }}_{t|t-1,0}^i$, and ${\zeta }_{t|t-1,0}^i$ are estimated

  $${\widehat{x}}_{t|t-1}^i={\displaystyle \frac{1}{2m}}{\displaystyle \sum _{k=1}^{2m}{X}_{k,t|t-1}^i}$$

  (41)

  $${\zeta }_{t|t-1,0}^i={[{\displaystyle \frac{1}{2m}}{\displaystyle {\sum }_{k=1}^{2m}{X}_{k,t|t-1}^i{(·)}^T}-{\widehat{x}}_{t|t-1}^i{(·)}^T+Q_t^i]}^{-1}$$

  (42)

  $${\widehat{\varsigma }}_{t|t-1,0}^i={\zeta }_{t|t-1,0}^i{\widehat{x}}_{t|t-1}^i$$

  (43)
• The prediction covariance is factorized

  $${({\zeta }_{t|t-1,0}^i)}^{-1}=S_{t|t-1}^i{(S_{t|t-1}^i)}^T$$

  (44)
• The prediction cubature points are evaluated ($k=1,2,\cdots ,2m$)

  $$X_{k,t|t-1}^i={\widehat{x}}_{t|t-1}^i+S_{t|t-1}^i{u}_k$$

  (45)
• The propagated prediction cubature points are evaluated ($k=1,2,\cdots ,2m$)

  $$Y_{k,t|t-1}^i=h(X_{k,t|t-1}^i)$$

  (46)
• The prediction measurement is estimated

  $${\widehat{y}}_{t|t-1}^i={\displaystyle \frac{1}{2m}}{\displaystyle {\sum }_{i=1}^{2m}{Y}_{k,t|t-1}^i}$$

  (47)
• The cross-covariance matrix is estimated

  $$P_{xy,t|t-1}^i={\displaystyle \frac{1}{2m}}{\displaystyle {\sum }_{k=1}^{2m}{X}_{k,t|t-1}^i{(Y_{k,t|t-1}^i)}^T-{\widehat{x}}_{t|t-1}^i{({\widehat{y}}_{t|t-1}^i)}^T}$$

  (48)

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 ${\widehat{x}}_0^i={[p_{x,0}^i{p}_{y,0}^i]}^T\mathrm{and}{\zeta }_0^i=P_0^{-1}.$

Step 1. (First stage) Each USV performs local filtering.

Measurements $y_{t-1}^i,i\in N$ are obtained for each USVs.

Calculate ${\widehat{x}}_{t|t-1}^i$ by Equation (41).

Calculate ${\tilde{y}}_{t|t-1}^i$ by Equation (15).

Calculate $P_{xy,t|t-1}^i$ by Equation (48).

Calculate the predicted information pair $({\widehat{\varsigma }}_{t|t-1,0}^i,{\zeta }_{t|t-1,0}^i)$ by Equations (18) and (19).

Calculate the information pair $({\eta }_{t,0}^i,{\mu }_{t,0}^i)$ 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 $({\widehat{\varsigma }}_{t|t-1,l}^i,{\zeta }_{t|t-1,l}^i)\mathrm{and}({\eta }_{t,l}^i,{\mu }_{t,l}^i),i\in N\mathrm{to}\mathrm{its}\mathrm{neighbor}\mathrm{USVs}j\in N_i.$

(2) $\mathrm{Each}\mathrm{USV}\mathrm{receives}\mathrm{information}\mathrm{pairs}({\widehat{\varsigma }}_{t|t-1,l}^j,{\zeta }_{t|t-1,l}^j)\mathrm{and}({\widehat{\eta }}_{t,l}^j,{\mu }_{t,l}^j)\mathrm{from}\mathrm{all}\mathrm{neighbors}j\in N_i.$

(3) $\mathrm{Calculate}\mathrm{the}\mathrm{information}\mathrm{vector}{\widehat{\varsigma }}_{t|t-1,l}^{i,j}$ by Equation (28).

(4) $\mathrm{Each}\mathrm{USV}\mathrm{fuses}\mathrm{two}\mathrm{information}\mathrm{pairs}({\widehat{\varsigma }}_{t|t-1,l+1}^i,{\zeta }_{t|t-1,l+1}^i)\mathrm{and}({\eta }_{t,l+1}^i,{\mu }_{t,l+1}^i)$ by Equations (30)–(33).

Information update and state vector settlement.

$\mathrm{Update}\mathrm{information}\mathrm{vector}{\widehat{\varsigma }}_{t,L}^i\mathrm{and}\mathrm{information}\mathrm{matrix}{\zeta }_{t,L}^i$ by Equations (34) and (35).

$\mathrm{Calculate}\mathrm{the}\mathrm{fused}\mathrm{state}\mathrm{vector}{\widehat{x}}_{t,L}^i$ 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 $F_t^i$ and measurement matrix $H_t^i$. According to [15], $F_t^i$ and $H_t^i$ can be calculated as

$$H_t^i={(P_{xy,t|t-1}^i)}^T{\zeta }_{t|t-1}^i$$

(49)

$$F_{t-1}^i={(P_{x_{t-1},x_{t|t-1}}^i)}^T{\zeta }_{t-1}^i$$

(50)

where $P_{xy,t|t-1}^i$ can be calculated by Equation (48), and $P_{x_{t-1},x_{t|t-1}}^i$ can be calculated as

$$P_{x_{t-1},x_{t|t-1}}^i={\displaystyle \frac{1}{2m}}{\displaystyle {\sum }_{k=1}^{2m}(X_{k,t-1}^i-{\widehat{x}}_{t-1}^i){(X_{k,t|t-1}^i-{\widehat{x}}_{t|t-1}^i)}^T}$$

(51)

Two unknown instrument diagonal matrices ${\alpha }_{t-1}^i=diag({\alpha }_{t-1,1}^i,{\alpha }_{t-1,2}^i,\cdots ,{\alpha }_{t-1,n}^i)$ and ${\beta }_t^i=diag({\beta }_{t,1}^i,{\beta }_{t,2}^i,\cdots ,{\beta }_{t,n}^i)$ 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

$$x_t^i={\alpha }_{t-1}^i{F}_{t-1}^i{x}_{t-1}^i+w_t^i$$

(52)

$$y_t^i={\beta }_t^i{H}_t^i{x}_t^i+v_t^i$$

(53)

Based on linear systems (52) and (53), the information filter can be rewritten as

$${\widehat{\varsigma }}_{t|t-1}^i={\zeta }_{t|t-1}^i({\alpha }_{t-1}^i{F}_{t-1}^i){\widehat{\varsigma }}_{t-1}^i$$

(54)

$${\zeta }_{t|t-1}^i={[{\alpha }_{t-1}^i{F}_{t-1}^i{({\zeta }_{t-1}^i)}^{-1}{({\alpha }_{t-1}^i{F}_{t-1}^i)}^T+Q_{t-1}^i]}^{-1}$$

(55)

$${\widehat{\varsigma }}_t^i={\widehat{\varsigma }}_{t|t-1}^i+{({\beta }_t^i{H}_t^i)}^T{(R_t^i)}^{-1}{y}_t^i$$

(56)

$${\zeta }_t^i={\zeta }_{t|t-1}^i+{({\beta }_t^i{H}_t^i)}^T{(R_t^i)}^{-1}{\beta }_t^i{H}_t^i$$

(57)

4.2. Proof of Consensus

Lemma 1. 

Consider a random vector $x$, and let $\widehat{x}$ and $P$ be the unbiased estimates and error covariance of $x$, respectively. When $\widehat{x}$ and $P$ satisfy the following equation,

$$E[(x-\widehat{x}){(x-\widehat{x})}^T]\le P$$

(58)

$(\widehat{\varsigma },\zeta )=(P^{-1}x,P^{-1})$ can be considered consistent [38].

Lemma 2. 

Define the function $C(·)$ to be monotonic and non-decreasing. Function $C(·)$ is determined by the following formula.

$${\zeta }_{t|t-1}^i=C({\zeta }_{t-1}^i)$$

(59)

Theorem 3. 

When the initial error covariance satisfies

$${\zeta }_{1|0}^i\le E^{-1}[(x_1-{\widehat{x}}_{1|0}^i){(x_1-{\widehat{x}}_{1|0}^i)}^T]$$

(60)

the algorithm CKHCF can maintain consistency in any consensus step at any time.

$${\zeta }_t^i\le E^{-1}[(x_t^i-{\widehat{x}}_t^i){(x_t^i-{\widehat{x}}_t^i)}^T]$$

(61)

Proof of Theorem 3. 

Assume that at time $k$, the following equation holds,

$${\zeta }_{t|t-1}^i\le E^{-1}[(x_t^i-{\widehat{x}}_{t|t-1}^i){(x_t^i-{\widehat{x}}_{t|t-1}^i)}^T],i\in N$$

(62)

From the choice of weight ${\lambda }_t^i$, it can be inferred that ${\lambda }_t^i\ge 1$. We can obtain

$$\begin{array}{ll}{\zeta }_{t,1}^i& ={\displaystyle \sum _{j\in N_i}{G}^{ij}[{\zeta }_{t|t-1,0}^j+{\lambda }_t^i{\mu }_{t,0}^i]}\\ & \ge {\displaystyle \sum _{j\in N_i}{G}^{ij}[{\zeta }_{t|t-1,0}^j+{\mu }_{t,0}^i]}\end{array}$$

(63)

Using the initialization algorithm, from Equation (62), we can obtain

$$\begin{array}{ll}{E}^{-1}[(x_t^i-{\widehat{x}}_{t,0}^i){(x_t^i-{\widehat{x}}_{t,0}^i)}^T]& \ge {\zeta }_{t|t-1,0}^i+{\lambda }_t^i{\mu }_{t,0}^i\\ & \ge {\zeta }_{t|t-1,0}^i+{\mu }_{t,0}^i={\zeta }_{t,0}^i\end{array}$$

(64)

Since the covariance intersection fusion rule is consistent, the following can be obtained

$$E^{-1}[(x_t^i-{\widehat{x}}_{t,l+1}^i){(x_t^i-{\widehat{x}}_{t,l+1}^i)}^T]\ge {\zeta }_{t,l+1}^i,\hspace{1em}l=0,1,\cdots ,L-1$$

(65)

From Equation (65) and Lemma 2, we can obtain

$$\begin{array}{ll}{\zeta }_{t+1|t,0}^i& =C({\zeta }_{t,L}^i)\\ & \le C(E^{-1}[(x_t^i-{\widehat{x}}_{t,L}^i){(x_t^i-{\widehat{x}}_{t,L}^i)}^T])\\ & =E^{-1}[(x_{t+1}^i-{\widehat{x}}_{t+1|t}^i){(x_{t+1}^i-{\widehat{x}}_{t+1|t}^i)}^T]\end{array}$$

(66)

According to mathematical induction, when the initial error covariance satisfies (60), the following inequalities hold true

$${\zeta }_{t,l}^i\le E^{-1}[(x_t-x_{t,l}^i){(x_t-x_{t,l}^i)}^T],\hspace{1em}i\in N,l\in N_i$$

(67)

In conclusion, the Proof of Theorem 3 is completed. □

4.3. Proof of Boundedness

Theorem 4. 

If the initial error covariance matrix $P_0^i={\zeta }_0^{i,-1}$ is bounded, and there is a positive real constant $\overline{a}$, $\overline{b}$, $\overline{q}$, and $\overline{r}$ that satisfy ${\alpha }_{t-1}^i{F}_{t-1}^i{({\alpha }_{t-1}^i{F}_{t-1}^i)}^T\le \overline{a}{I}_n$ and $\overline{q}\le Q_{t-1}^i$ , we can obtain

$${\zeta }_t^{i,-1}\le {\zeta }_{t|t-1}^{i,-1}\le p_k^{-1}{I}_n$$

(68)

where $p_t^{-1}={\lambda }_{\max }({\zeta }_0^i){\overline{a}}^t+\overline{q}{\displaystyle \sum _{k=0}^{t-1}{\overline{a}}^k}$, and $p_0^{-1}={\lambda }_{\max }({\zeta }_0^{-1})$, where ${\lambda }_{\max }({\zeta }_0^{-1})$ is the maximum eigenvalue of ${\zeta }_0^{-1}$.

Moreover, $p_t^{-1}<p={\displaystyle \frac{\max [p_0^{-1},\overline{q}]}{1-\overline{a}}}$ when $\overline{a}<1$.

Proof of Theorem 4. 

From Equation (57) and Lemma 2, we can obtain

$${\zeta }_t^i\ge {\zeta }_{t|t-1}^i\ge {\zeta }_{t-1}^i\ge \cdots \ge {\zeta }_0^i$$

(69)

So, we only need to prove that ${\zeta }_0^{i,-1}=P_0^i$ is bounded. We use mathematical induction to prove this conclusion.

For $t=0$, from Equation (69), we can obtain ${\zeta }_0^{i,-1}\le {\lambda }_{\max }({\zeta }_0^{i,-1})I_n=p_0{I}_n$. Assume ${\zeta }_{t-1}^{i,-1}\le p_{k-1}{I}_n$ at $t-1$. Next, we only need to prove that the above conclusion is still true at time $t$.

From Equations (55) and (57), we can obtain

$$\begin{array}{ll}{\zeta }_t^i& \ge {[{\alpha }_{t-1}^i{F}_{t-1}^i{({\zeta }_{t-1}^i)}^{-1}{({\alpha }_{t-1}^i{F}_{t-1}^i)}^T+Q_{t-1}^i]}^{-1}\\ & \ge {[p_{t-1}^{-1}{\alpha }_{t-1}^i{F}_{t-1}^i{({\alpha }_{t-1}^i{F}_{t-1}^i)}^T+Q_{t-1}^i]}^{-1}\\ & \ge {(p_{t-1}^{-1}\overline{a}+\overline{q})}^{-1}{I}_n\\ & \ge {[{\lambda }_{\max }({\zeta }_0^i){\overline{a}}^t+\overline{q}{\displaystyle \sum _{k=0}^{t-2}{\overline{a}}^{k+1}}+\overline{q}]}^{-1}{I}_n=p_t^{-1}{I}_n\end{array}$$

(70)

When $\overline{a}<1$, we have

$$p_t^{-1}={\lambda }_{\max }({\zeta }_0^{-1}){\overline{a}}^t+\overline{q}{\displaystyle \sum _{k=0}^{t-1}{\overline{a}}^k}$$

(71)

$$p_t^{-1}\le \max [{\lambda }_{\max }({\zeta }_0^{-1}),\overline{q}]{\displaystyle \sum _{k=0}^t{\overline{a}}^k}<{\displaystyle \frac{\max [{\lambda }_{\max }({\zeta }_0^{-1}),\overline{q}]}{1-\overline{a}}}$$

(72)

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:

$$x_k=\left(\begin{array}{ccccc}1& {\displaystyle \frac{\sin \mathsf{\Omega }T}{\mathsf{\Omega }}}& 0& -\left({\displaystyle \frac{1-\cos \mathsf{\Omega }T}{\mathsf{\Omega }}}\right)& 0\\ 0& \cos \mathsf{\Omega }T& 0& -\sin \mathsf{\Omega }T& 0\\ 0& {\displaystyle \frac{1-\cos \mathsf{\Omega }T}{\mathsf{\Omega }}}& 1& {\displaystyle \frac{\sin \mathsf{\Omega }T}{\mathsf{\Omega }}}& 0\\ 0& \sin \mathsf{\Omega }T& 0& \cos \mathsf{\Omega }T& 0\\ 0& 0& 0& 0& 1\end{array}\right)x_{k-1}+w_k$$

(73)

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 $x_0^i$ and error covariance $P_0^i$, $i\in 1~7$ of USVs are set as $x_0^1={(0,2,0,4,0.10)}^T$, $x_0^2={(15,2,30,4,0.10)}^T$, $x_0^3={(15,2,-30,4,0.10)}^T$, $x_0^4={(30,2,0,4,0.10)}^T$, $x_0^5={(45,2,0,4,0.10)}^T$, $x_0^6={(60,2,30,4,0.10)}^T$, $x_0^7={(60,2,-30,4,0.10)}^T$, $P_0^1=diag(2,0.3,1,0.2,{10}^{-4})$, $P_0^2=diag(0,0.2,1,0.1,{10}^{-4})$, $P_0^3=diag(1,0.4,0,0.2,{10}^{-4})$, $P_0^4=diag(1,0.1,0,0.3,{10}^{-4})$, $P_0^5=diag(1.2,0.25,1,0.3,{10}^{-4})$, $P_0^6=diag(1,0.1,1.5,0.3,{10}^{-4})$, and $P_0^7=diag(1,0.2,2,0.1,{10}^{-4})$.

The system noise settings for the seven USVs are set as $w_t^i, i\in 1~7$, which are Gaussian white noise with a mean of zero, and the covariances are set as $Q_t^1=diag(2,1,2,3,{10}^{-6})$, $Q_t^2=diag(1.5,4,1.5,4,{10}^{-6})$, $Q_t^3=diag(2.5,3,2.5,3,{10}^{-6})$, $Q_t^4=diag(1,2,1,2,{10}^{-6})$, $Q_t^5=diag(2,3.5,2,3,{10}^{-6})$, $Q_t^6=diag(1.8,2,1.8,3,{10}^{-6})$, and $Q_t^7=diag(2.2,1,2.2,3,{10}^{-6})$.

The measurement noise settings for the seven USVs are set as $v_t^i, i\in 1~7$, which are Gaussian white noise with a mean of zero. The position measurement noise covariances are the same as the $Q_t^i$ 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 $p_t^1=0.85$, $p_t^2=0.9$, $p_t^3=0.83$, $p_t^4=0.8$, $p_t^5=0.78$, $p_t^6=0.93$, and $p_t^7=0.88$.

The root mean-squared error (RMSE) is defined as

$$RMSE(t)=\sqrt{{\displaystyle \frac{1}{N}}{\displaystyle \sum _{n=1}^N{(x_t^i-{\widehat{x}}_t^i)}^2}}$$

(74)

where ${\widehat{x}}_t^i$ is the USV $i$ state at time $t$, and $N=50$ denotes the number of iterations of the Monte Carlo simulation.

Figure 3 and Figure 4 show the position state $p_x^i$ and $p_y^i$ 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 3. Position state $p_x^i$ RMSEs of CKHCF for USVs 1–7.

Figure 4. Position state $p_y^i$ RMSEs of CKHCF for USVs 1–7.

Figure 5 and Figure 6 show the position state $p_x^4$ and $p_y^4$ 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 5. Position state $p_x^4$ RMSEs of local filter and network filter for USV 4.

Figure 6. Position state $p_y^4$ RMSEs of local filter and network filter for USV 4.

Figure 7 and Figure 8 show the position state $p_x^4$ and $p_y^4$ 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 7. Position state $p_x^4$ RMSEs of random delay compensation and without compensation for USV 4.

Figure 8. Position state $p_y^4$ RMSEs of CKHCF with random delay compensation and without compensation for USV 4.

Figure 9 and Figure 10 show the position state $p_x^4$ and $p_y^4$ 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 9. Position state $p_x^4$ RMSEs of CKHCF, UKHCF, and EKHCF for USV 4.

Figure 10. Position state $p_y^4$ RMSEs of CKHCF, UKHCF, and EKHCF for USV 4.

Figure 11 and Figure 12 show the position state $p_x^4$ and $p_y^4$ 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.

Figure 11. Position state $p_x^4$ RMSEs of CKHCF, CKCIF, and CKCMF for USV 4.

Figure 12. Position state $p_y^4$ RMSEs of CKHCF, CKCIF, and CKCMF for USV 4.

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.