0Delayed State Nonparametric Filtering in Cooperative Tracking7163601

962

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

Delayed-State Nonparametric Filtering in Cooperative Tracking Mao Shan, Member, IEEE, Stewart Worrall, Member, IEEE, and Eduardo Nebot, Senior Member, IEEE

Abstract—This paper presents a novel nonparametric approach toward delayed-state filtering for cooperative tracking. Standard parametric cooperative localization/tracking approaches are generally aimed at problems that can be easily parameterized and/or are limited to incorporate only real-time measurements. This paper provides a nonparametric yet computationally tractable alternative that is suitable for tracking cases where real-time observations are not always possible, e.g., in a sparse mesh network. The proposed delayed-state cooperative particle filter features forward filtering and backward smoothing to incorporate measurements that are received with time delays. A record of historical marginal states is kept for each mobile node within a sliding time window, instead of the high-dimensional joint state. Essentially, it replaces the importance sampling in traditional particle filters by a Gibbs sampler, which is a Markov chain Monte Carlo method, to fuse all available egocentric and internode relative observations into the global position estimate, thus alleviating the high-dimensionality problems in cooperative tracking. The performance of the proposed approach is evaluated in a multiagent simulation, and experimental results from a large-scale multivehicle industrial operation clearly demonstrate that the proposed approach effectively facilitates the tracking of mobile nodes without position awareness, through the use of relative range, negative detection, and time-delayed measurements. Index Terms—Cooperative tracking, localization, nonparametric filtering, sensor fusion, sensor networks.

I. INTRODUCTION OCALIZING and tracking mobile nodes, such as people, robots, and vehicles, in an industrial environment is of significance in many autonomous and manned applications. Different forms of global navigation satellite system (GNSS)-based tracking algorithms have been widely used for this purpose. Unfortunately, in some practical applications, not all mobile nodes will have continuous access to absolute position information from GNSS. This can be caused by dropouts of GNSS signal, malfunction of hardware, failure of antennas, etc. Even when the GNSS is providing position information, there can be significant errors especially in cluttered environments, due to signal noise, multipath or poor satellite configuration. An alternate approach is to deploy a number of beacons at known positions and use trilateration/triangulation to estimate locations of the position unknown agents, which requires the working environment to be

L

Manuscript received June 26, 2014; revised March 10, 2015; accepted June 17, 2015. Date of publication July 21, 2015; date of current version August 4, 2015. This paper was recommended for publication by Associate Editor D. Scaramuzza and Editor D. Fox upon evaluation of the reviewers’ comments. The authors are with the Australian Centre for Field Robotics, The University of Sydney, Sydney, N.S.W. 2006, Australia (e-mail: [email protected]; [email protected]; [email protected]). Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TRO.2015.2450412

covered by a sufficient number of beacons in order to achieve a desired level of positioning accuracy. The localization problem can also be addressed with cooperative localization (CL). Pairwise measurements of relative range and/or bearing can be used to estimate the relative coordinates of all mobile nodes. A CL-based approach can have significant advantages since it can reduce or eliminate the requirement for infrastructure (e.g., fixed beacons), and the requirement for absolute position information (provided by GNSS) from all of the mobile nodes. In a typical cooperative tracking scenario, the absolute Euclidean positions and pairwise measurements are shared between the mobile nodes via a type of peer-to-peer (P2P) communication network such as a wireless sensor network [1], [2], mobile ad-hoc network (MANET) [3] or vehicular adhoc network (VANET) [4]. It is essential that information from a node should be received by the fusion center (another node or base station) in real time, using multihopping communication through the mesh network [5]. However, delays in the transmission of information are inevitable in real applications with a sparse mesh network. One of such examples is illustrated in Fig. 1, where an isolated agent problem occurs as the connection with a node is interrupted and the node is left without any neighbors. A possible solution to this issue is by introducing one or more “information carriers,” which are agents that pass close to the isolated agent and bring the information to the rest of the mesh network, although with some time delays. In addition, when an agent is not detected by others in a particular area, it is considered negative information, which also assists in constraining the possible location of the agent in the absence of other observations. This paper presents a computationally tractable nonparametric approach for cooperative tracking that is able to consider time-delayed information in the global position estimation of mobile nodes. The paper is mostly concerned with nonparametric filtering, but assumes that the collection of informative observations in a delay/disruption tolerant network (DTN) has been handled by an appropriate multiagent data collection approach, e.g., a variant of the observation harvesting technique in [6] that allows both time-delayed egocentric and relative position information of mobile agents to be exchanged and brought forward to the fusion center/base station through P2P communication. Essentially, the paper is a substantial extension to the Gibbs sampler-based cooperative particle filtering method proposed in the previous work [7] toward delayed-state nonparametric filtering, whereby the information (mostly time-delayed) received by the fusion center can be incorporated into the cooperative tracking. The proposed approach is also a significant step beyond the filtering algorithm in [6], which considers time-delayed observations but does not include cooperative tracking.

1552-3098 © 2015 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications standards/publications/rights/index.html for more information.

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

963

problem in a Bayesian form. A detailed description of the proposed DSCPF algorithm is presented in Section IV, with simulation results in Section V. Section VI presents experimental results from tracking multiple vehicles in a large-scale industrial operation. Finally, the conclusion and future work are presented in Section VII. II. RELATED WORK

Fig. 1. CL and the isolated agent issue. (a) demonstrates a typical CL application where every agent (but one) is connected with others nearby to form a mesh network. An isolated agent issue occurs when an agent does not have any neighbor around and as a consequence loses connection to the rest. In (b), another mobile agent passes and brings the information of the isolated agent, though time-delayed, back to the main group.

The proposed approach, defined here as the delayed-state cooperative particle filter (DSCPF), features forward filtering and backward smoothing, to incorporate all available egocentric and relative distance observations incorporating time delays, regardless of the sequence of their arrivals within a sliding time window. To achieve this, the filter keeps track of historical states for each tracked mobile node within the time window. It also maintains the marginal state of every node instead of a joint state of the group and has the importance sampling step in a classical particle filter replaced by a Gibbs sampler, which is a Markov chain Monte Carlo (MCMC) method. The forward filtering refers to the Gibbs sampler-based filtering process in chronological order, for the historical states using the delayed observations followed by the present state, while the backward smoothing further improves estimates of the historical states using “future” information, in reversed time sequence. The performance of the proposed approach is evaluated in a simulation by examining multiple mobile nodes moving randomly while communicating and sharing time-delayed information with one another. In addition, experiment results presented in the paper demonstrate that the algorithms can effectively improve the position estimates of mobile nodes without egocentric position information available by using relative distance, negative detection, and time-delayed observations. The remainder of this paper is organized as follows. Section II reviews the related work in the areas of CL/tracking and delayed-state filtering. Section III provides background information about delayed-state filtering and formulates the tracking

The advent of efficient P2P communication technology has made CL/tracking a very active area of research. An comprehensive review of related work is presented in [8]. Distributed and decentralized approaches [2], [9], [10] have advantages such as higher network robustness and scalability, while centralized approaches [1], [11] are attractive to applications where monitoring or surveillance is required from a “control room.” An typical example is given in [5] to track a fleet of vehicles in a mining safety scenario. Furthermore, centralized algorithms usually offer more accurate position estimates in a comparison with decentralized counterparts [12], although at the expense of higher bandwidth and computational costs for the central node. In terms of filtering method, the current CL approaches can be mostly categorized as either parametric or nonparametric. The parametric approaches include Kalman filter (KF) and extended Kalman filter (EKF) based [3], [9], [13]–[15], maximum likelihood estimation based [16], maximum a posteriori based [17], least-squares based [18] methods to name a few. Among them, the KF/EKF is often the most attractive due to the minimal computational costs, although these approaches are only suitable for applications with near, or at least local Gaussian properties. There is recent work in [19] aiming at improving robustness of KF/EKF to non-Gaussian and heavy-tailed noise. The nonparametric approaches are mostly particle filter based [2], [20], [21], such as nonparametric belief propagation (NBP), sum-product algorithm over a wireless network (SPAWN), and so forth. Compared with nonparametric methods, parametric approaches can produce more precise results. However, this is achieved only when parametric assumptions are correctly made. Nonparametric algorithms are considered a more practical choice for the tracking of multiple objects in cluttered scenarios, where accurate model parameterization is often not trivial for parametric and semiparametric (e.g., Gaussian sum based [22]) methods. The NBP was initially proposed for localizing in static networks [23]. Generally, it is a distributed messagepassing method on a graphical model. Its variants can be used for cooperative tracking in mobile networks [24]. However, a Gaussian mixture is still adopted in [23], [24] to approximate each message in belief propagation. Furthermore, it is only guaranteed to converge to the correct beliefs in singly connected networks (e.g., tree). Overconfident results would be obtained for networks multiply connected, or in other words networks with loops/cycles in terms of factor graphs [25], [26]. The same limitation is also found in the SPAWN [8]. Only a few solutions for networks with loops exist in the literature, and they are rarely used in localization [27]. The aforementioned inconsistent/overconfident estimation issue [14], [21] (a.k.a, circular reasoning [20], cyclic update [15])

964

is usually present in the decentralized CL approaches, essentially due to improper consideration of cross-correlation and dependency between nodes, and thus double counting of information. The work in [2] and [28] removes common past information by introducing channel filtering before the fusion stage. Nevertheless, it is subject to constraints in memory resource and scalability, as a separate channel filter is required for each communication link [2]. Besides, the use of channel filters is limited to tree-connected networks. The most intuitive way to track every node cooperatively is to maintain joint hypotheses. The cross-correlation is then handled implicitly in the filter; hence, the potential inconsistency issue is no longer a concern. However, the “dimensional curse” brought by a high-dimensional state space remains a challenge particularly when parametric filtering methods are not applicable. The problem per se is due to low efficiency of sampling when a traditional importance sampling-based algorithm attempts to extract samples for all components of the state space simultaneously, however with an inadequate ensemble size. It is concluded in [29]–[31] that the ensemble size should grow exponentially with system state dimension in bootstrap particle filters, to prevent the weight degeneracy (or weight collapse) issue. By marginalizing some variables, a Rao–Blackwellized particle filter [32] can outperform a standard particle filter with a reduced statespace size. The prerequisite is that a subset of the state space can be handled by a parametric filtering approach. As one of the most effective approaches to complex probabilistic systems, a Gibbs sampler [33] breaks the highdimensional state space in CL by sampling from each lowdimensional target state space conditionally on others, rather than directly from the joint, thereby scaling well with the dimensionality. The Gibbs sampler is a special case of the Metropolis– Hastings algorithm [34], [35], where the proposal densities are the conditionals and the acceptance rate of a random move is constantly one. Although having its origin from image processing in [36], the Gibbs sampler has been extensively used for dynamic state-space models in Bayesian statistics; see examples in [37]–[40]. An essential problem related to an MCMC-based method (and thus the Gibbs sampler) is its high-computational demand, which makes it less feasible in online state estimation applications [41], [42]. Nonetheless, because of its attractive capability in addressing high-dimensional problems, there is some recent work toward the application of the MCMC algorithms for realtime multitarget tracking and analyses, by considering proper optimizations. For instance, Khan et al. [43] reduce the computational cost by adopting the concepts of auxiliary variable particle filter and Rao–Blackwellizing, while Rabaste [39] takes advantage of a hybrid structure of MCMC and importance sampling algorithms. The issues and applicable optimizations of MCMC algorithms for a real-time analysis of dynamic systems are discussed in [44]. DTNs are appropriate candidates for applications where continuous network connectivity is unable to be achieved using traditional networks. DTNs use store-and-forward-type overlay functions for dealing with disconnected subnetworks and aims toward opportunistic transmission [45]. This is shown in Hag-

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

gle Project [46], where agents exchange messages with nearby devices. Berni et al. [47] and [48] show examples of applications of DTNs in complicated underwater scenarios, in which the acoustic communication channel suffers from frequent connection disruptions. The work in [49] introduces the concept of delayed-state decentralized data fusion, with past estimates retained in the statespace vector for managing historical dependences. Approaches based on delayed-state filtering differ from conventional CL ones by maintaining a history of agents states to allow fusion of past data when received. Delayed-state information filter is adopted in both [50] and [51] with a canonical-form representation of Gaussian states kept in its sparse information matrix. However, these existing approaches are parametric and require Gaussian assumptions to be satisfied. III. BACKGROUND OF DELAYED-STATE FILTERING A. Keeping Delayed Observations Define a global observation pool Ωk inside the fusion center at time k, which stores all kinds of received observations (absolute or relative, positive or negative, real-time or time-delayed, etc.) that can be used to constrain estimates of node positions. This paper assumes the use of an existing method to collect these observations from the field. After a trivial extension to additionally take into account internode relative measurements, the observation harvesting proposed in [6] or another similar approach could be used for this purpose. Interested readers could refer to [6] for details on this method. It is also assumed in the paper that the observations have been properly time-stamped; see Section VI-B about how this was done in the experiment. To better describe these observations in Ωk , we use ω kt to denote the collection of every observation that was generated at a historical/present time t (1 ≤ t ≤ k) and is available in the global observation pool at present time k. Note that ω kt could be empty, since the corresponding information might be absent in Ωk . Accordingly, the global observation pool can also be represented as Ωk = ω k1:k . As a subset of the global observation pool, Ωk |k −1 is defined as a finite collection of delayed observations received by the fusion center at time k. It also could be empty, as the fusion center might not receive any information at the particular time slot. Ωk |k −1 can be obtained by a relative complement operation of the global observation pools at two successive time steps, which means Ωk |k −1 = Ωk \ Ωk −1 . B. Bayesian Formulation Delayed-state filtering is based on the product rule in Bayesian estimation, on the basis of Markov assumption. In a single-node tracking case, we have  k  k    P (zt |xt ) P (xt |xt−1 ) P (x0 ) P (x0:k |z1:k ) ∝ t=1

t=1

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

965

Fig. 2. DSCPF. On the arrival of new time-delayed observation(s) within the time window T w , the filter jumps back to perform forward filtering starting from the time point T f f to the present. Optionally, it is followed by backward smoothing process for historical states from T b s back to k − T w + 1.

where xt is the state vector of the node at time t, k is the present time step, P (zt |xt ) is the likelihood function given observation zt , and P (xt |xt−1 ) is the state transition function from t − 1 to t. The full posterior probability density function P (x0:k |z1:k ) is inferred in the use of observations z1:k , regardless of their arrival sequence. Suppose a joint state vector representing a set of Nn nodes moving in a field at time t:  T T   1 T  2 T N n . Xt = xt xt ··· xt Define a sliding time window Tw , within which the states of all nodes moving in the field are cooperatively tracked by a central base station or fusion center. To achieve this, a delayedstate filter ideally keeps the temporal-spatial joint of states from within the time window. Estimates are updated using information from Ωk |k −1 , which has been previously defined to be the collection of delayed observations received at time k. Recursively, we have the joint posterior inferred as   P (Xk −T w +1:k |Ωk ) ∝ P Ωk |k −1 |Xk −T w +1:k  × P (Xk |Xk −1 ) P (Xk −T w :k −1 |Ωk −1 ) dXk −T w . (1) We make the following assumptions. 1) Every node moves independently from which  n in thep field, p . P x |x we have: P (Xk |Xk −1 ) = N p=1 k k −1 2) An egocentric position observation zpt is only dependent on xpt , i.e., the state of node p at time t. (p = q) is only condi3) A relative range observation zp→q t tional on the states of two involved nodes at time t. 4) True node identity knowledge (e.g., p, q) is broadcast with every observation, resulting in perfect data association. 5) Tw is set to be large enough so that for the majority of zpt and zp→q (p = q) in Ωk |k −1 , we have k − Tw + 1 ≤ t ≤ t k. Any observations out of the time window are discarded.

 Therefore, in (1), the observation component P Ωk |k −1 | Xk −T w +1:k ) is able to be factorized to egocentric and relative observations ⎛ ⎞    P Ωk |k −1 |Xk −T w +1:k = ⎝ P (zpt |xpt )⎠ z pt ∈Ω k |k −1

⎛

×⎝ z pt →q



⎞ P (zp→q |xpt , xqt )⎠ t

∈Ω k |k −1

where p = q and k − Tw + 1 ≤ t ≤ k. A marginal posterior P (xpt |Ωk ) for node p at an arbitrary time step t within the time window could be obtained by integrating the joint posterior distribution with respect to the states except for xpt in (1)  P (Xt |Ωk ) = P (Xk −T w +1:k |Ωk ) dXk −T w +1:t−1,t+1:k  P (xpt |Ωk ) =

P (Xt |Ωk ) dXt

(2)

where Xt is the joint state vector of the nodes except for node T  . p at time t, i.e., Xt = (xpt )T XTt IV. DELAYED-STATE PARTICLE FILTER FOR COOPERATIVE TRACKING To track the state of every node cooperatively, in total, there are (Tw + 1) × Nn sets of particles to keep track of the complete Tw historical states plus one present state for each of the Nn tracked agents in the sliding time window up to the present time k. The filter jumps to a historical time point to perform forward filtering (followed by backward smoothing, optionally), when new time-delayed observation(s) are received, as shown in Fig. 2.

966

At time k, for each time slot t ∈ [k − Tw , k], we define a particle base keeping Nn collections of L particles to represent the states of Nn nodes  ⎤  ⎡ 1,(i) 1,(i) {xt , wt }Li=1 ∼ P x1t |ω k1:t ⎢  ⎥  ⎢ ⎥ 2,(i) 2,(i) ⎢ {xt , wt }Li=1 ∼ P x2t |ω k1:t ⎥ ⎢ ⎥ k ⎢ ⎥. Θt = ⎢ ⎥ .. ⎢ ⎥ . ⎢ ⎥ ⎣ ⎦  N n ,(i) N n ,(i) L Nn {xt , wt }i=1 ∼ P xt |ω k1:t Thus, to keep track of all states within the time window Tw , we need Tw + 1 such particle bases   Θkk −T w :k = Θkk −T w Θkk −T w +1 · · · Θkk −1 Θkk . (3) This configuration is different to that in traditional particle filters, where a single particle set would be employed to represent the entire temporal-spatial joint state of nodes. A. Forward Filtering Define a starting point of forward filtering Tf f , which determines where the particle filter starts a repropagation process from, when time is evolved from k − 1 to k:   Tf f = fm in Ωk |k −1 . As the earliest time stamp of observations in Ωk |k −1 is found to be Tf f by function fm in (·), the algorithm is optimized to jump back and restart its filtering process from the time point Tf f instead of k − Tw + 1. This is because for 1 ≤ t < Tf f , we have ω k1:t = ω k1:t−1 . For time step t = Tf f to k and each target node p, a special joint posterior is inferred by generalizing the nonparametric cooperative tracking formulation defined in previous work [7]:     PXk t  P Xt |xpt , ω kt P xpt |ω k1:t       P Xt , xpt |ω kt P ω kt |xpt P xpt |ω k1:t−1  ×    = P xpt |ω kt P ω kt |ω k1:t−1     (4) ∝ P ω kt |Xt P xpt |ω k1:t−1  p k  where of the target node P xt |ω 1:t−1 =     pthep prediction P xt |xt−1 P xpt−1 |ω k1:t−1 dxpt−1 . These nodes grouped in Xt are defined as “auxiliary nodes,” which are temporarily aggregated together at a historical/present time point t to help track the “primary node” p. The auxiliary nodes are then marginalized away to obtain the marginal distribution of the primary node state xpt        P xpt |ω k1:t = P Xt |xpt , ω kt P xpt |ω k1:t dXt . (5) By repeating the algorithm in (4) and (5) for every node at each time Tf f ≤ t ≤ k, all nodes are updated cooperatively with both absolute and relative observations in ω k1:t . When the t comes to the present time k, we will have the posterior distribution inferred based on all observations available in the global observation pool, as Ωk = ω k1:k .

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

The joint posterior distribution PXk t in (4) is usually too complicated to sample from directly. Instead, a Gibbs sampler is introduced to sample from conditional distributions. The Gibbs sampler adopted is an extended version of the one described in [7], which was used for real-time filtering. That sampler is now generalized to fit the delayed-state framework. Define Nn variables θ1 , . . . , θN n corresponding to all Nn nodes in the joint posterior distribution. We have a replacing joint distribution written as     P θ1 , . . . , θp−1 , θp , θp+1 , . . . , θN n = P Xt |xpt , ω kt   × P xpt |ω k1:t . We initialize these variables deterministically or randomly and we have θ1 = θ01 , . . . , θN n = θ0N n . The Gibbs sampler draws a sample from the conditional distribution of each variable given the remaining variables at each step. A traversal of all Nn variables is defined as a scan of the sampling. As the scanning process repeats, a Gibbs chain is produced. In the Gibbs sampling, a relative observation could be converted to an absolute referenced observation. Given zqt →r and zrt →q and xrt = θjr at a scan j ≥ 0, for convenience, we join two likelihood functions together and, finally, denote the converted likelihood function as r      qt ↔θ j  P zqt →r |xqt , xrt = θjr P zrt →q |xrt = θjr , xqt . Ψ At the first iteration, if nodes are assumed to be mutually independent, a priori information of the nodes could be safely incorporated into the filtering. To achieve this, when processing the first iteration, we sample for each node q at each scan j ≥ 1:  +1 n ∝ P (zq1 |xq1 ) θjq ∼ P θq |θj1 , . . . , θjq −1 , θjq −1 , . . . , θjN−1  q −1  N   n  q ↔θ r  r q ↔θ q q q q j j −1   Ψ Ψ × P (x |x ) P (x ) dx 1

0

0

1

0

r =1

1

r =q +1

where zq1 , zq1 →r , zr1 →q ∈ ω k1 . From the second iteration onward, the nodes become crosscorrelated. Hence, for each time step k > 1 and for each auxiliary node q at each scan j ≥ 1, we sample  +1 n , . . . , θjN−1 θjq ∼ P θq |θj1 , . . . , θjq −1 , θjq −1  N   q −1 n   r r q ↔θ q ↔θ t j  t j −1 Ψ Ψ ∝ P (zqt |xqt ) (6) r =1

zqt , zqt →r , zrt →q

r =q +1

∈ where Then, for the primary node p at the jth scan, we sample  θjp ∼ P θp |θj1 , . . . , θjp−1 , θjp+1 , . . . , θjN n ⎞ ⎛ Nn  r   j ⎠  p↔θ ∝ P (zpt |xpt ) ⎝ Ψ P xpt |ω k1:t−1 (7) t ω kt .

r =1,r = p

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

TABLE I ALGORITHM: FORWARD FILTERING   Θ kt ← Forward Filtering Θ kt −1 , ω kt 1: par for p = 1 to N n do p ,(i) p ,(i) 2: load particle set {x t −1 , w t −1 }Li= 1 from Θ kt −1 3: initialize Gibbs variables θ 1 = θ 01 , · · · , θ N n = θ 0N n 4: for j = 1 to (N b p + N s i L ) do Sampling for each auxiliary node: 5: for q = 1  to p − 1, p + 1 to N n do 6: 7: 8: 9: 10: 11:

θ jq ∼ P

+1 n θ q |θ j1 , · · · , θ jq −1 , θ jq −1 , · · · , θ jN−1

with delayed observations from ω kt fused in end for Samplingfor the primary node: θ jp ∼ P

θ p |θ j1 , · · · , θ jp −1 , θ jp + 1 , · · · , θ jN n

with delayed observations from ω kt fused in if (j − N b p ) is an integer times of N s i do Extraction: particle index i = (j − N b p ) /N s i p ,(i) save sample x t = θ jp p ,(i)

12: set weight w t = 1/L 13: end if 14: end for p ,(i) p ,(i) L , wt }i = 1 to Θ t 15: save posterior particles {x t 16: end for

   where zpt , zp→r , zrt →p∈ ω kt and P xpt |ω k1:t−1 = P (xpt | t xpt−1 P xpt−1 |ω k1:t−1 dxpt−1 . Iterative runs of (6) and (7) generate a Gibbs chain. Following a sufficient burn-in period (of, say, Nbp scans), the chain approaches its stationary distribution. From the Gibbs chain, which has its effective length represented as Ncl scans after the burn-in period, we then extract a set of particles to approximate posterior distribution of the primary node   the marginal P xpt |ω k1:t . Theoretically, the desired posterior distribution could be approximated to any degree of accuracy with sufficient number of particles. To reduce autocorrelation, samples are extracted with a sampling interval of Nsi (or a thinning ratio of 1/Nsi in other words), i.e., by taking every (Nsi ) th value in the chain. Please note that the extracted posterior particles have equal weights; therefore, the resampling process to the posterior particle set is not required any more. By repeating the above algorithm for every node at time t, all nodes are updated cooperatively with both egocentric and relative observations in ω kt . The pseudocode of the algorithm is presented in Table I. In the naive concept of the algorithm, each extraction from the Gibbs chain produces only one posterior particle for the primary node, which means in order to collect in total L posterior particles, the chain should meet a length of exactly Ncl = Nsi L scans after the burn-in period. The efficiency of the Gibbs sampling process can be improved by having the primary node preprocessed with its egocentric observations zpt before the Gibbs sampling loop, so as to accelerate the in-loop processing. Meanwhile, when it comes to each (Nsi ) th scan in the Gibbs chain after the burn-in period, we extract and accumulate the updated set of the primary node’s weights. The accumulated weights are normalized upon finishing the sampling loop, to yield the posterior particle set. With this improvement, the quantity of

967

posterior particles L is constantly kept identical to the prior’s, regardless of the chain length Ncl . This could be used to reduce the computation cost as Ncl becomes independent of Nsi and L. Nevertheless, the resampling process should be used to prevent  weight degeneracy, when the quantity of effective particles N eff is below a threshold Nthr . Other optimization methods include: 1) construction of regional uninformative distributions for auxiliary nodes; and 2) selection of informative auxiliary nodes from neighbors, to conserve both memory and computation overheads. These are detailed in [7], where the effects of different Gibbs sampler parameters (the Gibbs chain length Ncl , the Gibbs sampling interval Nsi , and the effective sample size Nes ) on tracking performance in MANET scenarios are also evaluated. B. Backward Smoothing In the forward filtering process, a particle set kept in Θkt approximates the distribution of a node at time t by fusing observations up to the time point, a subset in the global observation pool Ωk . Equation (2) for delayed-state filtering III-B, how  in Section ever, computes P (xpt |Ωk ) (equals P xpt |ω k1:k ), which means xpt is not only filtered by observations up to time t (i.e., ω k1:t ), but also smoothed by information up to a later time k (i.e., ω kt+1:k ). Obviously, estimates based on P (xpt |Ωk ) tend to be   more certain than that on P xpt |ω k1:t , unless t = k. The smoothed distribution P (xpt |Ωk ) could be elaborated to a recursive form:       p k  P xt |ω 1:k = P xpt |xpt+1 , ω k1:t P xpt+1 |ω k1:k dxpt+1       P xpt |ω k1:t P xpt+1 |xpt   = P xpt+1 |ω k1:k dxpt+1 P xpt+1 |ω k1:t       p k  P xpt+1 |xpt P xpt+1 |ω k1:k      p = P xt |ω 1:t dxp . (8) P xt+1 |xpt P xpt |ω k1:t dxpt t+1 

of the smoothed  Equation (8) enables the obtaining P xpt |ω k1:k , given the original P xpt |ω k1:t at time t and P xpt+1 |ω k1:k , which is the smoothed density function at the next time step. To represent the smoothed distributions, we introduce additional particle bases similar to those defined in (3)   ∗k Θ∗k Θ∗k Θ∗k Θ∗k k −t w +1:k = Θk −t w +1 k −t w +2 · · · k −1 k where

Θ∗k t

⎡ ⎢ ⎢ ⎢ ⎢ =⎢ ⎢ ⎢ ⎢ ⎣

∗1,(i) L }i=1

1,(i)

, wt

2,(i)

, wt

{xt {xt

∗2,(i) L }i=1

  ∼ P x1t |Ωk   ∼ P x2t |Ωk

.. . N n ,(i)

{xt

∗N n ,(i) L }i=1

, wt

 n ∼ P xN t |Ωk

Define a starting point of backward smoothing as Tbs = fm ax (Ωk ) − 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎥. ⎥ ⎥ ⎥ ⎦

968

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

TABLE II ALGORITHM: BACKWARD SMOOTHING

TABLE III ALGORITHM: DSCPF

 k ∗k Θ ∗k t ← Backward Smoothing Θ t , Θ t + 1 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11:

k Θ ∗k k −t w + 1 : k , Θ k −t w

par for p = 1 to N n do p ,(i) p ,(i) L load particle set {x t , wt }i = 1 from Θ kt p ,(i) ∗p , ( i ) load particle set {x t + 1 , w t + 1 }Li= 1 from Θ ∗k t+1 for j = 1 to L do  L p ,(j ) p ,(m ) p ,(m ) (j ) α wt = x t + 1 |x t m =1 P end for for i = 1 to L do ∗p , ( i )

wt

p ,(i)

= wt

L j=1

P



x

p ,(j ) p ,(i) |x t t+1 α (j )



w

∗p , ( j ) t+1

end for p ,(i) ∗p , ( i ) L save smoothed particles {x t , wt }i = 1 to Θ ∗k t end for

where function fm ax (·) finds the largest time stamp of observations in the global observation pool Ωk . The algorithm is optimized to start its smoothing process from the time point Tbs back to k − Tw + 1, instead of from time k − 1. This is because ω k1:t = ω k1:k , when Tbs < t ≤ k. Accordingly, each Θkt is copied to Θ∗k t in the particles bases for all Tbs < t ≤ k. Then, for each node p, for t = Tbs , Tbs − 1, . . . , k − tw + 1 and i = 1 : L, recursively, we have the smoothed particle weights calculated as  p,(j ) p,(i) ∗p,(j ) L wt+1 P xt+1 |xt  ∗p,(i) p,(i)  = wt . wt L p,(j ) p,(m ) p,(m ) wt j =1 m =1 P xt+1 |xt To sum up, the smoothing is processed with particles reweighted to obtain an approximation to the smoothed density, while the original particle locations are maintained. The resulp,(i) ∗p,(i) L tant particle set {xt , wt }i=1 approximates the marginal p distribution P (xt |Ωk ) of node p, which has the identical form as that yielded in (2). The backward smoothing algorithm is summarized in Table II in the form of pseudocode. The smoothed estimates are extracted from the additional particles bases Θ∗k k −t w +1:k . However, it is important to note that only filtered particle bases Θkk −t w :k are kept and evolved to the next time step, while Θ∗k k −t w +1:k are calculated at every new step. More information about the backward smoothing is available in [52] and [53]. The particles reweighted by the smoothing process are only for historical states. The last/present state estimates would always be identical before and after smoothing. For this reason, the backward smoothing process in the DSCPF is optional. It could be skipped to conserve the computational cost in applications where the estimation of present states is primarily of concern. The pseudocode of the whole DSCPF algorithm is presented in Table III. C. Computational Complexity When the DSCPF is implemented in a centralized tracking framework, and the network is assumed to be fully connected (which means every node has a direct connection to every node

:k

 ← DSCPF Θ kk −1 −t w −1 : k −1 , Ω k , Ω k −1

Forward filtering: 1: Ω k |k −1 = Ω k \ Ω k −1 2: T f f = f m i n Ω k |k −1 3: for t = k − T w to T f f − 1 do 4: for p = 1 to N n do p ,(i) p ,(i) L 5: copy particles {x t , wt }i = 1 from Θ kt −1 to Θ kt 6: end for 7: end for 8: for t = T f f to k do 9: extract ω kt from Ω k   10: Θ kt ← Forward Filtering Θ kt −1 , ω kt 11: end for Backward smoothing (optional): 12: T b s = f m a x (Ω k ) − 1 13: for t = T b s + 1 to k do 14: for p = 1 to N n do p ,(i) p ,(i) L 15: copy particles {x t , wt }i = 1 from Θ kt to Θ ∗k t 16: end for 17: end for 18: for t = T b s to k − T w + 1 do  k ∗k 19: Θ ∗k t ← Backward Smoothing Θ t , Θ t + 1 20: end for

else, all the time), the overall time complexity of its forward filtering part (for both the naive and improved versions) is  O Tw Ncl Na Nn3 in this worst case. Again in a fully connected network, if the tracking is realized in a decentralized architecture, where a node treats itself as the only primary node, the computation burden is then distributed overall Nn nodes,and the time complexity could be reduced to O Tw Ncl Na Nn2 . In either tracking framework, raw egocentric and pairwise observations data are shared among the nodes, and the communication bandwidth cost for each node in the network is a quadratic function of Nn . The intended application of the proposed DSCPF algorithm is sparse mesh networks. In this environment, there is only intermittent communication between nodes with a very limited number of neighbors for most of the time, resulting in sporadic P2P observations. The experiment in this paper is an example of this type of application. This substantially reduces the auxiliary node quantity and internode measurements involved within each Gibbs sampling loop and, consequently, lowers the magnitude of the computational complexity and communication overheads. Even if the DSCPF is implemented in a denser network, the complexity magnitude could be reduced by pruning neighboring nodes that are not considered sufficiently “informative” for the tracking of the primary node [7]. This requires a tradeoff between reduced computation time and reduced tracking accuracy due to information loss. By assuming a capped quantity of auxiliary nodes in either scenario aforementioned, the time complexity of the forward filtering part would be significantly reduced to O (Tw Ncl LNn ) in a centralized framework, thanks to quicker computations from Lines 5–8 in Table I. It further drops to O (Tw Ncl L) when nodes are tracked in a decentralized manner. Note that they are,

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

    respectively, rewritten as O Tw Nsi L2 Nn and O Tw Nsi L2 in the DSCPF with the naive forward filtering version, in which Ncl = Nsi L. The time complexity of the backward  smoothing  part is represented as O Tw L2 Nn or O Tw L2 in a centralized or decentralized tracking architecture, respectively. In summary, the above analysis indicates a polynomial time complexity for the proposed DSCPF algorithm with node quantity Nn and proves reasonable scalability of the algorithm with the number of nodes. As a comparison, the traditional bootstrap particle filter suffers from exponential time complexity in the quantity of tracked targets [54]. Usually, a delayed-state cooperative tracking problem is high dimensional as adding a node imposes an increase of Tw dim (x) dimensions upon the joint’s dimension dim (Xk −T w +1:k ). The curse of dimensionality makes the bootstrap particle filter an impractical approach to such kinds of high-dimensional problems, while the DSCPF overcomes this limitation by exploiting a Gibbs sampler, because drawing samples from a lowdimensional conditional distribution is generally more efficient than drawing from the high-dimensional joint.

969

Fig. 3. Non-Gaussian distribution and its Gaussian approximation. The solid blue curve shows a non-Gaussian Student’s t-distribution as an example of the non-Gaussian distribution, defined by St (0, λ, ν), where ν = 3 is degrees of freedom, and the precision λ = 0.0133. is shown  Its Gaussian approximation 1 ν as a dashed line, represented by N 0, σ 2 , where σ = = 15. The λ ν −2 Student’s t is heavier tailed, meaning that a Gaussian approximation is overconfident in excluding the tail probability. This distribution is used to generate the simulated data to highlight the benefits of using a nonparametric filter.

V. TEST WITH NON-GAUSSIAN CASES A. Setup The proposed delayed-state cooperative tracking algorithm has been tested in N = 30 Monte Carlo linear and non-Gaussian cases. The simulation is setup with seven mobile nodes moving within 1-D space. Only four of the mobile nodes have selflocalization capability (GPS providing absolute global position measurements). Each node is equipped with a radio device to communicate directly with nearby nodes, and the signal strength provides an indication of the relative distance. For the convenience of analysis, the communication range is assumed to be long enough so that a fully connected network is formed. Both the absolute and relative measurements from these nodes are forwarded to a central location for global tracking of the entire fleet. It should be noted that the infrastructure is only for data gathering and fusion during the simulation, and it does not assist in the localization or tracking. Different to the assumption of real-time observations usually made in conventional scenarios, we assume that every observation will arrive at the fusion center with some random time delays. The fusion center uses the delayed observations collected to track every node in the field. With the cooperative tracking introduced, relative distance observations between nodes help improve tracking accuracy. Particularly, the nodes without egocentric position awareness can only be localized and tracked using the internode distance measurements. The tracking performance of the proposed DSCPF is compared with that of delayed-state Kalman filter (DSKF) in the simulation. A Student’s t-distribution illustrated in Fig. 3 is chosen to generate independent and identically distributed (i.i.d.) non-Gaussian noise on relative range measurements between nodes. It is similar to Gaussian but comparatively heavier tailed, which means that it is more likely to produce samples that fall far from the mean. The DSCPF employs a nonparametric ob-

Fig. 4. Random time delays of observations. The pattern of the histogram generally follows a binomial-like distribution. The count values of the last three bins (7, 8, and 9) are hard to determine from the figure. Their values are 3122, 539, and 21, respectively.

servation model to deal with the non-Gaussian noise, while the DSKF’s observation model has to model the noise using a Gaussian approximation, as illustrated in Fig. 3. All other components used in the simulation are linear and Gaussian and are identical for both approaches. In addition, unbiased measurements and whiteness of noise in kinematic and measurement models are assumed. These ensure that only the relative observation models will account for any differences in the tracking results. Initially, Nn = 7 mobile nodes are positioned with even intervals. During the simulation, every mobile node moves with random velocities. Both absolute and relative observations arrive with random time delays, which are set to present a binomiallike probability distribution within a time window (see Fig. 4). In the simulation, both filters are tracking the state of each node, which is composed of position and velocity in the 1-D space, T  i.e., x = rx vx . The simulation is initialized with parameters given in Table IV. These parameters are determined in order to produce conclusive results. The algorithm calculates one iteration per second of time. Some location errors are intentionally added to every node in the initial state. A narrow sliding time window is chosen to be 10 s. This is because the motion model used in the simulation is not designed for long-term prediction. In addition, the particle filter is initialized with L = 2000 particles for each primary node and Na = 500 only for every auxiliary node. Note that the forward filtering of the DSCPF used in

970

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

TABLE IV SIMULATION PARAMETERS Parameter

Value

total Monte Carlo simulation cases total iterations per case total mobile nodes N n time window size T w total mobile nodes with GPS enabled i.i.d. speed noise i.i.d. position observation noise i.i.d. range observation noise in DSKF i.i.d. range observation noise in DSCPF particle quantity for primary node L particle quantity for auxiliary node N a burn-in length of Gibbs sampler N b p interval of Gibbs sampling N s i effective length of Gibbs chain N c l

30 200 7 10 s 4   2 N 0,  0.22  m/s N 0, 5  m N 0, 15 2 m S t (0, 0.0133, 3) m 2000 500 200 5 2000

Fig. 5. Mean RMSEs of position estimates of nodes (in meters). The bottom three are those without GPS, and they have to be localized by relative distances to neighbors. Generally, those nodes with GPS enabled are observed being tracked more precisely.

the simulation is the optimized version with primary node preprocessing and weights accumulation (introduced at the end of Section IV-A). This explains why a chain length of Ncl = 2000 scans is adopted, much shorter than the supposed value of Nsi L otherwise. B. Performance Metrics To quantify the tracking accuracy, root-mean-square error (RMSE) is used as a measure of performance. The RMSE of position for overall Nn mobile nodes could be computed by ! 2 N n T w  p,t ˆx − rxp,t p=1 t=1 r RMSE = Nn × T w where rxp,t is the true position of node p at time t, and rˆxp,t is the estimate. Overall mean RMSE is then calculated by averaging over N = 30 independent Monte Carlo cases. Mean RMSE for an individual node could also be obtained by averaging RMSEs of the particular node in these cases. In addition, normalized estimation error squared (NEES) is adopted in the test as the metrics of consistency of the two tracking approaches. The NEES value for a given mobile node at time k is calculated by xk − x k )  (k) = (ˆ xk − xk )T P−1 k (ˆ ˆ k is the state estimate at time k, xk is its true value, and where x Pk is its associated covariance calculated by the filter. The consistency test is based on averaging NEES results over N Monte Carlo runs of a filter, which yields N 1  i  (k) . ¯ (k) = N i=1

Then, N ¯ (k) will have a χ2 (chi-square) distribution with N dim (xk ) degrees of freedom [55] [13], under the hypothesis that the tested filter is consistent and approximately linear and Gaussian. The state estimation errors are considered consistent with the filter-calculated covariances if ¯ (k) ∈ [r1 , r2 ], where the interval [r1 , r2 ] bounds the two-

Fig. 6. DSCPF without versus with smoothing. The mean RMSEs of position estimates of node #1 (without GPS) when smoothing is not enabled in DSCPF are shown in (a), while the results with smoothing enabled could be found in (b). The results are in meters. For a clear demonstration, the figure only presents and compares results within the time period from 115 to 144 s. From left to right, each horizontal line in (a) or (b) represents results of historical states and the last state at a particular time step. Despite the identical results for the last states in (a) and (b), the DSCPF with smoothing generally yields more accurate estimates (also with smoother variations) for historical states.

sided  calculated  95% probability concentration region and is 2 2 by χN dim (x k ) (0.025) /N, χN dim (x k ) (0.975) /N . A filter tends to produce optimistic estimates if ¯ (k) rises significantly higher than the upper bound, while if it stays below the lower bound for a majority of time, the filter is considered conservative [56]. C. Results The mean RMSEs of positions for all seven mobile nodes in the DSCPF tracking result are illustrated in Fig. 5. As observed, the algorithm tracks GPS enabled nodes at a higher accuracy level, with both absolute and relative measurements provided, than the nodes without GPS, which have to be localized by distance measurements from and to their neighbors. Fig. 6 gives details and makes a comparison between the tracking results with and without backward smoothing enabled in DSCPF. When it is enabled, estimates of past states could be improved by

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

971

Fig. 8. NEES consistency test for the tracking of node #1. The in-bound rate of the DSCPF tracking result is about 93.72%, close to the optimal value 95%, while the value for DSKF is only 59.16%.

Fig. 7. Mean RMSEs of positions in the non-Gaussian cases. For a clear display, only results of the last 100 s are presented. The tracking result of the noncooperative DSKF for nodes without GPS is not shown in the top figure as the independent tracking algorithm fails in such scenarios.

observations up to a later time. Also noteworthy is the identical last state estimates in the two DSCPFs. In terms of the overall mean RMSE, Fig. 7 compares the tracking results of the DSCPF algorithm and the DSKF in these linear and non-Gaussian cases. It could be concluded from the figure that the DSCPF outperforms the DSKF in tracking accuracy for both node types (with and without GPS). The tracking result from a noncooperative version of DSKF is also illustrated, where nodes are tracked independently. The advantage of cooperative tracking is clearly demonstrated in the figure. The DSCPF also tracks nodes with better estimation consistency than the DSKF does, as suggested by Fig. 8. The figure illustrates the result for one of the nodes, and a similar result is obtained about every other node. As previously mentioned, the Student’s t-distribution is more likely to generate samples that sit far away from its mean because of its “longer tails” than its Gaussian approximation. However, the DSKF is sensitive to the presence of observations that are considered “outliers,” i.e., those fall further than what its Gaussian observation model predicts. Consequently, an overoptimistic tracking result is observed for the DSKF in Fig. 8. This is not an issue to the DSCPF, as it has the real distribution of noise in observations more precisely modeled. It should be noted that the t-distribution is only one of many potential non-Gaussian representations for random variable noise, and this distribution in particular with heavy tails is often found in real-world applications. The differences between approaches have been clearly observed even when only one nonGaussian component (i.e., the relative distance observations) is modeled for this simulation. And this slightly non-Gaussian

property has caused noticeably inferior tracking accuracy and consistency of the DSKF according to the results. One can argue that the DSKF could be improved by using a Gaussian sum [22], [57] in its observation model to better approximate the non-Gaussian noise in this simple simulation. In any case, it is still hard to guarantee an accurate and efficient model parameterization process when parametric/semiparametric methods are needed to represent complex systems. Consider a tracking system with a higher degree of non-Gaussianity and/or nonlinearity, e.g., the experiment presented in the next section; the DSKF could not maintain tracking consistency. From this point of view, despite a higher computational cost than that of parametric approaches, the DSCPF is necessary for many cooperative tracking applications. VI. EXPERIMENTAL VALIDATION A. Experiment Setup Data from a working industrial operation were used to demonstrate the cooperative tracking algorithm presented. Three vehicles (V1, V2, and V3) were tracked with only one of them equipped with GPS module Fastrax IT600 and one infrastructure data collection point (C1) installed in the field. Fig. 9 shows the vehicle routes on the experiment field map, which is automatically generated and continuously updated using raw GPS data that are collected as part of the everyday vehicle operation in the field [58]. More specifically, two out of these three vehicles, V1 and V3, were not provided with GPS information, and they ran together with an almost constant distance between them. This means that the tracking algorithm in this experiment has to rely more on vehicle-to-vehicle (V2V) relative observations. All of these vehicles and the collection point could communicate using dual-band radios: 433-MHz Texas Instruments C1101 transceivers and 2.4-GHz standard 802.11b Wi-Fi. Each vehicle had the V2V relative observations together with other delayed observations brought to C1 and immediately the

972

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

Fig. 11. Discrete likelihood of distance given connectivity (left) and RSSI observation (right) in V2V communication as inputs. Values are normalized so that elements in each column sum to one.

Fig. 9. Vehicles routes on field map. Three mobile agents were running in a field with only one data collection point installed near intersection #205. The data collection point established a signal circle with a radius of 250 m. The figure also shows vehicles’ routes on the field map. V1 and V3 initialized a trip on the road next to area #208, whereas V2 was coming from a road near intersection #203.

Fig. 10. Time delays of different types of in total 1579 observations received by the base station. All V2I observations were real time (i.e., time delay equals zero), while those observations brought back by vehicles (egocentric GPS and V2V measurements) were mostly delayed. Negative V2I and V2V detections also have been included in the count.

central base station through an improved version of the observation harvesting method originally described in [6]. These V2V measurements harvested and vehicle-to-infrastructure (V2I) observations from C1 were then interpreted by a probabilistic proximity observation model, which will be detailed in Section VI-C. The distribution of actual time delays in observations received by the base station is presented in Fig. 10. As a comparison, the experiment was also run with a noncooperative tracking configuration, where intervehicle communication (and thus V2V information) was disabled. The long-term vehicle motion prediction approach proposed in [59] was adopted to predict positions of vehicles when they were not in the commu-

Fig. 12. Examples of the likelihood of proximity between vehicles. (a) demonstrates the likelihood of distance given an NC observation in V2V communication. The likelihood given a V2V PC observation and an RSSI reading of -85 dBm is presented in (b), while (c) shows the joint likelihood given multiple V2V observations (two PC observations with RSSI values and one NC observation) from three observer vehicles.

nication range of the infrastructure. The vehicle motion model was constructed based on historical data of 600 h (25 days) collected from five vehicles. The substantial time delays in observations and the severely nonlinear and non-Gaussian properties contained in the system models well justify the use of the proposed delayed-state nonparametric tracking approach in the experiment. The backward smoothing process was not enabled in the DSCPF algorithm when used for the experiment, as only the latest state estimates were considered important. We used the optimized forward filtering in the DSCPF, like the one used in the simulation. The DSCPF algorithm was run an iteration per second, with 5000 particles for every primary node. The length of Gibbs chain Ncl was set to 5000, and the sampling interval Nsi was set to 5, in order to produce explicit tracking results. The sliding time window Tw was set to 180 s. B. Time-Stamping of Measurements GPS time is used as the global timing source in the vehicle network. The local time in a vehicle with GPS is kept synchronized to the GPS time so as to precisely time-stamp its observations, while observations generated by a vehicle without GPS time are first time-stamped by its local free-running clock. Suppose an observation in vehicle p with a local time stamp Tp in this case. When it is transmitted to node q, Tp is then converted to the recip ient’s local time stamp Tq by taking into account the local time offset between the two communicating nodes. Given that when the communication happens, the current local time of vehicle p

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

973

Fig. 13. Interactions in V2I and V2V. (a) and (b) reveal change of relative distance in each of the three vehicle pairs with time, and between vehicles and the data collection point, respectively. Troughs in distance lines represent physical proximity points of two nodes. In (c) and (d), blue strips denote that successful communication between nodes was achieved, and blank otherwise. More specifically, T V 3 →V 2 is found to be 111 s, and T V 1 →V 2 to be 125 s. For V2I communication in (d), T V 2 →C 1 = 126 s, T V 3 →C 1 = 131 s and T V 1 →C 1 = 154 s. As V1 and V3 were traveling close together, we could observe comparatively more V2V communication activities between them throughout the trip time.

and node q is Tp and Tq , respectively, the time-stamp conversion process is then represented as Tq = Tp + (Tq − Tp ), assuming negligible time delays introduced in the conversion process. The local time stamp is ultimately translated to a global time stamp once the recipient is a vehicle with GPS time, or the base station. Let εm ax denote a maximum tolerant value of timing errors in the global time stamps that are converted from local ones so that the errors do not impose considerable detrimental effects on tracking performance. Recall that every observation to be collected and utilized by the fusion center should have a time delay no greater than Tw (it is discarded otherwise), the local free-running clock source is required to has a drift rate (or clock precision) Rd ≤ εm ax /Tw . This is not a difficult requirement in this experiment, given εm ax ∼ 10−2 s, Tw = 180 s, and that the vehicles’ local clocks are driven by computergrade quartz crystal oscillators, which have a typical drift rate Rd ∼ 10−6 [60]. C. Vehicle Proximity Detection Model A probabilistic vehicle proximity detection model was utilized in the experiment. This enables the filter to infer the approximate position of a vehicle relative to another vehicle using V2V, or to a fixed data collection point using V2I relative information. Specifically, the required observation data are composed of wireless connectivity and RSSI measurements. The advantage of using these measurements is that no additional bandwidth is required to measure RSSI and connectivity since this information is a by-product of the communication process. The data association problem is solved by transmitting a node identification with each data packet, which also enables “negative detections” when transmissions are not received from a given node. In the experiment, the relative information was provided by the 433-MHz wireless transceivers installed in each vehicle and infrastructure node. The 2.4-GHz band was mainly used for large-bandwidth data transmissions. The proximity de-

tection model was built with more than 0.75 million historical V2V and V2I communication samples. The binary connectivity measurement could be either Positive Connectivity (PC) or Negative Connectivity (NC), respectively, corresponding to the positive or negative/missed detection of a node in the proximity. Every observation of a PC event is accompanied by an RSSI measurement in the communication channel, which means the RSSI measurement is conditional on the occurrence of a PC observation. Thus, the joint likelihood function is written as P (z p→q = ztp→q |xpt , xqt ) = P (z r,p→q = ztr,p→q |xpt , xqt , z c,p→q = P C) " #$ % likeliho o d given RSSI

× P (z "

c,p→q

= PC |xpt , xqt ) #$ %

likeliho o d given connectivity

where ztp→q here is an observation set containing a PC observation ztc,p→q = P C and its associated RSSI reading ztr,p→q , both measured by observer q about node p at time t. If a node does not receive a packet from another node, an NC event occurs and no RSSI measurement is available. The likelihood function in this situation is denoted by P (z p→q = ztp→q |xpt , xqt ) = P (ztc,p→q = NC |xpt , xqt ) " #$ % likeliho o d given connectivity

ztp→q

where only includes an NC observation ztc,p→q = NC . Based on the historical probability of connectivity and histograms of RSSI data at different ranges, we built a V2V proximity detection sensor model in the form of likelihood matrices shown in Fig. 11, with the range divided into three partitions from 0 to infinity. Some examples of likelihood functions given V2V relative observations are shown in Fig. 12. The sensor model for V2I proximity detection was constructed in a similar way.

974

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

Fig. 14. Cooperative tracking results. In the experiment, V2 met V3 followed by V1. The base station had to estimate the positions of V3 and V1 by prediction until they established V2I communication with C1 at time T V 3 →C 1 (131 s) and T V 1 →C 1 (154 s), respectively. With cooperative tracking enabled, the positive V2V relative range observation between V2 and V3 at around time T V 3 →V 2 (111 s) were known to the base station via V2, which was in the communication range of C1. The base station was able to update the position estimate of V3 [see (d)], even when it was out of contact with the data collection point. The noncooperative tracking result only shows an update in estimation when V3 arrived at C1 at time T V 3 →C 1 , as demonstrated in (a). Similarly, V2 brought the relative measurements about V1 at around time T V 1 →V 2 (125 s) to the base station in the cooperative tracking, and the position estimate of V1 got updated, as shown in (e). Noteworthy is that, after V3 established communication with C1 at time T V 3 →C 1 , the relative range observations between V1 and V3 were available to the base station via V3. Consequently, the position of V1 was better estimated after that time point, as illustrated in (e). The noncooperative tracking result for V1 is given in (b) for a comparison. The tracking results of V2 with cooperative and noncooperative tracking are almost the same, as, respectively, shown in (f) and (c). The position distribution at a particular time point, e.g., V3 at time 125 s in (a), shows a pattern with the majority probability mass concentrated on a head followed by a long thin tail. This could be explained by a non-zero stopping probability on the road assumed in the long-term vehicle motion prediction model. A drift of mean position in the estimation is observed in (a) because of the incorporation of negative V2I connectivity information in the filtering. Right before T V 3 →C 1 , the tracking algorithm believed that V3 was probably still out of C1’s communication range, according to NC observations between them. This negative information gradually eliminated the head part of the position distribution while the probability mass of the tail increased, causing drifting of the mean position. Once V2I communication was established between V3 and C1 at T V 3 →C 1 , the drift was corrected by the algorithm by wiping out the position probability mass outside the signal coverage of C1. A similar case also happened to V1 in (b). In (c) and (d), intersection #205 is displayed as a block, because an area/intersection is considered as a single position slot in the tracking algorithm.

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

975

Fig. 15. Comparisons of estimation errors and entropies. (a) suggests generally smaller tracking errors for V3 with cooperative tracking, starting before the time T V 3 →V 2 (111 s) due to the negative V2V relative information enabled in cooperative tracking. The entropies of V3 in (b) also began to descend at the time point, when it was tracked in a cooperative way. In (c), the position errors of V1 with cooperative tracking initially rose before time T V 1 →V 2 (125 s) and soon dropped, and its entropies was reduced from T V 1 →V 2 onward in (d). The tracking results of V2 with cooperative or noncooperative tracking are not illustrated in the figure, as they did not show a noticeable difference.

D. Entropy of Target Agent(s) Entropy [10] is adopted as an indicator of the uncertainty contained in position estimates of the target vehicles. As in [59], each road segment connecting two intersections or areas (as shown in Fig. 9) is first discretized evenly into continuous “position slots.” An area/intersection is also taken into account as an individual position slot for simplicity. Then, the probability mass on position slot s could be calculated by summing up all weights of particles on the slot at time k, i.e.,  wki . pmk (s) = r ik ∈s

The entropy for a mobile agent at time k is calculated by H (xk ) = −

n 

pmk (s) log2 pmk (s) .

s=1

E. Experiment Results The relative distance and communication activities of V2V and V2I are presented in Fig. 13. Some important time points in the communication activities are summarized as follows. 1) TV 3→V 2 : The first time V2 observed V3. 2) TV 1→V 2 : The first time V2 observed V1. 3) TV 2→C 1 : The last communication between V2 and C1. 4) TV 3→C 1 : The first communication between V3 and C1. 5) TV 1→C 1 : The first communication between V1 and C1. V2 first arrived at intersection #205 and drove north. During its trip, it met V3 at time TV 3→V 2 and then V1 at TV 1→V 2 , which were moving together to the south. Finally, V3 followed by V1 drove to C1 at TV 3→C 1 and TV 1→C 1 , respectively. The motion model tended to predict the positions of V3 faster than the true values due to a slow start up of V3. Before the time point TV 3→V 2 , V2 did not detect V3 in its communication range. This negative detection information (i.e., NC events) was soon

known by C1 through V2I communication and helped correct the faster prediction of V2’s position. V2 had a positive relative range measurement about V3 through V2V communication with it at the time point, which was again known by the base station and used to improve the estimate of the position of V3. This is demonstrated in Fig. 14(d) at around the time marked TV 3→V 2 . Noteworthy is that V3 did not have egocentric GPS updates; therefore, the estimate improvement was contributed by the V2V relative information and the GPS update of V2. The position estimate was further refined when V3 established V2I communication with C1, as illustrated in Fig. 14(d) at the time marked TV 3→C 1 . Likewise, V1 did not have GPS information and its position estimate was first improved at time TV 1→V 2 , when it had V2V communication with V2. During the time TV 3→C 1 and TV 1→C 1 , the estimate was further improved a few times when V3 periodically uploaded positive V2V relative observations about V1 to the base station through C1. Finally, the position estimate was updated when it had a direct V2I transaction with C1, which is illustrated at the time TV 1→C 1 in Fig. 14(e). Estimation results without cooperative tracking are demonstrated in Fig. 14(a) and (b). The comparison of the results demonstrates an improvement in tracking performance by the use of cooperative tracking, which incorporates the relative measurements between vehicles. This could also be concluded by quantitatively comparing estimation errors and entropies of V3 and V1, as illustrated in Fig. 15. The tracking of V2 does not show much difference with cooperative or noncooperative tracking, as, respectively, shown in Fig. 14(f) and (c). This could be explained by that, V2 kept in contact with C1 for most of the time. V2 moved out of the communication range of C1 at the time TV 2→C 1 . After that, there was a position update in the cooperative tracking when V1 brought the delayed GPS update of V2 to C1. The improvement, nevertheless, is not obvious enough when we compare the two figures.

976

IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 4, AUGUST 2015

VII. CONCLUSION AND FUTURE WORK This paper has presented a novel particle filter to cooperatively track multiple agents with time-delayed observations. When a globally connected network is practically infeasible, which is often the case in large environments, egocentric position and relative range information can only be shared among neighboring (connected) mobile nodes and forwarded opportunistically to the fusion center, inevitably causing some time delays. The proposed nonparametric approach, named DSCPF, significantly extends the previous work in [7] to additionally consider time-delayed information in the cooperative tracking of mobile nodes. Specifically, the forward filtering part of the cooperative tracking approach involves a Gibbs sampler, which replaces the importance sampling in traditional particle filters, and utilizes it for sequential Bayesian inference given received delayed observations. The DSCPF approach can be implemented optionally with backward smoothing, the use of which can further reduce uncertainty since estimates of the past states can be improved using “future” observations. There are several conclusions that could be drawn according to the simulation and experiment results. The first is that the DSCPF, as a nonparametric tracking approach, demonstrates superior performance in the slightly non-Gaussian simulation cases compared with the results of the parametric algorithm. It has also been illustrated in the experiment that internode interactions combined with a limited number of data collection points enable consistent cooperative tracking without the assumptions of full network coverage, and that every mobile node is provided with egocentric position information. Compared with individual tracking scenarios, the cooperative tracking is proven to be beneficial in circumstances where a portion of the nodes lose their location awareness, by incorporating relative distance measurements between neighbors. In addition, the estimates of the nodes with egocentric information available could also be improved. The proposed approach is especially effective in tracking those mobile nodes that rarely or never make contact with the central base station or data collection points, due to geographical segmentation, tasks undertaken, or other reasons. The only requirement is that they are observed by other mobile nodes traveling past so that their information could be carried back to the fusion center. The approach possesses reasonable scalability with the number of nodes and could further benefit from denser node networks, which naturally mean more frequent communication, lower network latency, and more information that can be used to refine position estimates. The future work includes a more detailed investigation into the fundamental principle of the Gibbs sampler-based cooperative tracking framework. In addition, the algorithm will be optimized toward a more efficient sampling process and a further reduced computational cost.

REFERENCES [1] W. Chen and X. Meng, “A cooperative localization scheme for zigbeebased wireless sensor networks,” in Proc. 14th IEEE Int. Conf., Sep. 2006, vol. 2, pp. 1–5.

[2] L.-L. Ong, T. Bailey, and H. Durrant-Whyte, “Decentralized particle filtering for multiple target tracking in wireless sensor networks,” in Proc. 11th Int. Conf. Inf. Fusion, Jun. 2008, pp. 342–349. [3] L. Dong, “Cooperative localization and tracking of mobile ad hoc networks,” IEEE Trans. Signal Process., vol. 60, no. 7, pp. 3907–3913, Jul. 2012. [4] Y. P. Fallah, C.-L. Huang, R. Sengupta, and H. Krishnan, “Analysis of information dissemination in vehicular ad-hoc networks with application to cooperative vehicle safety systems,” IEEE Trans. Veh. Technol., vol. 60, no. 1, pp. 233–247, Jan. 2011. [5] S. Worrall and E. M. Nebot, “Using non-parametric filters and sparse observations to localize a fleet of mining vehicles,” in Proc. IEEE Int. Conf. Robot. Autom., Roma, Italy, Apr. 2007, pp. 509–516. [6] M. Shan, S. Worrall, F. Masson, and E. Nebot, “Using delayed observations for long-term vehicle tracking in large environments,” IEEE Trans. Intell. Transp. Syst., vol. 15, no. 3, pp. 967–981, Jun. 2014. [7] M. Shan, S. Worrall, and E. Nebot, “Nonparametric cooperative tracking in mobile ad-hoc networks,” in Proc. IEEE Int. Conf. Robot. Autom., Hong Kong, May 2014, pp. 423–430. [8] H. Wymeersch, J. Lien, and M. Z. Win, “Cooperative localization in wireless networks,” Proc. IEEE, vol. 97, no. 2, pp. 427–450, Feb. 2009. [9] R. Madhavan, K. Fregene, and L. E. Parker, “Distributed cooperative outdoor multirobot localization and mapping,” Auton. Robots, vol. 17, no. 1, pp. 23–39, Jul. 2004. [10] E. Adamey and U. Ozguner, “Cooperative multitarget tracking and surveillance with mobile sensing agents: A decentralized approach,” presented at the 14th IEEE Int. Conf. Intell. Transp. Syst., Washington, DC, USA, Oct. 2011. [11] N. Patwari, J. N. Ash, S. Kyperountas, A. O. Hero-III, R. L. Moses, and N. S. Correal, “Locating the nodes: Cooperative localization in wireless sensor networks,” IEEE Signal Process. Mag., vol. 22, no. 4, pp. 54–69, Jul. 2005. [12] T. Eren, “Cooperative localization in wireless ad hoc and sensor networks using hybrid distance and bearing (angle of arrival) measurements,” EURASIP J. Wireless Commun. Netw., vol. 2011:72, pp. 541–552, Aug. 2011. [13] A. Bahr, M. R. Walter, and J. J. Leonard, “Consistent cooperative localization,” presented at the IEEE Int. Conf. Robot. Autom., Kobe, Japan, May 2009. [14] G. P. Huang, N. Trawny, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based consistent EKF estimators for multi-robot cooperative localization,” Auton. Robots, vol. 30, no. 1, pp. 99–122, 2011. [15] K. Y. K. Leung, T. D. Barfoot, and H. H. T. Liu, “Decentralized localization of sparsely-communicating robot networks: A centralized-equivalent approach,” IEEE Trans. Robot., vol. 26, no. 1, pp. 62–77, Feb. 2010. [16] N. Patwari, A. O. Hero-III, M. Perkins, N. S. Correal, and R. J. O’Dea, “Relative location estimation in wireless sensor networks,” IEEE Trans. Signal Process., vol. 51, no. 8, pp. 2137–2148, Aug. 2003. [17] E. D. Nerurkar, S. I. Roumeliotis, and A. Martinelli, “Distributed maximum a posteriori estimation for multi-robot cooperative localization,” presented at the IEEE Int. Conf. Robot. Autom., Kobe, Japan, May 2009. [18] N. A. Alsindi, K. Pahlavan, B. Alavi, and X. Li, “A novel cooperative localization algorithm for indoor sensor networks,” presented at the 17th Annu. IEEE Int. Symp. Personal, Indoor Mobile Radio Commun., Helsinki, Finland, Sep. 2006. [19] G. Agamennoni, J. I. Nieto, and E. M. Nebot, “An outlier-robust Kalman filter,” presented at the IEEE Int. Conf. Robot. Autom., Shanghai, China, May 2011. [20] A. Howard, M. J. Matari´c, and G. S. Sukhatme, “Putting the ’‘’ in ‘team’: An ego-centric approach to cooperative localization,” presented at the IEEE Int. Conf. Robot. Autom., Taipei, Taiwan, Sep. 2003. [21] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, “A probabilistic approach to collaborative multi-robot localization,” Auton. Robots, vol. 8, no. 3, pp. 325–344, Jun. 2000. [22] D. Hai, H. Zhang, and Z. Zheng, “Cooperative localization of WSN aided by robot,” in Proc. IEEE Int. Conf. Autom. Logistics, Hong Kong, Aug. 2010, pp. 609–613. [23] A. T. Ihler, J. W. Fisher, R. L. Moses, and A. S. Willsky, “Nonparametric belief propagation for self-localization of sensor networks,” IEEE J. Sel. Areas Commun., vol. 23, no. 4, pp. 809–819, Apr. 2005. [24] V. Savic and S. Zazo, “Cooperative localization in mobile networks using nonparametric variants of belief propagation,” Ad Hoc Netw., vol. 11, pp. 138–150, 2013.

SHAN et al.: DELAYED-STATE NONPARAMETRIC FILTERING IN COOPERATIVE TRACKING

[25] Y. Weiss, “Correctness of local probability propagation in graphical models with loops,” Neural Comput., vol. 12, no. 1, pp. 1–41, 2000. [26] J. S. Yedidia, W. T. Freeman, and Y. Weiss, “Constructing free energy approximations and generalized belief propagation algorithms,” IEEE Trans. Inf. Theory, vol. 51, no. 7, pp. 2282–2312, Jul. 2005. [27] V. Savic, A. Poblaci´on, S. Zazo, and M. Garc´ıa, “Indoor positioning using nonparametric belief propagation based on spanning trees,” EURASIP J. Wireless Commun. Netw., vol. 2010, p. 963576, 2010. [28] S. Utete and H. F. Durrant-Whyte, “Reliability in decentralized data fusion networks,” in Proc. IEEE Int. Conf. Multisensor Fusion Integration Intell. Syst., Las Vegas, NV, USA, 1994, pp. 215–221. [29] C. Snyder, T. Bengtsson, P. Bickel, and J. Anderson, “Obstacles to highdimensional particle filtering,” Monthly Weather Rev., vol. 136, no. 12, pp. 4629–4640, 2008. [30] T. Bengtsson, P. Bickel, and B. Li, “Curse-of-dimensionality revisited: Collapse of the particle filter in very large scale systems,” in Probability and Statistics: Essays in Honor of David A. Freedman, vol. 2. Shaker Heights, OH, USA: Inst. Math. Statist., 2008, pp. 316–334. [31] P. Bickel, B. Li, and T. Bengtsson, “Sharp failure rates for the bootstrap particle filter in high dimensions,” in Pushing the Limits of Contemporary Statistics: Contributions in Honor of Jayanta K. Ghoshreedman, vol. 3. Shaker Heights, OH, USA: Inst. Math. Statist., 2008, pp. 318–329. [32] A. Doucet, N. de Freitas, K. Murphy, and S. Russell, “Rao-Blackwellized particle filtering for dynamic Bayesian networks,” in Proc. 16th Annu. Conf. Uncertainty Artif. Intell., San Francisco, CA, USA, 2000, pp. 176–183. [33] G. Casella and E. I. George, “Explaining the Gibbs sampler,” Amer. Statist., vol. 46, no. 3, pp. 167–174, 1992. [34] W. K. Hastings, “Monte Carlo sampling methods using Markov chains and their applications,” Biometrika, vol. 57, no. 1, pp. 97–109, Apr. 1970. [35] S. Chib and E. Greenburg, “Understanding the Metropolis-Hastings algorithm,” Amer. Statist., vol. 49, no. 4, pp. 327–335, Nov. 1995. [36] S. Geman and D. Geman, “Stochastic relaxation, Gibbs distribution and Bayesian restoration of images,” IEEE Trans. Pattern Anal. Mach. Intell., vol. PAMI-6, no. 6, pp. 721–741, Nov. 1984. [37] A. Doucet and R. Holenstein, “Particle Markov chain Monte Carlo methods,” J. Royal Statist. Soc.: Ser. B, vol. 72, no. Part 3, pp. 269–342, 2010. [38] C. Hue, J.-P. L. Cadre, and P. P´erez, “Sequential Monte Carlo methods for multiple target tracking and data fusion,” IEEE Trans. Signal Process., vol. 50, no. 2, pp. 309–325, Feb. 2002. [39] O. Rabaste, “Multi-target tracking with MCMC-based particle filters,” presented at the 18th Eur. Signal Process. Conf., Aalborg, Denmark, Aug. 2010. [40] L. Paninski, K. Rahnama, and M. Vidne, “Robust particle filters via sequential pairwise reparameterized Gibbs sampling,” presented at the 46th Annu. Conf. Inf. Sci. Syst., Princeton, NJ, USA, Mar. 2012. [41] W. R. Gilks and C. Berzuini, “Following a moving target: Monte Carlo inference for dynamic Bayesian models,” J. Royal Statist. Soc.: Ser. B, vol. 63, no. 1, pp. 127–146, 2001. [42] Z. Chen. (2013). Bayesian filtering: From Kalman filters to particle filters, and beyond, Adaptive Syst. Lab., McMaster Univ., Hamilton, ON, Canada, Tech. Rep. [Online]. Available: http://soma.crl.mcmaster. ca/zhechen/homepage.htm [43] Z. Khan, T. Balch, and F. Dellaert, “MCMC data association and sparse factorization updating for real time multitarget tracking with merged and multiple measurements,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 28, no. 12, pp. 1960–1972, Dec. 2006. [44] J. S. Liu and R. Chen, “Sequential Monte Carlo methods for dynamic systems,” J. Am. Statist. Assoc., vol. 93, no. 443, pp. 1032–1044, Sep. 1998. [45] E. Yoneki and J. Crowcroft, “Towards data driven declarative networking in delay tolerant networks,” presented at the 2nd Int. Conf. Distrib. Event Based Syst., Rome, Italy, 2008. [46] J. Su, J. Scott, P. Hui, J. Crowcroft, E. Lara, C. Diot, A. Goel, M. Lim, and E. Upton, “Haggle: Seamless networking for mobile applications,” presented at the 9th IEEE Int. Conf. Ubiquitous Comput., Innsbruck, Austria, 2007. [47] A. Berni, D. Merani, J. Potter, and R. Been, “Heterogeneous system framework for underwater networking,” in Proc. IEEE Conf. Military Commun., Sep. 2011, pp. 2050–2056. [48] D. Merani, A. Berni, J. Potter, and R. Martins, “Heterogeneous system framework for underwater networking,” in Proc. IEEE Baltic Congr. Future Internet Commun., Feb. 2011, pp. 103–108. [49] T. Bailey and H. Durrant-Whyte. (2007). Decentralized data fusion with delayed states for consistent inference in mobile ad hoc networks, Australian Centre for Field Robotics, University of Syd-

[50] [51] [52] [53] [54]

[55] [56] [57] [58] [59] [60]

977

ney, Tech. Rep. [Online]. Available: www-personal.acfr.usyd.edu.au/ tbailey/papers/delayedstateddf.pdf H. Mu, T. Bailey, P. Thompson, and H. Durrant-Whyte, “Decentralized solutions to the cooperative multi-platform navigation problem,” IEEE Trans. Aerosp. Electron. Syst., vol. 47, no. 2, pp. 1433–1449, Apr. 2011. J. Capit´an, L. Merino, F. Caballero, and A. Ollero, “Delayed-state information filter for cooperative decentralized tracking,” presented at the IEEE Int. Conf. Robot. Autom., Kobe, Japan, May 2009. M. Klaas, M. Briers, N. de Freitas, A. Doucet, S. Maskell, and D. Lang, “Fast particle smoothing: If I had a million particles,” in Proc. Int. Conf. Mach. Learning, 2006, pp. 25–29. A. Doucet and A. M. Johansen, “A tutorial on particle filtering and smoothing: fifteen years later, ” in, The Oxford Handbook of Nonlinear Filtering. Oxford, U.K.: Oxford Univ. Press, 2009. Z. Khan, T. Balch, and F. Dellaert, “Efficient particle filter-based tracking of mulitple interacting targets using an MRF-based motion model,” presented at the IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, NV, USA, 2003. Y. Bar-Shalom, X.-R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation. New York, NY, USA: Wiley, 2001. T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency of the EKF-SLAM algorithm,” presented at the IEEE/RSJ Int. Conf. Intell. Robots Syst., Beijing, China, Oct. 2006. J. R. Schoenberg, M. Campbell, and I. Miller, “Posterior representation with a multi-modal likelihood using the gaussian sum filter for localization in a known map,” J. Field Robot., vol. 29, no. 2, pp. 240–257, 2012. S. Worrall and E. Nebot, “Automated process for generating digitized maps through GPS data compression,” presented at the Australasian Conf. Robot. Autom., Brisbane, Australia, Dec. 2007. M. Shan, S. Worrall, and E. Nebot, “Probabilistic long-term vehicle motion prediction and tracking in large environments,” IEEE Trans. Intell. Transp. Syst., vol. 14, no. 2, pp. 539–552, Jun. 2013. H. Marouani and M. R. Dagenais, “Internal clock drift estimation in computer clusters,” J. Comput. Syst., Netw., Commun., vol. 2008, no. 9, pp. 1–7, Jan. 2008.

Mao Shan (M’12) received the B.S. degree in electrical engineering from Shaanxi University of Science and Technology, Xi’an, China, in 2006, and the M.S. and Ph.D. degrees from The University of Sydney, Sydney, Australia, in 2009 and 2014, respectively. He is a Research Associate with the Australian Centre for Field Robotics, The University of Sydney. His research interests include autonomous systems, motion planning, localization, and tracking algorithms and their applications.

Stewart Worrall (M’08) received the Ph.D. degree from The University of Sydney, Sydney, Australia, in 2009. He is a Research Fellow with the Australian Centre for Field Robotics, The University of Sydney. His research is focused on the study and application of technology for vehicle automation and improving vehicle safety.

Eduardo Nebot (SM’09) received the B.S. degree in electrical engineering from Universidad Nacional del Sur, Bah´ıa Blanca, Argentina, and the M.S. and Ph.D. degrees from Colorado State University, Fort Collins, CO, USA. He is a Professor with The University of Sydney, Sydney, Australia, and the Director of the Australian Centre for Field Robotics. His main research areas are in field robotics automation and intelligent transport systems. The major impact of his fundamental research is in autonomous system, navigation, and safety.