Multiple Vessel Cooperative Localization Under Random Finite Set Framework With Unknown Birth Intensities

The key challenge for multiple vessel cooperative localization is considered as data association, in which state-of-the-art approaches adopt a divide-and-conquer strategy to acquire measurement-to-target association. However, traditional approaches suffer both the computational time and accuracy issues. Here, an improved algorithm under Random Finite Set statistics (RFSs) is proposed, in which the Probability Hypothesis Density (PHD) filter is utilized to address the aforementioned issues, by jointly estimating both the number of vessels and the corresponding states in complex environments. Furthermore, to avoid the prior requirement constrain with respect to the PHD filter, the pattern recognition method is simultaneously utilized to calculate the birth intensities. Simulation results exhibit the proposed approach performs better than normal PHD for multiple vessel cooperative localization, in scenarios of unknown birth intensity.


I. INTRODUCTION
Cooperative localization plays an important role for tasks of safety navigation [1], [2]. To achieve this goal, network agents are equipped with proprioceptive and exteroceptive sensors to achieve situational awareness [3]. However, most CL approaches drop performance in scenarios of unknown measurement-to-target association. Specially, the exteroceptive measurements often suffer clutter, to hardly distinguish interested targets. Meanwhile, communication network also has limited bandwidth, which makes CL tasks quite challenging [4]. Furthermore, cooperation among networks comes at huge computation resource in terms of real-time performance, which also need to be carefully optimized [5].
Various approaches have been investigated, which could be divided into two categories: Machine Learning (ML) [6] and Target Tracking (TT) [7], [8]. ML based approaches take cooperation and long-term reward into account, for providing novel solutions to traditional methods [9]. Deep Reinforcement Learning (DRL) is a recently emerging trend in ML, which considers the interaction between agent and The associate editor coordinating the review of this manuscript and approving it for publication was Wangli He . environment [10]. In particularly, cooperative localization problem could be considered from a multi-agent reinforcement learning perspective [11]. However, ML based approaches rely on large set of training data, and corresponding movements of each agents have seldom freedom. Meanwhile, measurement-to-target association is often considered as prior information under DRL framework. Target tracking based approaches consist of a team of cooperative agents estimating their positions with shared information. Classical tracking methods have been extensively studied and considered as a divide-and-conquer strategy: first calculate the measurement-to-target association, and then use filtering technologies to estimate the states [12]- [14]. Under target tracking framework, robust association function is needed [15], [16]. However, it is a chicken-and-egg problem: in density cluttered scenarios, calculating correct association is quite challenging.
Random Finite Set (RFS), as a recently emerging theory, has been proposed as an alternative solution to classical target tracking methods [17]- [20]. By modeling set-valued states and set-valued measurements under Bayesian framework, RFS points out a potential direction to address association issues. However, RFS framework has no closed form solutions in general, whereas the Probability Hypothesis Density (PHD) filter is considered as a sub-optimal approximation which propagates the first-order statistical moment of the RFS variables [21], [22].
In our previous work, the PHD filter is proposed to solve cooperative issues, however, preliminary conditions are required, such as the knowledge of the birth intensity (regions of new targets). In fact, this is quite challenging to acquire in practice [23]. Various approaches have been investigated for unknown birth intensities estimation, mainly focus on the non-linearly scenarios such as [24]- [26]. In this paper, the pattern recognition method is simultaneously utilized with the PHD filter [27]. Between consecutive frames, the topology information (distance) among interested targets is closed to invariant variable, hence the point matching method could be directly utilized to effectively distinguish interested and non-interested targets [28].
This paper is structured as follows: Sec. II describes the background of multiple vessel cooperative localization. Sec. III introduces the extended PHD solution with unknown birth intensity. Sec. IV demonstrates the effectiveness and robustness of the proposed approach, and the conclusions are drawn in Sec. V.

II. BACKGROUND DESCRIPTION
The scenario of multiple vessel cooperative localization is described on Fig.1: • Each vessel could localize itself by proprioceptive sensors like GPS. In this scenario, measurements originating from proprioceptive sensor is formatted with position and orientation (x, y, θ).
• Vessels could localize themselves by exteroceptive sensors like radar. In this scenario, measurements originating from exteroceptive sensor is formatted with range and bearing (r, φ).
• The communication bandwidth is guaranteed for information sharing.
• Clutter is also existed, and there is no information regarding measurement-to-target association. The performance of multiple vessel cooperative localization has been significantly improved, which is guaranteed by information sharing from both proprioceptive and exteroceptive sensors [29].
Traditional tracking based approaches could be divided into centralized solutions [30], and decentralized solutions [31]- [33]. On one hand, the performance of centralized solution drops in scenarios of dynamic structure; on the other hand, decentralized solution affects by both the covariance intersection and the communication bandwidth. Besides, data association also plays an important role in both centralized and decentralized approaches.
The proposed algorithm achieves high performance in low density clutter environment, but rapidly drops in highly clutter scenarios. This paper extends our previous work for applying PHD filter and point matching method to jointly estimate both states and corresponding parameters.

III. PROPOSED APPROACH
In this section, the PHD filter is utilized to cooperatively localize vessels, while the point matching approach is simultaneously applied to classify new birth vessels and clutter.

A. RFS FRAMEWORK AND PHD FILTER
The major challenge in multiple vessel cooperative localization is data association, in which the measurement-to-target association is unknown, or difficult to calculate. Meanwhile, as the density of clutter grows, not only the real-time performance but also the estimation performance drops significantly.
The RFS framework is based on set-valued state and setvalued observation, with respect to a hidden Markov chain model, while the PHD filter is based on recursive Bayesian framework, and with respect to predict and correct formulations [34], [35]. To illustrate the PHD filter more clearly, Fig. 2 exhibits that the received observations and the corresponding states during the estimation phase. At each step, the collected measurements are directly utilized to update the corresponding states from the set-valued space, without considering specific measurement-to-target associations.
The targets in a multi-target scenario at time k are represented as a finite set of vectors x k,1 , . . . , x k,N (k) , which takes values from the state space X ∈ R n x . Similarly, the observations are represented as a finite set of vectors z k,1 , . . . , z k,M (k) , which takes values from the observation space Z ∈ R n z . N (k) and M (k) represent the number of targets and observations at time k, respectively. These finite sets are known as the multi-target state and observation: where F(X ) and F(Z) denote the sets of all finite subsets of X and Z, respectively. Under RFS framework, the state model must encapsulate the time varying numbers of targets in a multi-target scenario. Also, the model must consider sensor imperfections such as missed detections and false alarms. The multi-target state is modeled as the union of different random finite sets: where S k|k−1 represents the targets that have survived from the previous time increment k − 1, which is modeled as a Bernoulli RFS meaning targets can either survive with probability P S,k (x k−1 ) and take on the new value {x k } with probability density f k|k−1 (x k |x k−1 ) or die and become the empty set ∅ with probability 1 − P S,k (x k−1 ). k represents targets which are spontaneously born at the current time k, which is modeled as a Poisson RFS specified by a mean birth rate and spatial birth density, or equivalently by its PHD or intensity γ k (·) where the mean birth rate is γ k (x)dx and the spatial birth density is Similarly, the set observation Z k composed by measurements originally from θ k (x) and the clutter κ k , modeled by a Poisson distribution.: where θ k represents the measurements that originate from the targets and is modeled as a Bernoulli RFS which generates a detection with probability P D,k (x k ) and yields the measurement {z k } with probability density g k (z k |x k ), or results in a missed detection yielding an empty measurement set ∅ with probability 1 − P D,k (x k ). K k represents the set of false alarms or clutter and is modeled as a Poisson RFS, specified by its intensity κ k (·) where the mean clutter rate is κ k (z)dz and the spatial clutter density is Notice that the PHD filter also evolves in two steps: prediction and update. The posterior density of the multi-target is called the intensity function D, where the transition function is denoted as f k|k−1 (x|ζ), given the previous state ζ.
The prediction of the PHD filter is represented as: To update the intensity function, we have: where g k (z i |x) denotes the likelihood of state x given an observation z i . The prediction in Eq. (5) includes components whose intensities are affected by targets that enter the scene (γ k ), targets that spawn new targets (β(x|ζ)), and targets that survive from the previous time step P S . D k−1 is the posterior PHD from the previous time step.
The update in Eq. (6) corrects the predicted PHD by including evidence from the current set of observations. Knowledge about scene clutter κ(z) is also embedded into the update step. Equation (7) illustrates that the integral of the PHD over a certain domain yields the estimated number of targets N (k) in that domain at time k. However, the PHD is not a probability density and does not necessarily sum to 1.
Noted that the PHD recursion has multiple integrals that have no closed form solutions in general. One of the common approaches to mitigate this problem is to use GM-PHD approximations. The GM-PHD filter [36] is a specialized version of the PHD filter, which assumes that the target's motion and observation process can be modeled as: where x refers to the current state, z to the current measurement, ζ to the previous state, N (·; m, P) denotes a Gaussian distribution with mean m and covariance P, F k−1 is the state transition matrix, Q k−1 is the process noise covariance, H k is the observation matrix, R k is the observation noise covariance. Survival and detection probabilities are supposed to be constant on the entire observed area: Birth targets γ k are modeled by a RFS written as a Gaussian mixture: where ω γ ,k and P (i) γ ,k are the weight, mean and covariance of the birth Gaussians and J γ ,k is their amount.
If the posterior PHD at time k − 1 is a Gaussian mixture: then the predicted PHD (5) of time k is a Gaussian mixture VOLUME 8, 2020 and the update PHD equation (6) at time k is also a Gaussian mixture and is given by where In standard GM-PHD framework, birth intensity γ k is assumed to be known. However, in practice, the birth intensity γ k is quite challenging to acquire.
To online estimate the birth intensity, the point matching method from patter recognition domain is simultaneously implemented under the random finite set framework.

B. POINT MATCHING ALGORITHM
Point matching is a hot topic in fields of computer vision and pattern recognition, and widely utilized in many applications such as object recognition, motion detection, and pose estimation [37]- [39]. The goal of point matching is to find whether there exists a mapping between the two point sets, either in complete matching or incomplete matching [40]. Researchers have done many contributions on incomplete matching in scenarios of existing outlines (spurious or lost points) [41]. Sofar, point matching could also be solved by using neural network [42] and clustering approaches [43], [44].
In this paper, as Euclidean transformation among multivessels are close to invariant sets in consecutive steps, the point matching method has been utilized to effectively extracting invariant sets. Afterwards, the extracted invariant sets are then considered as new targets under random finite set framework.The pseudo code of the matching algorithm could be found in [45], which is described as: 1) Once two sets M and N with points p and q, and an error threshold are given, the support index matrix could be calculated by I x = (I ij ) p×q and r = min(p, q); to I s ; 3) If length(I s ) < r, we have r = r − 1 and return to step 2;

C. IMPLEMENTATION DETAILS
To avoid the requirement of birth intensities, the point matching algorithm has been jointly implemented. After measurement Z k = (Z t k ∪ Z c k ) is received at consecutive steps, the real target set Z t k and clutter set Z c k could be classified. Meanwhile, new targets are then acquired based on the Mahalanobis distance as follows: where X k−1 denotes the surviving targets at last step, R represents the measurement uncertainty (it has to be mentioned that the converted observation uncertainties from both proprioceptive and exteroceptive sensors are jointly calculated.) Furthermore, as vessels are observed from both proprioceptive and exteroceptive sensors, the uncertainties of measurements could also be divided into two categories: observations solely from proprioceptive sensors Z t p k , and converted observations from both proprioceptive and exteroceptive sensors Z t pe k . With respect to PHD estimation, most applications focus on scenarios of single platform multiple targets, whereas multiple platforms multiple targets often implemented based on the multi-sensor PHD fusion framework. In cooperative localization, as vessels are measured by both sensors, the Gaussian mixture technique is employed to build a posterior PHD feedback fusion strategy. Hence, the GMPHD is sequentially implemented to overcome the deviation consistence issue from the converted measurements in framework of single platform multiple targets. 4518 VOLUME 8, 2020  At time k − 1, we have First, the process equation (8), measurement equation (9) and D k−1 (x) are utilized to predict the intensity D 1 k|k−1 (x). Then, D 1 k|k−1 (x) is updated with set Z t p k to obtain the PHD D 1 k (x) at time k. Finally, for set Z t pe k , we use D 1 k (x) as the predicted PHD in the similar way as The number of Gaussian components are for both Z t p k and Z t pe k , respectively. Although the GMPHD filter has significantly reduced the computation complexity under RFS framework, the calculated Gaussian components still grows explosively. To address such issue, closed Gaussian components will be merged into one Gaussian at each step.

IV. SIMULATION
To evaluate the performance, the proposed approach is validated with simulated data over [−1000, 1000] × [−1000, 1000] surveillance region. Meanwhile, measurement noise for proprioceptive sensor is i.i.d. with covariance diag[25m 2 , 25m 2 , 0.1mrad 2 ], while noise for exteroceptive sensor is diag[4m 2 , 0.1mrad 2 ]. The parameters of detection and survival probabilities are defined by 0.99, whereas the birth intensities are completely unknown. Furthermore, the average number of clutter follows Poisson distribution with λ c = 5 × 10 −7 m −2 . Fig. 4 shows the performance of the proposed approach, compared with state of the art approaches. Fig. 4a exhibits not only the converted measurements, but also the ground truths (each vessel follows the constant velocity model). Due to density clutter, the task of cooperative localization becomes quite challenging. Fig. 4b demonstrates the estimated results of the proposed approach, in contrast to the normal GMPHD in cooperative localization [23], and the state exchanged approach (SECL, [32]). During the PHD localization, the birth intensities for target '1' and target '2' are known, whereas target '3'(observed at step 25), target '4' (observed at step 40) and target '5' (observed at step 55) are vice versa. During the SECL localization, the measurement-to-target association is calculated based on the Nearest Neighbor method [13]. Notice that PHD estimations are represented as triangles, due to the fact that PHD operates on the set space, whereas the integration of whole track is thus missed. Nevertheless, keeping individual trajectory is far beyond the scope of this paper. Figure 4c illiterates the performance by calculating OSPA (Optimal Subpattern Assignment) metric [46]. Notice that the proposed approach addresses the unknown birth intensity issue, to successfully estimate the states. However, it is observed that the OSPA metric of normal PHD jumps significantly (three times), due to the fact that undetected vessels are considered as clutter. With respect to the SECL approach, as the data association has already been calculated, the performance is close to the proposed approach.
Furthermore, vessels with maneuver models are also considered (motion parameters are assumed to be known, meanwhile the GMPHD utilizes both linear and nonlinear models). Fig. 5a shows that vessel 4 and vessel 5 are spawned at 40 and 55 steps which follows the constant turn model, whereas the first three vessels keep the constant velocity model. Fig. 5b and Figure 5c demonstrates the performance of each algorithms. Notice that the proposed approach successfully keeps the robust in maneuvers, whereas the normal PHD jumps significantly and becomes worse.
As aforementioned, not only data association, but also communication bandwidth has seriously affected and restricted the performance of cooperative localization. But, the proposed approach also addresses the bandwidth issue. Take constant velocity model for example, assuming each measurement takes two communication bits during transmission, the PHD filter requires nearly 2n 2 bits bandwidth (a total of n measurements is acquired by proprioceptive sensors, n is the size of the vessel group. And, a total of n(n − 1) for the converted measurements). With respect to other approaches like SECL, the localization process utilizes not only the states but also the covariance. Assuming each state is consisted by 4 bits (2 bits for positions and 2 bits for velocities), the corresponding covariance is consisted by 16 bits. In summary, it takes additional 20n 2 bits bandwidth resources, in contrast to the proposed approach.
The real time performance of the proposed approach is also guaranteed. A general analytical to evaluate the whole complexity of the proposed approach is considered as o(m 2 n) (m, n denote the number of the measurements and states, respectively).
Sofar, the point matching method is only used for consecutive frames during the estimation, however, the curve fitting based approach also could be used without known the target motions [47]. Our future work will focus on combing point matching approach with such smoothing based approach to investigate the performance of cooperative localization.

V. CONCLUSION
Challenges for multi-vessel cooperative localization are summarized as high density clutter, low communication bandwidth and computational burdens. In this paper, the PHD filter has been simultaneously implemented with the point matching algorithm. During the PHD estimation, the invariant topology is specially utilized to construct the birth intensities from consecutive observations. Simulation results demonstrate the proposed approach performs better than normal PHD, in scenarios of unknown birth intensity.