- freely available
- re-usable

*Sensors*
**2014**,
*14*(1),
995-1009;
doi:10.3390/s140100995

^{1}

^{2}

Published: 8 January 2014

## Abstract

**:**This paper studies the problem of multiple vehicle cooperative localization with spatial registration in the formulation of the probability hypothesis density (PHD) filter. Assuming vehicles are equipped with proprioceptive and exteroceptive sensors (with biases) to cooperatively localize positions, a simultaneous solution for joint spatial registration and state estimation is proposed. For this, we rely on the sequential Monte Carlo implementation of the PHD filtering. Compared to other methods, the concept of multiple vehicle cooperative localization with spatial registration is first proposed under Random Finite Set Theory. In addition, the proposed solution also addresses the challenges for multiple vehicle cooperative localization, e.g., the communication bandwidth issue and data association uncertainty. The simulation result demonstrates its reliability and feasibility in large-scale environments.

## 1. Introduction

Accurate vehicle localization is the current trend in the field of intelligent vehicles for the purpose of autonomous driving. Single vehicle localization is often performed by fusing both proprioceptive and exteroceptive sensors presented in [1]. Furthermore, GPS [2,3], cameras [4], laser scanners [5,6], dead reckoning [7] and Boolean information [8] are also used. However, with the developed inter-vehicle communication technology [9], the information exchange among the vehicles becomes available. With a wireless communication device, the multiple vehicle cooperative localization can be improved by taking advantage of information sharing. Various methods are investigated for cooperative localization, such as Extended Kalman Filter [10–12], Bayesian formalism [13], Markov Localization [14], Maximum Likelihood Estimation [15] and Maximum A Posteriori Estimation [16,17]. In our previous work [18], we presented a probability hypothesis density (PHD) solution for the purpose of addressing the following issues:

The communication bandwidth issue.

The communication bandwidth issue is introduced by Nerurkar [19,20]. The wireless transceivers used for information exchanging may have a low bandwidth, due to the restriction of the power. However, a high communication bandwidth for transmitting the estimation state, including large covariance matrices, is often required by the standard fusion methods.

The uncertainty issue.

Data association plays an important role in the cooperative localization introduced by Franchi et al. [21,22]. In their work, a data associator is proposed to compute all possible hypotheses based on the distances. With the development of the V2V (vehicle-to-vehicle) communication systems, vehicles are able to localize and identify the other members of the group correctly. Various aspects of inter-vehicle communication are surveyed in [23]. However, measurements often consist of clutters, which are hard to identify when aggregated in a small region. The standard techniques that are based on nearest neighbor association are therefore invalidated. In order to build up a more generic localization framework, addressing the association uncertainty is required.

Based on the above issues, we proposed the PHD filter to consider the whole group behavior instead of each vehicle [18]. In the PHD filter, the collection of individual vehicles is treated as a set-valued state, and the collection of individual measurements is treated as set-valued observation. Modeling set-valued state and set-valued observation as a random finite set (RFS) provides a novel potential in multiple vehicle cooperative localization, which has already been exhibited previously.

This paper enhances the previous work by joint spatial registration and states estimation simultaneously. Spatial registration is considered as follows: once the measurement is acquired form the sensor, two kinds of errors are also included. One belongs to the random noises described as Gaussian white noise, while the other is called the bias (systematic error), which is considered as a fixed value originated from the sensor calibration. The purpose of the spatial registration is to estimate the related bias during the whole process. To the best of the authors' knowledge, the spatial misregistration problem has not been considered in multiple vehicle cooperative localization under Random Finite Set Theory. The multiple vehicle states and the biases of the sensors are jointly estimated recursively via the PHD filter. The sequential Monte Carlo (SMC) method is used to implement the approach, considering non-linear and non-Gaussian conditions [24].

In this paper, we adopt the same framework as proposed previously: the measurements from both proprioceptive and exteroceptive sensors are projected to a global plane that constitutes of the observation set. The PHD filter recursively estimates the dynamic states and spatial biases according to the observations.

The contributions of the proposed approach are as follows:

We are among the first to consider multiple vehicle cooperative localization with unknown biases under Random Finite Set Theory. The proposed PHD filter can estimate and compensate for the measurement biases accurately, which performs better localizations. In addition, by utilizing the PHD filter, the challenges for multiple vehicle cooperative localization are also overcome [18], e.g., the communication bandwidth is bounded and the data association issue is eliminated.

The rest of this paper is organized as follows: Section 2 briefly describes multiple vehicle cooperative localization with measurement misregistration. Section 3 introduces the mathematic background of the PHD and its implementation. Section 4 presents simulation results. Finally, the paper is concluded in Section 5.

## 2. Background Description

As illustrated in Figure 1, the description of multiple vehicle cooperative localization is as follows:

Each vehicle is able to localize itself. Here, we assume that the corresponding measurements are in the 2D global coordinate.

Each vehicle is able to measure the relative position of the other vehicles. Here, we assume that the measurements are formatted with the range and bearing in a local coordinate. Furthermore, each measurement error is formed of white noises and fixed biases.

Vehicles are equipped with communication transceivers for information exchanging.

The communication network does not have the identification capability regarding the data association issue. Each vehicle observes the whole environment and transmits its observations over the network. There is no prior information regarding the data association issue provided by the inter-vehicle communication.

Multiple vehicle cooperative localization improves the precision of the localization. Assuming the measurements' noises for proprioceptive sensors are larger than a certain threshold, the localization may be imprecise when vehicles are aggregated in a small region. However, the precision is ensured with the help of the exteroceptive sensors. Furthermore, vehicles can localize themselves under the condition that only one proprioceptive sensor is available, with the help of cooperative localization.

Much work has been done for cooperative localization by using a centralized extended Kalman filter [10,12] and a decentralized solution [11,25,26]. The state of the group is viewed as a single system. The localization is obtained by exchanged data from each vehicle. The Kalman filter reduces the uncertainty of the localization by information sharing. However, the disadvantage is the large quantity of transmitted data. This quantity grows exponentially as the number of vehicles increases. In addition, for a dynamic multiple vehicle structure, the Kalman filter requires an association process, which takes huge resources.

Instead of a centralized architecture, a decentralized solution is investigated, where multiple fusion centers exist and each of them handles only local information (only the observed neighbors). However, the computational demand is very high. In addition, it often leads to the over-convergence problem when handling inter-estimate correlation among various sources. Since the over-convergence problem is caused by inter-estimation correlation, a natural idea for addressing this problem is investigated controlling of the data flow within the vehicle network. Howard [13] maintains a heuristic method based on a dependency tree to update the distribution dependencies. However, the author assumes that distributions are only dependent on the distribution that was last used and are independent of the others. This assumption is restrictive, as circular updates can still occur. The authors in [25] propose a state exchange-based method, which only allows independent estimates to be shared within the network. However, communication and computation demand is also increased. The authors in [26] proposed a covariance intersection filter [27] to handle the inter-estimate correlation issue. Similar work can also be found in [28,29], which exploits the advantage of estimation. However, as mentioned above, the computational complexity becomes an issue when the number of vehicles increases.

Neither centralized approaches nor decentralized approaches consider the spatial registration issue during the localization process. As we can see from Figure 2, each vehicle observes its surrounding environment with the help of the exteroceptive sensors. However, since the biases exist, the network cannot localize well when the local sensor coordinate transformed to the global coordinate. Therefore, the goal of multiple vehicle cooperative localization is to take spatial registration into account to acquire a more precise localization.

In this paper, we adopt the sequential Monte Carlo PHD filter implementation to jointly estimate the biases and the states simultaneously. The proposed SMC PHD filter handles spatial registration well under multiple vehicle cooperative scenarios during the whole process.

## 3. The Probability Hypothesis Density Filter

The PHD filter based on Random Finite Random Finite Set Theory is proposed because of its superior performance in the multiple targets tracking domain.

#### 3.1. Overview on RFS Statistics

The random finite set is a hidden Markov chain model with set-valued state and set-valued observation, while the PHD filter is a predicted and corrected framework for recursive Bayesian filtering in such an RFS formulation. A comparison of the RFS approach and traditional multiple-target tracking methods has been given in [30]. In the PHD filter, the collection of individual targets is treated as a set-valued state, and the collection of individual observations is treated as a set-valued observation. Figure 3 is a basic introduction to the PHD filter, which illustrates that the observations and the corresponding states are considered on a single-valued space at each step [31]. The PHD filter operates on the set space and avoids the combinatorial problem that arises from data association.

Regarding the superior performance, the PHD filter has been used for various scenarios, e.g., SLAM [32,33], extended target tracking [34] and multi-robot localization [35,36].

This paper extends the earlier work for multiple vehicle cooperative localization [18] by utilizing the sequential Monte Carlo PHD solution. The sequential Monte Carlo PHD implementation for biases estimation is first proposed by Lian et al. in [24]. In this paper, the multiple vehicle localization scenario is considered to exhibit its great potentials.

#### 3.2. Mathematic Background

The targets in a multi-target scenario at time k are represented as a finite set of vectors, X_{k}_{,1},…, X_{kN}_{(}_{k}_{)}, which takes values from the state space, χ ∈ ℝ^{n}^{x}.

_{k}

_{|}

_{k}

_{− 1}(x

_{k}|x

_{k}

_{−1}).

Similarly, the observations are represented as a finite set of vectors. Suppose L sensors with overlapping coverage provide measurements of the targets in χ by taking values in the observation space, Ƶ ∈ ℝ^{n}^{z}.

_{k}∈ X

_{k}, the measurement equation can be expressed as:

Let
${\beta}_{k}={[{({\beta}_{k}^{1})}^{\mathbf{T}},\dots ,{({\beta}_{k}^{L})}^{\mathbf{T}}]}^{\mathbf{T}}$ denote the L sensors' measurement bias vector; the task is to estimate the posterior PDF of the joint states X_{k} and bias β_{k}. This posterior is denoted by p_{k}(X_{k}, β_{k}|Z_{1:}_{k}), where Z_{1:}_{k} is a shortened notation for the sequence of measurement sets received up to time k. With the goal of deriving the PHD filter for joint spatial registration and states estimation, joint state **y**_{k} = (**x**_{k}, β_{k}) is therefore augmented.

Using these random finite set models, it is possible to construct process and observation models analogous to the single-target case. Randomness in X_{k}, β_{k} and Z_{k} can be encapsulated into a multi-target transition density and multi-target observation likelihood.

Under the above models, the multi-target Bayes filter propagates the posterior multi-target density, π_{k}(·|Z_{1:}_{k}), recursively in time. However, due to its combinatorial nature, it is intractable in most applications. To alleviate this, the PHD filter propagates the first moment or PHD D_{k}(·) of multi-target posterior density π_{k}(·).

The PHD recursion is given by:

_{D,k}denotes the probability that the measurements originate from the targets and P

_{S,k}denotes the probability that the target survives at time k. The intensity function, γ

_{k}(·), illustrates the birth target between k − 1 and k, whereas the clutter is specified by its intensity, κ

_{k}(·).

Analogous to the standard PHD filter, the spawn process of the augmented state is modeled as Poisson RFS with intensity ρ(**y**_{k}|**y**_{k}_{−1}). Assuming the state, X_{k}, is independent of bias β_{k}, the birth and spawn intensities, the transition density and the survival probability of the joint state, **y**_{k}, are written as:

Since the sensor biases are considered as non-random parameters, the following equations are therefore acquired:

Assume the biases are independent; the PHD update step is approximated as:

**y**

_{k}, is defined as the expected number of the objects in S:

It is noted that the PHD recursion has multiple integrals that have no closed form solutions in general. Therefore, the sequential Monte Carlo method is implemented. The MCMC (Markov Chain Monte Carlo) move step is provided for increasing the particle variety after the re-sample step, without affecting the validity of the approximation [37]. The K-means method is also utilized to cluster the resampled particles [38]. More details of the SMC PHD implementation can be found in [24]. Based on the above procedure, we address the issues for the PHD filter in multiple vehicle cooperative localization with spatial registration. The exteroceptive sensors' biases (bearing and range) and each vehicles' location are estimated simultaneously during the whole process.

#### 3.3. Implementation Issues

For the process model, the state **x**_{k} = [p_{x,k}, p_{y,k}, ṗ_{x,k}, ṗ_{y,k}]^{T} of each vehicle consists of position (p_{x,k},p_{y,k}) and velocity (ṗ_{x,k}, ṗ_{y,k}). Assume that the process noise is zero mean Gaussian white noise with the covariance matrix, Q_{k}. Then, the Markovian transition probability density of x_{k} is modeled as:

_{k}and F

_{k}are given by the constant velocity model as:

**I**

_{n}and

**O**

_{n}denote, respectively, the n×n identity and zero matrices and δ is the standard deviation of the process noise.

For the measurement model, measurements are originated from both proprioceptive and exteroceptive sensors projecting to the ground plane. To map the state to the observation space, the observation matrix is H_{k} = [**I**_{2}, **0**_{2}], while the measurement vector is **z** = [p_{x},p_{y}]^{T}.

The measurement errors of the exteroceptive sensors are comprised of the biases ${\beta}_{k}^{l}={[\mathrm{\Delta}{\rho}_{k}^{l},\mathrm{\Delta}{\theta}_{k}^{l}]}^{\mathbf{T}}$ (range and bearing) and the random measurement noises ${\mathbf{v}}_{k}^{l}={[\delta {\rho}_{k}^{l},\delta {\theta}_{k}^{l}]}^{\mathbf{T}}$. Assume that the biases do not drift against time; the dynamic model ${\beta}_{k}={[{({\beta}_{k}^{1})}^{\mathbf{\text{T}}},\dots ,{({\beta}_{k}^{L})}^{\mathbf{T}}]}^{T}$ can be considered as a first order Gauss–Markov process with the transition density:

Here, we only consider the measurements from both proprioceptive and exteroceptive sensors as the set-valued observation. For instance, we measure the vehicles' relative positions according to the exteroceptive sensors. Measurements are then acquired by coordinate transformation to the global plane. Assuming there are L vehicles for the cooperative localization, the size of the observation set is L(L − 1) at each step.

## 4. Simulation

The simulation was implemented on the ground plane over the surveillance region [−6, 000, 6, 000] × [−6,000,6,000] m^{2} for a period of 200 s. In fact, with the number of vehicles increasing, the final results are affected, due to the uncertainty issues. In this paper, only four vehicles are implemented to illustrate the potential of the PHD filter in the field of cooperative localization. In simulation, the biases of the exteroceptive sensors are considered as β^{1} = [−25 m, 75 mrad]^{T}, β^{2} = [55 m, −60 mrad]^{T}, β^{3} = [−40 m, 25 mrad]^{T}, β^{4} = [35 m, −45 mrad]^{T}. The random measurement noise for the proprioceptive sensor is assumed to be zero mean Gaussian white noise with covariance matrix R_{GPS} = diag[15 m^{2},15 m^{2}], while the exteroceptive sensor is with R_{Radar} = diag[5 m^{2}, 5 mrad^{2}]. The inter-vehicle communication is also available to exchange the information on the network. In addition, the V2V communication system does not have the ability to identify the others during the whole process. For the purpose of comparison, the simulation is also compared with the standard PHD filter, which does not consider the spatial registration [18].

In Figure 4, “+” denotes the vehicle position estimated by the PHD filter after considering spatial registration, while “○” denotes the standard PHD estimation. The solid line denotes the actual vehicle trajectory. The whole trajectory of each vehicle is missing, due to the PHD filter. Compared with standard target tracking methods, the PHD operates on the set space and avoids the association issue. Both observations and estimations are set-valued (for example, although the PHD filter can estimate the number of the vehicles and the corresponding states, it does not distinguish between them. Thus, the identification of a single vehicle over the whole process is not available;. However, for multiple vehicle cooperative localization, keeping the whole trajectory of each vehicle is out of the scope of this paper.

It can be seen that the estimation derived by the standard PHD filter deviates from the truth, due to the the effect of the sensor bias. However, after considering the spatial registration, the estimations are more close to the true trajectories.

Furthermore, the circular position error probability (CPEP) is also utilized to evaluate the performance of both methods. Given the true and estimated state sets ${\mathit{\chi}}_{k}={\{{\mathbf{x}}_{k}^{l}\}}_{l=1}^{{L}_{k}}$ and ${\widehat{\mathit{\chi}}}_{k|k}={\{{\widehat{x}}_{k|k}^{l}\}}_{l=1}^{{\widehat{L}}_{k|k}}$, the CPEP is calculated by:

_{k}

**x̂**

_{k}

_{|}

_{k}and H

_{k}

**x**

_{k}denote the estimated and true positions in global coordinates. In this paper, we let r = 20 m.

Figure 5 exhibits the CPEP at each step. It illustrates that the CPEP is smaller than the standard PHD by considering the sensors' biases, which exhibits the conclusion that cooperative localization with spatial registration is more close to the ground truth. The standard PHD filter's performance is rather ineffective without considering the spatial registration issue.

Figure 6 illustrates the estimations of the biases during the whole process. As we can see, the estimated values converge to the true biases in a few steps. After the biases elimination in the proposed PHD filter, the cooperative localization accuracy is significantly improved compared with the standard PHD filter, which is exhibited in Figure 5.

Figure 7 illustrates the number of vehicles, which is estimated by the PHD filter under the data association uncertainty issue. With time increasing, the vehicle numbers estimated by the proposed PHD filter are close to the true number, by the process of bias elimination. However, the performance of the standard PHD filter is imprecise compared to the true vehicle number, without eliminating the sensors' biases.

In conclusion, combined with our previous work [18], the benefits of the PHD filter are as follows:

First, it reduces and bounds the requirements of the communication bandwidth in a multiple vehicle environment. The inter-vehicle communication system transmits the measurements to the PHD filter, which takes little bandwidth. Compared to other methods, the proposed approach has the lowest consumption requirements for the communication bandwidth, since each vehicle only transmits its observations.

Second, it works under extreme conditions, which often happen in real environments (where the association uncertainty exists, the number of the vehicles is unknown, sensor delays and communication unavailability occur and the measurement is distracted by noise). The PHD filter not only illustrates the high performance of the localization, but also exhibits the robustness under the dynamic structure of the group.

Third, the spatial registration issue is addressed to jointly estimate the states and the corresponding biases during the whole process. Compared to others, the concept of multiple vehicle cooperative localization with spatial registration is first proposed under Random Finite Set Theory.

Because of the superiority mentioned above, we have strong reason to believe that the PHD filter has a great potential in the field of cooperative localization.

## 5. Conclusions

In this paper, a recursive Bayesian solution for multiple vehicle cooperative localization with spatial registration is first proposed. The sensors' biases and the vehicles' states are estimated simultaneously based on the PHD filter. In comparison to related work, the whole group of vehicles and the sensors' biases are viewed as a single set-valued state, the measurements are collected as a single set-valued observation, which is used to update the behavior of the set-valued state. The proposed approach also overcomes the challenges existing in multiple vehicle cooperative localization, e.g., low communication bandwidth and data association uncertainty. Experimental results exhibit the high performance of the joint spatial registration PHD filter.

Future work will focus on the evaluation of the proposed approach in a non-synthetic environment.

This work has been supported by the China Scholarship Council, the German Research Foundation (DFG) and the Technische Universität München within the funding programme Open Access Publishing.

## Conflicts of Interest

The authors declare no conflicts of interest.

## References

- Laneurit, J.; Chapuis, R.; Chausse, F. Accurate vehicle positioning on a numerical map. Int. J. Control Autom. Syst.
**2005**, 3, 15–31. - Kao, W.W. Integration of GPS and Dead-Reckoning Navigation Systems. Proceedings of the Vehicle Navigation and Information Systems Conference, Dearborn, MI, USA, 20–23 October 1991; Volume 2, pp. 635–643.
- Rezaei, S.; Sengupta, R. Kalman filter-based integration of DGPS and vehicle sensors for localization. IEEE Trans. Control Syst. Technol.
**2007**, 15, 1080–1088. - Zhang, F.; Chen, G.; Stahle, H.; Buckl, C.; Knoll, A. Visual Odometry Based on Random Finite Set Statistics in Urban Environment. Proceedings of the 2012 IEEE Intelligent Vehicles Symposium (IV), Alcala de Henares, Spain, 3–7 June 2012; pp. 69–74.
- Sohn, H.J.; Kim, B.K. A Robust Localization Algorithm for Mobile Robots with Laser Range Finders. Proceedings of the 2005 IEEE International Conference on Robotics and Automation (ICRA '05), Barcelona, Spain, 18–22 April 2005; pp. 3545–3550.
- Liu, M.; Huang, S.; Dissanayake, G. Feature Based SLAM Using Laser Sensor Data with Maximized Information Usage. Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011; pp. 1811–1816.
- John-Olof, N.; Peter, H. Recursive Bayesian Initialization of Localization Based on Ranging and Dead Reckoning. Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–8 November 2013; pp. 1399–1404.
- Remy, G.; Sebastien, L.; Laurent, H. A Visibility Information for Multi-Robots Localization. Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–8 November 2013; pp. 1426–1431.
- Khaled, Y.; Tsukada, M.; Santa, J.; Choi, J.; Ernst, T. A usage oriented analysis of vehiclular networks: from technologies to applications. J. Commun.
**2009**, 4, 357–368. - Roumeliotis, S.; Rekleitis, I. Analysis of Multirobot Localization Uncertainty Propagation. Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems, (IROS 2003), Las Vegas, NV, USA, 27 October–1 November 2003; Volume 2, pp. 1763–1770.
- Roumeliotis, S.; Bekey, G. Distributed multirobot localization. IEEE Trans. Robot. Autom.
**2002**. - Karam, N.; Chausse, F.; Aufrere, R.; Chapuis, R. Cooperative Multi-Vehicle Localization. Proceedings of the 2006 IEEE Intelligent Vehicles Symposium, Tokyo, Japan, 13–15 June 2006; pp. 564–570.
- Howard, A.; Mataric, M.; Sukhatme, G. Putting the ‘I’ in ‘Team’: An Ego-Centric Approach to Cooperative Localization. Proceedings of the 2003 IEEE International Conference on Robotics and Automation (ICRA '03), Taipei, Taiwan, 14–19 September 2003; Volume 1, pp. 868–874.
- Fox, D.; Burgard, W.; Thrun, S. Markov localization for mobile robots in dynamics environments. J. Artif. Intell. Res.
**1999**, 11, 391–427. - Howard, A.; Matark, M.; Sukhatme, G. Localization for Mobile Robot Teams Using Maximum Likelihood Estimation. Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems, EPFL, Switzerland, 1–4 October 2002; Volume 1, pp. 434–439.
- Nerurkar, E.; Roumeliotis, S.; Martinelli, A. Distributed Maximum a Posteriori Estimation for Multi-Robot Cooperative Localization. Proceedings of the 2009 IEEE International Conference on Robotics and Automation (ICRA '09), Kobe, Japan, 12–17 May 2009; pp. 1402–1409.
- Trawny, N.; Roumeliotis, S.; Giannakis, G. Cooperative Multi-Robot Localization under Communication Constraints. Proceedings of the 2009 IEEE International Conference on Robotics and Automation (ICRA '09), Kobe, Japan, 12–17 May 2009; pp. 4394–4400.
- Zhang, F.; Staehle, H.; Chen, G.; Buckl, C.; Knoll, A. Multiple Vehicle Cooperative Localization Under Random Finite Set Framework. Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '13), Tokyo, Japan, 3–8 November 2013; pp. 1405–1411.
- Nerurkar, E.D.; Zhou, K.X.; Roumeliotis, S.I. A Hybrid Estimation Framework for Cooperative Localization under Communication Constraints. Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '11), San Francisco, CA, USA, 25–30 September 2011; pp. 502–509.
- Nerurkar, E.; Roumeliotis, S.I. A Communication-Bandwidth-Aware Hybrid Estimation Framework for Multi-Robot Cooperative Localization. Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '13), Tokyo, Japan, 3–8 November 2013; pp. 1418–1425.
- Franchi, A.; Oriolo, G.; Stegagno, P. Mutual localization in multi-robot systems using anonymous relative measurements. Int. J. Robot. Res.
**2013**, 32, 1302–1322. - Franchi, A.; Oriolo, G.; Stegagno, P. Mutual Localization in a Multi-Robot System with Anonymous Relative Position Measures. Proceedings of the 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '09), St. Louis, MO, USA, 10–15 October 2009; pp. 3974–3980.
- Luo, J.; Hubaux, J.P. A Survey of Inter-Vehicle Communication. Technical Report; School of Computer and Communication Sciences: EPFL, Switzerland, 2004.
- Lian, F.; Han, C.; Liu, W.; Chen, H. Joint spatial registration and multi-target tracking using an extended probability hypothesis density filter. Radar Sonar Navig. IET
**2011**, 5, 441–448. - Karam, N.; Chausse, F.; Aufrere, R.; Chapuis, R. Localization of a Group of Communicating Vehicles by State Exchange. Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 519–524.
- Li, H.; Nashashibi, F. Cooperative Multi-Vehicle Localization Using Split Covariance Intersection Filter. Proceedings of the 2012 IEEE Intelligent Vehicles Symposium (IV), Alcala de Henares, Spain, 3–7 June 2012; pp. 211–216.
- Julier, S.; Uhlmann, J. A Non-Divergent Estimation Algorithm in the Presence of Unknown Correlations. Proceedings of the 1997 American Control Conference, Albuquerque, NM, USA, 4–6 June 1997; Volume 4, pp. 2369–2373.
- Arambel, P.; Rago, C.; Mehra, R. Covariance Intersection Algorithm for Distributed Spacecraft State Estimation. Proceedings of the 2001 American Control Conference, Arlington, VA, USA, 25–27 June 2001; Volume 6, pp. 4398–4403.
- Julier, S.; Uhlmann, J. Using Multiple SLAM Algorithms. Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '03), Las Vegas, NV, USA, 27 October–1 November 2003; Volume 1, pp. 200–205.
- Goodman, I.; Mahler, R.; Nguyen, H. Mathematics of Data Fusion; Kluwer Academic: Norwell, MA, USA, 1997.
- Vo, B.N. Random Finite Sets in Stochastic Filtering. Technical Report; EEE Department University: Melbourne, Australia, 2009.
- Mullane, J.; Vo, B.N.; Adams, M.; Vo, B.T. A random-finite-set approach to bayesian SLAM. IEEE Trans. Robot.
**2011**, 27, 268–282. - Kalyan, B.; Wijesoma, W.S.; Lee, K.W. FISST-SLAM: Finite set statistical approach to simultaneous localization and mapping. Int. J. Robot. Res.
**2010**, 29, 1251–1262. - Granstrom, K.; Lundquist, C.; Orguner, U. A Gaussian Mixture PHD Filter for Extended Target Tracking. Proceedings of the 2010 13th Conference on Information Fusion (FUSION '10), EICC, Edinburgh, UK, 26–29 July 2010; pp. 1–8.
- Stegagno, P.; Cognetti, M.; Rosa, L.; Peliti, P.; Oriolo, G. Relative Localization and Identification in a Heterogeneous Multi-Robot System. Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA '13), Karlsruhe, Germany, 6–10 May 2013; pp. 1857–1864.
- Dames, P.; Kumar, V. Cooperative Multi-Target Localization with Noisy Sensors. Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA' 13), Karlsruhe, Germany, 6–10 May 2013; pp. 1877–1883.
- Isaac, A.; Willett, P.; Bar-Shalom, Y. MCMC methods for tracking two closely spaced targets using monopulse radar channel signals. Radar Sonar Navig. IET
**2007**, 1, 221–229. - Clark, D.; Bell, J. Multi-target state estimation and track continuity for the particle PHD filter. IEEE Trans. Aerosp. Electron. Syst.
**2007**, 43, 1441–1453.

**Figure 1.**Multiple vehicle cooperative localization system.

**Figure 2.**Measurements from the exteroceptive sensor.

**Figure 3.**Set-valued states and set-valued observations.

**Figure 4.**Estimated positions by both filters.

**Figure 5.**The circular position error probability (CPEP) against time.

**Figure 6.**The estimated biases of the vehicles.

**Figure 7.**The estimated number of vehicles. PHD, probability hypothesis density.

© 2014 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).