Vehicle Localization via Cooperative Channel Mapping
Xinghe Chu, Zhaoming Lu, David Gesbert, Luhan Wang, Xiangming Wen
VVEHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 1
Vehicle Localization via Cooperative ChannelMapping
Xinghe Chu, Zhaoming Lu, David Gesbert,
Fellow, IEEE , Luhan Wang, Xiangming Wen
Abstract —This paper addresses vehicle positioning, a topicwhose importance has risen dramatically in the context of futureautonomous driving systems. While classical methods that useGPS and/or beacon signals from network infrastructure fortriangulation tend to be sensitive to multi-paths and signalobstruction, our method exhibits robustness with respect tosuch phenomena. Our approach builds on the recently proposedChannel-SLAM method which first enabled leveraging of multi-path so as to improve (single) vehicle positioning. Here, wepropose a cooperative mapping approach which builds uponthe Channel-SLAM concept, referred to here as Team Channel-SLAM. Team Channel-SLAM not only exploits the stationarynature of many reflecting objects around the vehicle, but alsocapitalizes on the multi-vehicle nature of road traffic. The keyintuition behind our method is the exploitation for the first timeof the correlation between reflectors around multiple neighboringvehicles. An algorithm is derived for reflector selection andestimation, combined with a team particle filter (TPF) so as toachieve high precision simultaneous multiple vehicle positioning.We obtain large improvement over the single-vehicle positioningscenario, with gains being already noticeable for moderate vehicledensities, such as over % improvement for a vehicle densityas low as 4 vehicles in 132 meters’ length road. Index Terms —Cooperative Vehicle Localization, Radio-basedLocalization and Tracking, SLAM (Simultaneous Localizationand Mapping)
I. I
NTRODUCTION A UTONOMOUS driving is widely considered as a po-tential way to improve traffic safety and mitigate trafficcongestion issues [2]. Vehicle localization is known to play acrucial role in autonomous driving because stable and accuratepositioning is needed to assist perception, trajectory planning,and controlling [3]. What’s more, rapid growth in intelligenttraffic system applications also fuels the demand for precisionlocalization and tracking [4]. A variety of mechanisms aretypically resorted to in the context of vehicle positioning [5]–[7], the most common tools include Global Positioning System(GPS), Inertial Measurement Unit (IMU), Radio DetectionAnd Ranging (RADAR) and Light Detection And Ranging(LiDAR). GPS provides absolute positioning, but its biggestproblem is when non-line-of-sight (NLOS) and multi-pathconditions arise, as its signal is obstructed by buildings or other
Part of this work was accepted for publication in a preliminary conferencepaper [1].Xinghe Chu, Zhaoming Lu, Luhan Wang and Xiangming Wen are with theBeijing Key Laboratory of Network System Architecture and Convergence,Beijing Laboratory of Advanced Information Networks, Beijing University ofPosts and Telecommunications, Beijing, China, e-mails: { chuxinghe, lzy0372,wluhan, xiangmw } @bupt.edu.cn. ( Corresponding author: Zhaoming Lu.)
David Gesbert is with the Communications Systems Department, EURE-COM, Sophia Antipolis, France, email: [email protected]. objects [8], which can result in ten-meter scale positioningerrors in GPS-challenged areas, like urban canyons [9]. IMUprovides motion information, but its positioning error tends toaccumulate over time, so that the estimated position may driftaway [3]. RADAR is usually used for relative positioning, yetit cannot estimate the distance if there are obstacles. LiDARcan improve the precision of localization, but it is an expensivechoice and high-power devices are required. The performanceof LiDAR is also sensitive to severe weather conditions likerain and snow [3].In terms of radio-based localization, the development of5G new radio has brought new opportunities for vehicularpositioning and tracking [10], [11]. Radio-based positioningis typically based on the estimation of distance and angle pa-rameters, including angle of arrival (AoA), angle of departure(AoD) and time of arrival (ToA) [12], [13]. Large bandwidthand multiple antennas in 5G result in higher angle and timeresolution, which is essential for high precision localization[9]. The basic time unit in Long Term Evolution (LTE) is T s = 1/(∆ f ref · N f , ref ) ≈ . , where ∆ f ref = 15 · Hz and N f , ref = 2048 . However the basic time unit in 5G NR is T c = 1/(∆ f max · N f ) ≈ . , where ∆ f max = 480 · Hz and N f = 4096 [14], [15]. Hence, time resolution hasimproved greatly in 5G NR. Additionally, the introductionof massive Multiple Input Multiple Output (MIMO) arrayscan significantly improve angle estimation. The overall hightemporal and spatial resolution of millimeter wave (mmWave)MIMO makes it possible to estimate those above parametersjointly with satisfactory accuracy [16], [17].When it comes to the positioning problem itself, the wirelessresearch literature already offers a rich body of very interestingcontributions [18]–[26]. Unfortunately, classical radio-basedlocalization method is hindered by multi-path propagationthat introduces parameter ambiguities and noise. [21] presentsan Least-Squares Support-Vector Machines (LS-SVM) basedNLOS identification and mitigation approach to estimate theposition of objects by NLOS path identification and mitigation.Though the approach is promising, it does not make full useof the multi-path structure.Other interesting works have considered explicitly the pres-ence of the multi-paths [27]–[30]. SLAM-related methodssuch as [29]–[31] are particularly promising for how theyare able to leverage AoA and ToA measurements in a clevercombined way. As a special case, the so-called Channel-SLAM method was recently proposed for vehicular positioning[4], [29]. The key concept behind Channel-SLAM is thatmulti-paths are recast as virtual direct line of sight radiolinks that originate from virtual additional transmitters, whose a r X i v : . [ ee ss . SP ] F e b EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 2 position is piece-wise stationary and depends on the shapeand position of surrounding urban reflectors (see Fig. 1 for asimplified example). ToA and AoA estimated from vehiclescan then be used to localize the virtual transmitters (VTs)and the vehicle simultaneously . A recursive Bayesian filter[32] or Rao-Blackwellized particle filter (RBPF) [33] canbe used to solve this joint estimation. However, while theapproach is promising, it also exhibits limitations that motivatethe research elaborated in this paper. In particular, the aboveapproach fails to exploit the inherent multi-user nature ofvehicular traffic and treats the localization of each vehiclecompletely independently. This is a gross over-simplification,even in moderately dense traffic, which actually leads tooverlooking an opportunity to much improve the localizationaccuracy.In this paper, a multi-vehicle collaborative mapping andpositioning approach is proposed. It is referred to as TeamChannel-SLAM (TCS). The main idea behind TCS is toexploit the multi-vehicle nature of typical road traffic andthe ability for different vehicles to cooperate with eachother so as to improve the localization performance. Thekey intuition behind this contribution is the recognition thatmultiple vehicles closely following each other will exhibita strongly correlated multi-path structure because they arelikely to share one or two common key reflectors in theform of nearby buildings. Our key point is that the existenceof several common virtual transmitters provides a strongadditional structure to the vehicle localization problem, henceleading to improved estimation performance in the context ofSLAM-based algorithms, with gains expected to grow withtraffic density. Note that preliminary work capturing some ofthe ideas was presented to a conference version in [1]. Thispresent work, however, extends this earlier work in severalways. Importantly, in the present paper we provide a methodto associate common virtual transmitters (CVT) to differentneighboring vehicles based on a clustering approach. Thismethod allows to track and update the state of commonfeatures dynamically. Secondly, a stochastic batch iterationmethod based on particle filter called team particle filter(TPF) is introduced in this paper, which allows to improveperformance substantially by updating the stochastic batchof CVT particles and vehicle particles iteratively. The maincontributions of this paper are summarized as follows: • We propose a joint common virtual transmitter (CVT)and vehicle positioning framework which operates onmultiple vehicles simultaneously. A key intermediate toollies in the formation and maintenance of suitable dynamicclusters of CVTs across multiple vehicles, which ispresented in this paper. • We propose a new algorithm coined Team Particle Fil-ter (TPF) to achieve multiple vehicle localization andCVT mapping simultaneously. The algorithm iterativelyestimates the location of multiple vehicles and CVTsthrough vehicle particle filters and CVT particle filterscooperatively. • We verify the proposed localization and mapping systemthrough simulations based on ToA and AoA measure- ments from the literature. Our numerical results indicatethat the algorithm leads to over improvement overthe single-vehicle channel-SLAM situation for a trafficdenstiy as low as 4 vehicles in a 132 meter long road.For higher densities, higher gains ensue.The rest of the paper is organized as follows. Section IIreviews the related works of the proposed localization system.The system model is described in section III. In sectionIV, we present a CVT formation and cluster maintenancemechanism, and in section V, TPF is proposed for multiplevehicle localization and CVT Mapping. Simulation results areshown in section VI.II. R
ELATED W ORKS A ND B ACKGROUND
Since the proposed localization system achieves simulta-neous multiple vehicle localization and CVT mapping, thissection highlights the existing works on multi-path assistedlocalization.Multi-path propagation has long been regarded as a draw-back for radio-based localization technologies. A large numberof contributions draw their focus on multi-path eliminationand LOS path extraction [22], [34], [35]. However, from amere information theory standpoint, multi-path componentscan bring spatial information for vehicular positioning, whichmakes it possible to turn them from foe to friend in the contextof high-accuracy localization [36].[37] presents an omnidirectional NLOS identification andlocalization scheme. The authors make use of multi-pathsto localize the target nodes by NLOS identification andshared reflecting point detection. [37] utilizes multi-paths forlocalization and finds the common features among multi-paths, but shared reflecting points do not usually exist amongmulti-paths, which will limit the performance of localization.However, in Team Channel-SLAM, as will be introduced insection IV-A, the multi-paths are recast into LOS links fromvirtual transmitters to the receiver and it is in fact likely thatsome of the multi-paths will share the same reflectors. [38]utilizes multi-paths to detect the position, driving direction andsize of a vehicle whose LOS link is obstructed. The approachin [38] is promising that the shape of an unknown vehiclecan also be estimated, but the limitation is that the estimationis restricted to single time-slot worth of data. However, manyfeatures (like reflecting planes and virtual transmitters) remainstatic over multiple time slots, which will further enhancepositioning in the presence of noise.To make efficient use of the location information carriedby multi-paths, Channel-SLAM was initially proposed in[29]. Channel-SLAM estimates the position of the virtualtransmitters and the user’s position jointly with a simultane-ous localization and mapping method. There are also somevery interesting single-vehicle Channel-SLAM contributions.[9] exploits the virtual anchors corresponding to vehiclesto achieve downlink vehicular positioning, however, a priormap is needed in this contribution. Without foreknowledge ofenvironment, [39] presents a message passing-based estimatorwhich jointly estimates the position and orientation of a mobileterminal, and also the location of reflectors or scatters when
EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 3 there does not exist line-of-sight (LOS) path. However, it needsa joint estimation of AoA, AoD and ToA. [40] and [41] alsodevelop the Channel-SLAM method for single-user situationwithout any preknowledge of the environment, but they bothneed the existence and identification of the LOS path. ThemmWave imaging technology is used for reflected path in[42] to achieve Channel-SLAM, but [42] does not consider thedynamic nature of reflectors for moving objects. [43] utilizea non-Bayesian estimator and an extended Kalman filter forsingle-user Channel-SLAM that can be adopted to an unknownenvironment, but [43] does not consider the association ofobservations between different time slots. [44] proposes acooperative positioning and mapping approach based on multi-model probability hypothesis density (PHD) filter and mapfusion, which is an interesting method for multiple vehiclelocalization. However, [44] needs the estimation of AoD andthe knowledge of a measurement probability additionally,where the later indicates how likely it is for a VT with a certainlocation to give rise to measurements (referred to as ToA, AoAand AoD) to a vehicle in a certain position. Unfortunately, theconstraint on the measurement probability knowledge limitsthe environmental adaptability of the algorithm because thatdistribution will change over time due to the moving ofvehicles and changing of the reflectors. Note that the TeamChannel-SLAM goes further than all the above works byexploiting the common virtual transmitters among multiplemoving vehicles with changing reflectors. This is thanks to thefact that multiple neighboring vehicles will share one or morereflectors so that the VTs can be observed through severalparallel implicit models, and the multiple observations of thesame VT will lead to a more accurate position estimation.III. SYSTEM MODEL
A. Vehicle and Virtual Transmitter Modeling
We consider a scenario with one base station (BS) and M vehicles. Each vehicle is equipped with one radio userequipment (UE). The base station acts as the only transmitterthat transmits radio signals to vehicles, and the vehicles actas M receivers that receive signals from the base station. Thestate of vehicle at time t k is denoted as X ( t k ) = { x ( t k ) , ... x m ( t k ) , ..., x M ( t k ) } (1)where x m ( t k ) is the state for m -th vehicle, m = 1 , ..., M , x m ( t k ) = { r V m ( t k ) , v m ( t k ) } (2)where r V m ( t k ) ∈ R and v m ( t k ) ∈ R are its position andvelocity, respectively.As shown in Fig. 1, when there is a reflecting path betweenthe base station and the m -th vehicle at time slot t k , the multi-path can be recast into a LOS link between the m -th vehicleand a VT, who locates in the mirror position of the base stationover the reflecting plane and is static over some time [29]. Thestate of VTs observed by each vehicle at time slot t k is denotedas: V ( t k ) = { r V T ( t k ) , ..., r V T m ( t k ) , ..., r V T M ( t k ) } (3) Virtual TransmitterBase Station
Reflecti ng Plane k t -1 k t +1 k t Fig. 1: The signal from the base station is reflected and thenreceived by a vehicle-borne UE. Then a virtual transmitter canbe seen locating at the mirror position of the transmitter (BS)over the reflecting plane sending signals to the moving UEthrough a LOS link.where r V T m ( t k ) , m = 1 , , ...M denote the position of theVTs for m -th vehicle, r V T m ( t k ) = (cid:8) r V T ( m,pm ) ( t k ) (cid:9) N m ( t k ) p m =1 (4) N m ( t k ) denote the number of multi-paths observed by the m -th vehicle at time slot t k , and r V T ( m,p m ) ( t k ) ∈ R is theposition of the VT recast by the p m -th multi-path from the m -th vehicle at time slot t k , which can be calculated by (7)in subsection B.Note that for vehicle localization, since the height of vehi-cles is constant over time, only a 2D scenario is considered.However, for VT (or CVT later in the paper) localization, 3-Dpositioning is considered. B. Multi-Path Observation
The measurements from multi-paths include ToA and AoA.They are assumed to be given, and the particular choice ofmethod used to obtained such measurements is left unspecified[45]. The observation at time slot t k is denoted as Z ( t k ) = { z ( t k ) , ..., z m ( t k ) , ..., z M ( t k ) } z m ( t k ) = (cid:8) z ( m,p m ) ( t k ) (cid:9) N m ( t k ) p m =1 (5)where z m ( t k ) denotes all the multi-path measurements from m -th vehicle, and z ( m,p m ) ( t k ) is the p -th path in z m ( t k ) denoting as z ( m,p m ) ( t k ) = (cid:110) (cid:98) α ( m,p m ) ( t k ) , (cid:98) d ( m,p m ) ( t k ) (cid:111) (6) (cid:98) α ( m,p m ) ( t k ) = (cid:16)(cid:98) θ ( m,p m ) , (cid:98) ϕ ( m,p m ) (cid:17) is the estimation ofAoA, where (cid:98) θ ( m,p m ) and (cid:98) ϕ ( m,p m ) denote the polar angle andazimuth angle of AoA, respectively. (cid:98) d ( m,p m ) ( t k ) is the ToAestimation multiplied by the light speed.So the position of the VT recast from the multi-pathmeasured as z ( m,p m ) ( t k ) is calculated as r V T ( m,p m ) = r V m ( t k ) + −→ R (cid:0) z ( m,p m ) ( t k ) (cid:1) (7)where −→ R ( α , d ) is the vector operator, which is defined as −→ R ( α , d ) = d · (sin ( ϕ ) cos ( θ ) , sin ( ϕ ) sin ( θ ) , cos ( ϕ )) . EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 4 (cid:3)
Vehicle 1 (cid:3)
Vehicle 2 Transmitted
Signal (a) The Common Physical Transmitter Model.
40 60 80 100 120-50-250255075100 Base Station
CRVTVehicle 1
Vehicle 2
Common Reflector
Extension Line
Transmitt ed Signal (cid:3)(cid:3)
Signal (b) The Common-Reflector Virtual Transmitter Model.
Fig. 2: The Common Virtual Transmitter model includes the common physical transmitter model and the common-reflectorvirtual transmitter model.
C. Problem Formulation
A SLAM based method is introduced to solve the vehiclelocalization problem. SLAM is a map-based localization andtracking method without prebuilt map [46], [47]. Specifically,in feature-based SLAM [48], [49], the map is made up ofan unknown number of features, whose state is estimatedsimultaneously with the objects based on sequential measure-ments from sensors. Consequently, SLAM estimates the stateof targets and the map features around those targets by sensorslike camera, LiDAR or radio signal, where the localizationprocess and the mapping process are ongoing simultaneously.Mathematically, SLAM computes the following probabilitydistribution p ( x ( t k ) , m | z ( t k ) , u ( t k ) , x ( t )) (8)where x refers to the state (e.g. position) of the target, m contains the features of the map, z stands for observationsfrom sensors, and u stands for the motion information betweendifferent time slots.The Channel-SLAM and Team Channel-SLAM borrowfrom the SLAM principles to solve a new problem which isthat of vehicle positioning, where the localization refers tovehicle localization, and the mapping refers to the positioningof VTs (subliming to CVT in Team Channel-SLAM). Thelocalization and mapping process are executed simultaneouslyin this paper for high accuracy vehicle localization. In de-tail, the CVT positioning process is achieved based on theobservation of multi-path as well as the vehicle localizationprocess, and the CVT positioning process can also assistwith the vehicle localization process in return to improve thepositioning accuracy.In the Channel SLAM context, the probability distributionturns into p ( x ( t k ) , V | z ( t k ) , u ( t k ) , x ( t )) (9)where V stands for the state of virtual transmitters. Since in Team Channel-SLAM, localization simultaneouslytargets multiple vehicles, the probability distribution evolvesinto p X ( t k ) (cid:124) (cid:123)(cid:122) (cid:125) x ,..., x M , C (cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12)(cid:12) Z ( t k ) (cid:124) (cid:123)(cid:122) (cid:125) z ,...,z M , U ( t k ) (cid:124) (cid:123)(cid:122) (cid:125) u ,...,u M , X ( t ) (cid:124) (cid:123)(cid:122) (cid:125) x ,..., x M (10)where M is the number of vehicles, and C stands for state ofcommon virtual transmitters, which will be introduced in nextsection.IV. CVT F ORMATION AND C LUSTER M AINTENANCE
A. Common Virtual Transmitter model
When there are M vehicles in the network, there would betotally M (cid:80) m =1 N m ( t k ) paths at time slot t k . On the one hand,the base station is the common origin to all the paths. On theother hand, some multi-paths may be reflected by the samereflector, thereby some groups of VTs may have the sameposition in theory. Such a group of VTs will be lumped intowhat we call a Common Virtual Transmitter (CVT). As shownin Fig.2, there are two kinds of CVT. a) Common Physical Transmitter: This refers to the casewhere the paths originate directly from the same physicaltransmitter (here the BS) in LOS fashion as shown in Fig.2(a). b) Common-Reflector Virtual Transmitter:
As shown inFig.2 (b), when multi-paths share the same reflector, the VTsobserved by these multi-paths can be seen as a CVT with aunique position.
B. Affinity Propagation Based Clustering
The VTs treated as one CVT are usually not completelyidentical because the noise corrupts the observations. However,they are expected to be close to each other. This notion ofcorrelated VT can be articulated via a concept of VT cluster ,which is now described. The key advantage of clusteringtogether the common (or near common) virtual transmitters
EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 5 that associate with multiple vehicles is that a better robustnessto noise is obtained in the estimation process.Affinity propagation [50], [51] is a popular clusteringmethod that can collect a group of nodes by choosing an exem-plar (cluster head) for each node without preknowledge of thenumber of clusters and their size. For clustering purposes, weneed to introduce the negative logarithm of distance betweenVTs defined below, which is referred to later on as similarity value. s ( p , q ) = − ln (cid:0)(cid:13)(cid:13) r V T p − r V T q (cid:13)(cid:13) + 1 (cid:1) p (cid:54) = q , r V T p , r V T q ∈ (cid:110)(cid:8) r V T ( m,p m ) (cid:9) N m ( t k ) p m =1 (cid:111) Mm =1 (11)where s ( p , q ) is the similarity value of VT p with respectto VT q , which indicates how well VT q is suited to bethe exemplar for VT p [50]. Note that p and q are thecorresponding multi-path index in equation (11).The affinity propagation algorithm continuously updates anditerates a so-called responsibility value r ( p , q ) and availabil-ity value a ( p , q ) , where the former means the accumulatedevidence for how well-suited VT q can serve as the exemplarfor VT p and the latter means the accumulated evidence forhow appropriate it would be for VT p to choose VT q asits exemplar. How we obtain such values iteratively is shownbelow in equations (12 ∼ p , the VT q that maximizes the sum of responsibility value and availability value is selected as an exemplar of VT p [50]. One iterationof affinity propagation for CVT cluster formation is done asfollows: a) Calculate the responsibility value: r ( p , q ) ← s ( p , q ) − max q (cid:48) s . t . q (cid:48) (cid:54) = q { a ( p , q (cid:48) ) + s ( p , q (cid:48) ) } (12) b) Damp the responsibility value: r ( p , q ) ← (1 − λ ) r ( p , q ) + r ( p , q ) old (13)In equation (13), λ is a damping factor between 0 and 1, whichis set to 0.9 to gives a good compromised between the effectof eliminating oscillations and convergence rate according to[52]. r ( p , q ) old is the responsibility value at previous iteration. c) Calculate the availability value: a ( p , q ) ← min , r ( q , q ) + (cid:88) p (cid:48) / ∈{ p , q } max { , r ( p (cid:48) , q ) } (14) d) Damp the availability value: a ( p , q ) ← (1 − λ ) a ( p , q ) + a ( p , q ) old (15) λ is the damping factor the same as equation (13), and a ( p , q ) old is the availability value at previous iteration. e) Calculate the self-availability value: a ( q , q ) ← (cid:88) p (cid:48) (cid:54) = q max { , r ( p (cid:48) , q ) } (16) f) Choose exemplar: (cid:40) if iter > N iter E p = arg max q { a ( p , q ) + r ( p , q ) } if iter ≤ N iter return step a) (17) Algorithm 1:
CVT Formation and Cluster Mainte-nance
Input: vehicle state : X ( t k − ) observations : Z ( t k ) motion inf ormation : U ( t k ) Output:
CV T state : C ( t k ) Calculate X ( t k ) based on X ( t k − ) and U ( t k ) Calculate V ( t k ) based on X ( t k ) and Z ( t k ) as (7) Clustering the CVT as (11 ∼ New VT joining as (23 ∼ Neighbor CVT cluster merging as (29 ∼ Out-dated CVT deleting as (32)where VT E p is the exemplar for VT p .After a) ∼ f) , N C ( t k ) exemplars are chosen among all theVTs at time slot t k . E ( u ) , u = 1 , ..., N C ( t k ) (18)where E ( u ) denotes the u -th exemplar. Then N C ( t k ) clustersare selected by grouping the VTs with the same exemplartogether. S u ( t k ) = { p | E p = E ( u ) } , u = 1 , ..., N C ( t k ) (19)where p is the index of the multi-paths shaped like ( m, p m ) ,and S u ( t k ) is the set of multi-paths such that their correspond-ing VTs have the same exemplar E ( u ) .The VTs with the same exemplar are declared to be groupedinto one CVT cluster. Thus the state of CVT clusters aredenoted as: C ( t k ) = { C u ( t k ) } N C ( t k ) u =1 C u ( t k ) = { r C u ( t k ) , I C u ( t k ) } (20)Since each exemplar corresponds to a CVT cluster, so N C ( t k ) can also denote the number of CVT clusters at time slot t k ,and r C u ( t k ) is the 3-D position of CVT, which is the meanvalue of its corresponding VTs’ positions. r C u ( t k ) = E (cid:0) r V T p ( t k ) (cid:1) , p ∈ S u ( t k ) (21)where E ( · ) is the expectation operator, and r V T p ( t k ) is theposition of VTs in cluster u .The common virtual transmitter index vector notation I u ( t k ) is then introduced to indicate whether the u -th CVTcluster contains a VT observed by vehicle. If the m -th vehiclehas an observation to that CVT, then the m -th element in I u ( t k ) (denoted as π u,m ( t k ) ) equals to the correspondingmulti-path index p m , otherwise π u,m ( t k ) is 0. I C u ( t k ) = [ π u, ( t k ) , ..., π u,m ( t k ) , ..., π u,M ( t k )] π u,m = (cid:40) ˆ p m , if ˆ p m = S u ( t k ) ∩ { ( m, p m ) } N m ( t k ) p m =1 (cid:54) = φ , otherwise (22)where { ( m, p m ) } N m ( t k ) p m =1 denotes all the multi-path indexes of m -th vehicle. EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 6
C. Dynamic Cluster Maintenance and Association Mechanism
Due to the dynamic nature of the moving vehicles, theobserved VTs will also be dynamically changed. Thus on theone hand, there may possibly arise some VT observations to aCVT that does not exist in the previous time slot. On the otherhand, it is also possible that there will no longer exist any VTobservation to a certain CVT existing in the previous timeslot. So there exists a birth-death process for CVTs. To thisend, when the CVT clusters are established by the algorithmmentioned in section IV-B for one particular time slot, theyhave to be updated from time slot to time slot based on thenewly observed VTs. In the below, we introduce a mechanismto firstly associate every new VT observation to a certain CVTclusters and then update the CVT cluster dynamically. Themain idea behind this section is cluster association and clustermaintenance: • Cluster Association:
To associate the newly observed VTsto the current CVT clusters. • Cluster Maintenance:
To update the CVT clusters dynam-ically based on the previously observed VTs and newlyobserved VTs.The mechanism is denoted as dynamic cluster maintenanceand association mechanism, which is described as follows: a) New VT joining: when a multi-path is newly observed,the position of its corresponding VT is then calculated by (7). V ( t k +1 ) = (cid:110)(cid:8) r V T ( m,p m ) ( t k +1 ) (cid:9) N m ( t k +1 ) p m =1 (cid:111) Mm =1 (23)The data association quality is defined as : Q A ( π u,m ( t k +1 ) = p m ) = − ln (cid:16) ∆ ( m,p m ) u ( t k ) + 1 (cid:17) ∆ ( m,p m ) u ( t k ) = (cid:13)(cid:13) r C u ( t k ) − r V T ( m,p m ) ( t k +1 ) (cid:13)(cid:13) (24)Where Q A ( π u,m ( t k +1 ) = p m ) indicates how close the VTobserved by multi-path ( m, p m ) is to the u -th CVT, whichfurther indicates how likely that the VT originated from multi-path ( m, p m ) belongs to CVT cluster u .For multi-path ( m, p m ) , the CVT cluster that maximizes Q A ( π u,m ( t k +1 ) = p m ) is denoted as u A : u A = arg max u { Q A ( π u,m ( t k +1 ) = p m ) } (25)Then the common virtual transmitter index vector related tothe m -th vehicle π u,m ( t k ) , u = 1 , , ..., N C ( t k ) is updatedas: π u,m ( t k +1 ) = p m , if Q A ( π u,m ( t k +1 ) = p m ) ≥ L A && u = u A , otherwise (26)where L A is a suitable data association threshold.If the multi-path ( m, p m ) is associated with CVT cluster u ,the position of CVT cluster u is updated as: S u ( t k +1 ) = ( m, p m ) ∪ S u ( t k ) r C u ( t k +1 ) = E (cid:0) r V T p ( t k +1 ) (cid:1) , p ∈ S u ( t k +1 ) (27)However, there may exist some multi-paths that cannotassociate to any CVT cluster because those multi-paths are reflected new reflectors. In this situation, those multi-paths areplaced into standalone CVT clusters. Those CVT clusters areestablished as follows: r C u (cid:48) ( t k +1 ) = r V T ( m,p m ) ( t k +1 ) π u (cid:48) ,m (cid:48) ( t k +1 ) = (cid:40) p m , if m (cid:48) = m , otherwise (28)where we use u (cid:48) to denote the index for the standalone CVT,and π u (cid:48) ,m (cid:48) ( t k +1 ) , m (cid:48) = 1 , , ..., M is the common virtualtransmitter index vector for the u (cid:48) -th CVT. b) Neighbor CVT cluster merging : when two CVTs areclose to each other, they can be merged into one CVT cluster.Then we define the cluster merging quality as: Q M ( C i ( t k ) , C j ( t k )) = − ln (cid:16) ∆ ji ( t k ) + 1 (cid:17) ∆ ji ( t k ) = (cid:13)(cid:13) r C i ( t k ) − r C j ( t k ) (cid:13)(cid:13) (29)Where Q M ( C i ( t k ) , C j ( t k )) indicates how likely CVT clus-ter i is equivalent to CVT cluster j .For CVT cluster i and CVT cluster j , cluster i is equivalentto cluster j if they satisfy the following equation: Q M ( C i ( t k ) , C j ( t k )) ≥ L M I C i ( t k ) ◦ I C j ( t k ) = O (30)where L M is a suitable CVT cluster merging threshold and ◦ is the Hadamard product defined as ( A ◦ B ) ij = ( A ) ij ( B ) ij ,which prevents that there are more than one multi-path froma vehicle participating in the formation of one CVT cluster.If CVT cluster i is equivalent to CVT cluster j , the mergedCVT cluster u (cid:48) is updated as: r C u (cid:48) ( t k ) = E (cid:0) r V T p ( t k ) (cid:1) , p ∈ S i ( t k ) ∪ S j ( t k ) I C u (cid:48) ( t k ) = I C i ( t k ) + I C j ( t k ) (31) c) Out-dated CVT deleting : After a) and b) , if there is acertain CVT with no VT observation for it, the CVT will bedeleted after t d time slots. C u ( t k ) = Φ , if t k (cid:80) t = t k − t d I C u ( t ) = OC u ( t k ) , otherwise (32)As a summary, the CVT formation and cluster maintenancemechanism are shown in Algorithm 1. Notice that U ( t k ) inalgorithm 1 is the motion information for each vehicle at timeslot t k , which will be introduced in (44 ∼
45) in next section.V. M
ULTI -V EHICLE LOCALIZATION AND
CVT M
APPING
In the above, we presented the mechanisms for CVT for-mation and maintenance. We now turn to the problem ofvehicle localization, which is going to be performed jointlywith that of CVT mapping. As this problem boils down to alarge dimensional non-convex estimation problem, we proposea team particle filter based on particle filter [53]–[55], whichis well suited for this type of situation. As shown in Fig. 3,the team particle filters consist of CVT particle filters thatcorrespond to possible 3D location candidates for the CVT,and vehicle particle filters that also correspond to possible 2D
EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 7 (cid:89)(cid:72)(cid:75)(cid:66)(cid:51)(cid:41)(cid:66)(cid:20) (cid:89)(cid:72)(cid:75)(cid:66)(cid:51)(cid:41)(cid:66)(cid:80) (cid:17)(cid:17)(cid:17)(cid:17)(cid:17)(cid:17) (cid:17)(cid:17)(cid:17) (cid:89)(cid:72)(cid:75)(cid:76)(cid:70)(cid:79)(cid:72)(cid:3)(cid:51)(cid:41) (cid:38)(cid:57)(cid:55)(cid:3)(cid:51)(cid:41)(cid:55)(cid:72)(cid:68)(cid:80)(cid:3)(cid:51)(cid:41) batch b N = ( ) ( ) ( ) ( ) u N b f b fS S w w (cid:215)(cid:215)(cid:215) ( ) ( ) b jV w ( ) ( ) ( ) ( ) b j b jV Vm w w (cid:215)(cid:215)(cid:215) ( ) ( ) ( ) ( ) u b f b fS S w w (cid:215)(cid:215)(cid:215) (cid:17) (cid:89)(cid:72)(cid:75)(cid:66)(cid:51)(cid:41)(cid:66)(cid:20) (cid:89)(cid:72)(cid:75)(cid:66)(cid:51)(cid:41)(cid:66)(cid:80) (cid:17)(cid:17)(cid:17) (cid:17)(cid:17)(cid:17)(cid:17)(cid:17)(cid:17) (cid:17)(cid:17)(cid:17) (cid:89)(cid:72)(cid:75)(cid:76)(cid:70)(cid:79)(cid:72)(cid:3)(cid:51)(cid:41) (cid:38)(cid:57)(cid:55)(cid:3)(cid:51)(cid:41)(cid:55)(cid:72)(cid:68)(cid:80)(cid:3)(cid:51)(cid:41) batch b N = ( ) ( ) ( ) ( ) u N b f b fS S w w (cid:215)(cid:215)(cid:215) ( ) ( ) b jV w ( ) ( ) ( ) ( ) b j b jV Vm w w (cid:215)(cid:215)(cid:215) ( ) ( ) ( ) ( ) u b f b fS S w w (cid:215)(cid:215)(cid:215) ( ) ( ) N b fS w (cid:17) (cid:17) t = k t - (cid:17)(cid:17)(cid:17) t = t t k t = (cid:17)(cid:17)(cid:17) (cid:17)(cid:17)(cid:17)(cid:17)(cid:17)(cid:17) (cid:17)(cid:17)(cid:17) (cid:89)(cid:72)(cid:75)(cid:76)(cid:70)(cid:79)(cid:72)(cid:3)(cid:51)(cid:41) (cid:38)(cid:57)(cid:55)(cid:3)(cid:51)(cid:41)(cid:55)(cid:72)(cid:68)(cid:80)(cid:3)(cid:51)(cid:41) ( ) CVT-PF
C k
N t
CVT-PF 1
CVT-PF u Veh-PF 1
Veh-PF m Veh-PF M Veh-PF M Veh-PF M
CVT-PF 1 CVT-PF 1 ( ) CVT-PF
C k
N t - CVT-PF u CVT-PF u ( ) CVT-PF C N t ( ) ( ) ( ) ( ) p , m m V k V k m k t t u t - r r ( ) ( ) ( ) p u u C k C k t t - r r ( ) ( ) b bu N a aC C w w (cid:215)(cid:215)(cid:215) ( ) b jV w ( ) ( ) b b j jV Vm w w (cid:215)(cid:215)(cid:215) ( ) ( ) b bu a aC C w w (cid:215)(cid:215)(cid:215) ( ) ( ) b bm M j jV V w w (cid:215)(cid:215)(cid:215) ( ) bN aC w b b = b Fig. 3: Team particle filter structure. For each time slot, i) both particle filters firstly update their particles from the previous timeslot through p ( r V m ( t k ) | r V m ( t k − ) , u m ( t k ) ) and p ( r C u ( t k ) | r C u ( t k − ) ) . ii) both particle filters then update their particlesiteratively by updating ( b ) w ( j ) V m ( t k ) and ( b ) w ( f ) C u ( t k ) . iii) Finally the state of vehicles and CVTs are obtained by calculating (cid:98) r V m ( t k ) and (cid:98) r C u ( t k ) .location candidates for the vehicles. For each time slot, bothtypes of particles are updated as shown in Algorithm 2. Atconvergence, the probability density of CVT parameters andthe location of vehicles are obtained.The CVT particle filters and vehicle particle filters are in-troduced in section V-A and section V-B. The implementationof team particle filter is then introduced in section V-C. A. CVT Particle Filter
In this subsection, N C ( t k ) CVT particle filters are intro-duced to estimate the position of the N C ( t k ) CVTs at the timeslot t k . In a CVT particle filter, the position of that CVT isapproximated by N C particles according to the Monte CarloSampling [56], where each particle contains a 3-D position r ( a ) C u ( t k ) and a weight w ( a ) C u ( t k ) , a = 1 , ..., N C for that 3-D position. For example, (cid:104) r ( a ) C u ( t k ) , w ( a ) C u ( t k ) (cid:105) indicates thebelief that CVT u locates in r ( a ) C u ( t k ) is w ( a ) C u ( t k ) . Hence theprobability distribution function for the position of the u -thCVT can be approximated as: p (cid:0) r C u ( t k ) (cid:12)(cid:12) z p ku ( t k ) (cid:1) ≈ N C (cid:80) a =1 w ( a ) C u ( t k ) × δ (cid:16) r C u ( t k ) − r ( a ) C u ( t k ) (cid:17) (33)where we define the set of vehicles that observe a VTbelonging to the CVT cluster u at time t k as V ku , and the set of its corresponding multi-paths as p ku in the u -th CVT. V ku = { m | π u,m ( t k ) (cid:54) = 0 } p ku = (cid:8) ( m, π u,m ( t k )) (cid:12)(cid:12) m ∈ V ku (cid:9) (34)Then the z p ku ( t k ) can be used to denote the multi-pathobservations from all the vehicles to the u -th CVT.For the particle filter of the u -th CVT, according to [53]–[55], [57], the weight of its a -th particle is: w ( a ) C u ( t k ) ∝ p (cid:16) z p ku ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) Cu ( t k ) (cid:17) p (cid:16) r ( a ) Cu ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) Cu ( t k − ) (cid:17) q (cid:16) r ( a ) Cu ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) Cu (0: t k − ) ,z p ku (0: t k ) (cid:17) × w ( a ) C u ( t k − ) (35)where q (cid:16) r ( a ) C u ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u (0 : t k − ) , z p ku (0 : t k ) (cid:17) is known asimportant distribution [54], which is set as CVT transitionprobability in this paper: q (cid:16) r ( a ) C u ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u (0 : t k − ) , z p ku (0 : t k ) (cid:17) = p (cid:16) r ( a ) C u ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u ( t k − ) (cid:17) (36)since the position of CVT is constant over time, so p (cid:16) r ( a ) C u ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u ( t k − ) (cid:17) = δ (cid:16) r ( a ) C u ( t k ) − r ( a ) C u ( t k − ) (cid:17) (37)Hence the CVT particle state can be drawn as r ( a ) C u ( t k ) ∼ p (cid:16) r ( a ) C u ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u ( t k − ) (cid:17) (38) EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 8 and the weight updating equation is given by: w ( a ) C u ( t k ) = w ( a ) C u ( t k − ) p (cid:16) z p ku ( t k ) (cid:12)(cid:12)(cid:12) r ( a ) C u ( t k ) (cid:17) (39)The prior in (39) in team particle filter will be described inSection V-C in detail. B. Vehicle Particle Filter and Motion Update
In this subsection, M vehicle particle filters are introducedto estimate the position of the M vehicles. Similarly, in avehicle particle filter, the position of that vehicle is approxi-mated by N V particles according to the Monte Carlo Sampling[56], where each particle contains a 2-D position r ( j ) V m ( t k ) anda weight w ( j ) V m ( t k ) , j = 1 , ..., N V for that 2-D position. Forexample, (cid:104) r ( j ) V m ( t k ) , w ( j ) V m ( t k ) (cid:105) indicates the belief that vehicle m locates in r ( j ) V m ( t k ) is w ( j ) V m ( t k ) . Hence the probabilitydistribution function for the position of the m -th vehicle canbe approximated as: p ( r V m ( t k ) | z m ( t k ) , u m ( t k ) ) ≈ N V (cid:80) j =1 w ( j ) V m ( t k ) δ (cid:16) r V m ( t k ) − r ( j ) V m ( t k ) (cid:17) (40)For the particle filter of the m -th vehicle, according to [53]–[55], [57], the weight of its j -th particle is: w ( j ) V m ( t k ) ∝ p (cid:16) r ( j ) Vm ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) Vm ( t k − ) , u m ( t k ) (cid:17) × p (cid:16) z m ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) Vm ( t k ) (cid:17) q (cid:16) r ( j ) Vm ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) Vm ( t k − ) ,z m ( t k ) ,u m ( t k ) (cid:17) × w ( j ) V m ( t k − ) (41)set the vehicle transition probability as important distribution[54], [57]: q (cid:16) r ( j ) V m ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) V m ( t k − ) , z m ( t k ) , u m ( t k ) (cid:17) = p (cid:16) r ( j ) V m ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) V m ( t k − ) , u m ( t k ) (cid:17) (42)so the state of vehicle particles can be drawn as: r ( j ) V m ( t k ) ∼ p (cid:16) r ( j ) V m ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) V m ( t k − ) , u m ( t k ) (cid:17) (43)In this work, we consider the case of noisy motion informa-tion to test the robustness of our method. Assume the motioninformation can be observed with Gaussian error: U ( t k ) = { u ( t k ) , ..., u m ( t k ) , ..., u M ( t k ) } (44)where u m ( t k ) is the motion for m -th vehicle, u m ( t k ) = { v V m ( t k ) } v V m ( t k ) = v realV m ( t k ) + n v · e jn ω (45)where n v ∼ N (cid:0) , σ v (cid:1) and n ω ∼ N (cid:0) , σ ω (cid:1) is the noise ofspeed and orientation of speed that follow Gaussian distribu-tion [58]. The transmission model can be described as r V m ( t k ) = r V m ( t k − ) + ( v V m ( t k − ) + v V m ( t k )) · T (46)Then the weight update equation is given by: w ( j ) V m ( t k ) = w ( j ) V m ( t k − ) p (cid:16) z m ( t k ) (cid:12)(cid:12)(cid:12) r ( j ) V m ( t k ) (cid:17) (47)The prior in (47) in team particle filter will be described inSection V-C in detail. Algorithm 2:
Team Particle Filter
Input: vehicle particles : p (0) ( r V m ( t k )) CV T particles : p (0) ( r C u ( t k )) Output: estimated CV T state : (cid:98) X ( t k ) estimated vehicle state : (cid:98) C ( t k ) for b ≤ N b do sub - process one : CV T mapping for u = 1 : N C ( t k ) do Select CVT particle batch p ( b ) ( r C u ( t k )) Update w ( a b ) C u ( t k ) as (39),(48 ∼ Resampling CVT particles sub - process two : vehicle localization for m = 1 : M do Select vehicle particle batch p ( b ) ( r V m ( t k )) Update w ( j b ) V m ( t k ) as (47),(50 ∼ Resampling vehicle particles sub - process three : state estimation Calculate the position of vehicles (cid:98) r ( b ) V m ( t k ) as (53) Calculate the position of CVT (cid:98) r ( b ) C u ( t k ) as (52) if (cid:13)(cid:13)(cid:13)(cid:98) r ( b ) V m ( t k ) − (cid:98) r ( b − V m ( t k ) (cid:13)(cid:13)(cid:13) < ξ then break ; Calculate (cid:98) r C u ( t k ) and (cid:98) r V m ( t k ) as (52 ∼ C. Team Particle Filter Implementation
Different from the classical particle filter reported in theliterature [53]–[55], [57], the team particle filter combines theCVT particle filters and the vehicle particle filters to estimatethe state of CVTs and vehicles jointly. As shown in Algorithm2, the team particle filter introduces a stochastic batch iterationmethod that divides the update process into N b iterations.In each iteration, a stochastic batch of particles are chosenfrom the vehicle particles and CVT particles for the jointestimation of CVTs and vehicles. Each iteration contains threesub-processes : CVT mapping, vehicle localization and stateestimation. a) CVT Mapping : In each iteration, a batch of particlesare chosen from each CVT particle filter stochastically and theweight of the particles in the chosen batch are then updatedby (39).The particles for the u -th CVT particle filter at time slot t k are drawn by (38) denoting as p (0) ( r C u ( t k )) , and thena particle batch is stochastically chosen from p (0) ( r C u ( t k )) in b -th iteration denoting as p ( b ) ( r C u ( t k )) , b = 1 , , ..., N b .We then denote the particles contained in p ( b ) ( r C u ( t k )) as p ( b ) ( r C u ( t k )) = (cid:110) r ( a b ) C u ( t k ) , w ( a b ) C u ( t k ) (cid:111) , thus the observa-tion PDF in (39) for the particle batch chosen in b -th iterationis given by: p (cid:16) z p ku ( t k ) (cid:12)(cid:12)(cid:12) r ( a b ) C u ( t k ) (cid:17) = (cid:81) p =( m,p m ) ∈ p ku (cid:82) p (cid:16) z p ( t k ) (cid:12)(cid:12)(cid:12) r ( a b ) C u ( t k ) , r V m ( t k ) (cid:17) × p ( r V m ( t k )) d r V m ( t k ) (48) EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 9 where the update process in (48) is based on the sum effectof the multi-paths in p ku .We suppose the distance between the particle r ( a b ) C u ( t k ) andthe particle level estimated CVT (cid:98) r ( p ,j ) C u ( t k ) follows Gaussiandistribution, the conditional PDF in the right part of equation(48) can then be calculated. (cid:98) r ( p ,j ) C u ( t k ) = r ( j ) V m ( t k )+ (cid:42) R ( z p ( t k )) , p = ( m, p m ) ∈ p ku (49)where (cid:98) r ( p ,j ) C u ( t k ) denote the particle level estimation for the u -th CVT based on the observation from the multi-path in p ku and the j -th particle from the corresponding vehicle particlefilter. b) Vehicle Localization : In each iteration, a batch of parti-cles are chosen from each vehicle particle filter stochasticallyand the weight of the particles in the chosen batch are thenupdated by (47).The particles for the m -th vehicle particle filter at time slot t k are drawn by (43) denoting as p (0) ( r V m ( t k )) , and thena particle batch is stochastically chosen from p (0) ( r V m ( t k )) in b -th iteration denoting as p ( b ) ( r V m ( t k )) , b = 1 , , ..., N b .We then denote the particles contained in p ( b ) ( r V m ( t k )) as p ( b ) ( r V m ( t k )) = (cid:110) r ( j b ) V m ( t k ) , w ( j b ) V m ( t k ) (cid:111) , thus the observa-tion PDF in (47) for the particle batch chosen in b -th iterationis given by: p (cid:16) z m ( t k ) (cid:12)(cid:12)(cid:12) r ( j b ) V m ( t k ) (cid:17) = N m ( t k ) (cid:81) p m =1 ,π um,m ( t k )= p m (cid:82) p (cid:16) z ( m,p m ) ( t k ) (cid:12)(cid:12)(cid:12) r ( j b ) V m ( t k ) , r C um ( t k ) (cid:17) × p (cid:0) r C um ( t k ) (cid:1) d r C um ( t k ) (50)where the update process in (50) is based on the sum effect ofthe multi-paths observed by the m -th vehicle, and u m is theindex of CVT that observed by vehicle m through its p m -thmulti-path at time slot t k satisfying π u m ,m ( t k ) = p m .We suppose the distance between the particle r ( j b ) V m ( t k ) and the particle level estimated vehicle position (cid:98) r ( p m ,a ) V m ( t k ) follows Gaussian distribution, the conditional PDF in the rightpart of equation (50) can then be calculated. (cid:98) r ( p m ,a ) V m ( t k ) = r ( a ) C um ( t k ) − (cid:42) R (cid:0) z ( m,p m ) ( t k ) (cid:1) p m = 1 , , ..., N m ( t k ) , π u m ,m ( t k ) = p m (51)where (cid:98) r ( p m ,a ) V m ( t k ) denote the particle level estimation for the m -th vehicle based on its p m -th multi-path observations andthe a -th particle from the corresponding CVT particle filter. c) State Estimation : According to (33), the position ofthe u -th CVT is estimated as (time dimension is omitted forshort): (cid:98) r C u ( t k ) = (cid:82) r C u × p (cid:0) r C u (cid:12)(cid:12) z p ku (cid:1) d r C u ≈ (cid:82) r C u × (cid:18) N C (cid:80) a =1 w ( a ) C u × δ (cid:16) r C u − r ( a ) C u (cid:17)(cid:19) d r C u = N C (cid:80) a =1 w ( a ) C u × (cid:16)(cid:82) r C u × δ (cid:16) r C u − r ( a ) C u (cid:17) d r C u (cid:17) = N C (cid:80) a =1 w ( a ) C u ( t k ) × r ( a ) C u ( t k ) (52) Algorithm 3:
Team Channel-SLAM
Input: initial vehicle state : X ( t ) observations : Z ( t K ) motion inf ormation : U ( t K ) Output: estimated vehicle state : (cid:98) X ( t K ) estimated CV T state : (cid:98) C ( t K ) Initialize the initial particles r ( j ) V m ( t ) from X ( t ) for t = t : t K do CV T F ormation and Cluster M aintenance asAlgorithm 1 Draw vehicle particles p (0) ( r V m ( t k )) as (43) Draw CVT particles p (0) ( r C u ( t k )) as (38) T eam P article F ilter as Algorithm 2According to (40), the position of the m -th vehicle isestimated as: (cid:98) r V m ( t k ) = (cid:82) r V m × p ( r V m | z m , u m ) d r V m ≈ (cid:82) r V m × N V (cid:80) j =1 w ( j ) V m × δ (cid:16) r V m − r ( j ) V m (cid:17) d r V m = N V (cid:80) j =1 w ( j ) V m × (cid:16)(cid:82) r V m × δ (cid:16) r V m − r ( j ) V m (cid:17) d r V m (cid:17) = N V (cid:80) j =1 w ( j ) V m ( t k ) × r ( j ) V m ( t k ) (53)As a summary of Section IV and V, the process of TeamChannel-SLAM is shown in Algorithm 3.VI. S IMULATION R ESULTS
In this section, a simulation experiment is carried out totest the performance of the proposed algorithm. Performanceevaluation of ToA and AoA estimation in LTE was reportedin [45] and the results show that the median error of ToAand AoA is e md = 1 . m and e mθ = 1 . ◦ , respectively. Sincethe noise of ToA and AoA can be assumed as Gaussiandistribution [58], the variance of ToA and AoA error iscalculated as: (cid:82) e md − e md √ πσ d e − x σ d dx = ⇒ σ d = 2 . (cid:82) e mθ − e mθ √ πσ θ e − x σ θ dx = ⇒ σ θ = 2 . ◦ (54)As a result, building on the recommendation of [45], thevariance of the distance measurement noise σ d was set to . , and the variance of the angle measurement noise σ α was set to . ◦ (both for polar angle and azimuth angle).As for the noise of motion information, the variance of speederror and its orientation error set as is σ v = 0 . m/s and σ ω = 0 . /s [58]. In this paper, we suppose the initialposition of vehicle is get from GPS, whose error follows aGaussian distribution with σ gpsr = 3 m . Since there is infinityin Gaussian distribution, the errors mentioned above are allcut by σ to make it more reasonable.The sampling interval t δ is set as t δ = 0 . , the out-datedCVT cluster deleting time delay t d is set as t d = 10 ∗ t δ .The data association threshold L A and CVT cluster merging EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 10
Fig. 4: Simulation scenario and estimated trajectories. The estimated trajectories that the density of vehicles ρ v =4 vehicles / (32 × ) are shown by purple lines. The real trajectories and real speeds of the vehicles are indicatedvia the color bar. The estimated vehicle positions and CVT positions at the 150-th time slot are shown in the figure, and thereflecting planes at that time are indicated by the red-colored surface in the buildings. The four enlarged parts (”P1”, ”P2”,”P3”, and ”P4”) indicate the initial positions of the four vehicles.threshold L M are set as L A = L M = − . , which indicatethe near worst VT estimation error from equation (7). L A = L M = − ln ( ε m + 1) ε = ( d m + nσ d ) + d − d m ( d m + nσ d ) cos ( nσ θ ) (55)where ε m is the near worst VT estimation error, d m = 100 m is the maximum communication range for a base stationand n = 2 , which means that the cumulative probability inGaussian distribution is over 0.95 ( Φ (2 σ ) = 0 . ). So thesetting of L A and L M indicates that the 95.44 percentile of theToA and AoA error is considered within the data associationthreshold and CVT cluster merging threshold to cover a nearworst situation.Totally 300 time slots’ movement of the vehicles are trackedunder different vehicle densities ( t K = 300 ∗ t δ ). The particlenumber for both CVT PF and vehicle PF is set to 120 ( N C = N V = 120 ), which shows a acceptable compromise betweenthe accuracy and complexity. The number of iterations in TeamChannel-SLAM N b is set as N b = 10 to ensure that there areas much iterations as possible and also that there are enoughparticles in each particle batch according to the number ofparticles in vehicle particle filter and CVT particle filter. Theresults are based on 100 simulation runs. A. Vehicle Trajectory and Scenario Setting
As shown in Fig. 4, the base station locates at [50 m, , m ] ,and the buildings distribute besides the road. The position ofthe buildings are unknown in TCS, and the position of base station is also not required in this paper because authenticationmay be needed to get its position for security reasons. Thereare totally 8 lanes in the road, the width of each lane is 4meters. The vehicles run clockwise along the four trajectories,and the speed of each vehicle is indicated by the color bar.Three kinds of motion models (uniform motion, uniformly ac-celerated motion and uniform circular motion) are consideredin the trajectory in order to make the simulation more realistic.All the vehicles drive within the area within [ − m, m ] of y -axis and [0 m, m ] of x -axis. Since the number of vehicleswill influence the performance of the algorithm, we explorethe performance of TCS under different vehicle densities. Wedefine the number of vehicles per unit area as vehicle densitydenoted as ρ V . For example, the vehicle density in Fig. 4 is ρ V = 4 vehicles / (32 × ) ≈ . × − vehicle / m .For convenience, we use vehicle / (cid:0) × (cid:1) as the dimen-sion of vehicle density. We explore the performance of TCSwhen ρ V = 1 , , , , , ,
24 vehicle / (cid:0) × (cid:1) .Fig. 4 also shows the estimated trajectory of vehicles whenvehicle density ρ V = 4 vehicles / (cid:0) × (cid:1) . The initialposition of the four vehicles are circled by black lines andalso enlarged in ”P1”, ”P2”, ”P3”, and ”P4”. We can see thatthe initial positioning error of the vehicles are large and theestimated vehicle trajectories get gradually close to the realtrajectory. So we can see preliminary from Fig. 4 that thevehicles are well tracked and CVTs are well estimated byour algorithm. However a more precise quantitative analysisis done in the following subsections. EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 11 (a) CVT particles converging to the real CVT position over time (the depth of the color represent the density of the particles). (b) level vehicle positioning error ε over time under different vehicledensities ρ V = 1 , , , , ,
24 vehicle / (cid:0) × (cid:1) . Team Channel-SLAM Channel-SLAM [2 ] Team Channel-SLAM Channel-SLAM [2 ] (c) MAE Positioning errors of Team Channel-SLAM and Channel-SLAM overdifferent vehicle densities. Fig. 5: Particle convergence of TPF and the positioning errors over time and different vehicle densities. Note that in (a)only particles locating in x ∈ [48 , , y ∈ [ − , and z ∈ [6 , are shown. (b) shows the 80% level vehicle positioningerror [ ε | p ( ε V ≤ ε ) = 0 . ] over time under different vehicle densities (cid:2) vehicle / (cid:0) × (cid:1)(cid:3) . (c) shows the MAE (MeanAbsolute Error) positioning errors for vehicles and CVT [ e V , e C | e V = E ( | ε V | ) , e C = E ( | ε C | ) ] utilizing the Team Channel-SLAM algorithm and Channel-SLAM algorithm over different vehicle densities (cid:2) vehicle / (cid:0) × (cid:1)(cid:3) . B. Particle Convergence
Fig. 5 (a) shows the particles for a certain CVT at a) t = 5 ∗ t δ , b) t = 20 ∗ t δ , c) t = 35 ∗ t δ and d) t = 50 ∗ t δ .The position of the CVT is indicated by a red circle andwhat needs to be explained is that in order to show thedistribution of particles more clearly, the particles are indicatedby semitransparent blue balls. Thereby the area with higherdepth of color indicates the higher possibility that the CVTlocates here. From Fig. 5 (a), we can see that the particlesget closer to the true position of the CVT from t = 5 ∗ t δ to t = 50 ∗ t δ . This indicates that CVT formation and clustermaintenance mechanism can associate and maintain the CVTclusters well so that the TPF can converge the CVT particleswell over time. C. Error Analysis
In order to show the performance of TCS, Channel-SLAM[29], [33] is introduced as a benchmark in this paper. Basedon RBPF (Rao-Blackwellized particle filter) [33], Channel-SLAM utilizes the super-particle filters (super-PF) and the sub-particle filters (sub-PF) to estimate the state of a single-vehicleand its corresponding VTs. In Channel-SLAM, the super-PFis used for vehicle positioning and the sub-PF is used for VT positioning. We set the particle number of super-PF the sameas vehicle PF in TCS ( N super = N V = 120 ) and the particlenumber of sub-PF the same as CVT PF in TCS ( N sub = N C =120 ). However, in Channel-SLAM, each particle in super-PFpossess N m ( t k ) sub-PFs, so the total particle number of sub-PF (the particles used for VT estimation) is N sub × N super × N m ( t k ) in Channel-SLAM, compared with N C × N m ( t k ) inTCS.
1) Positioning Error over Vehicle Density:
Fig. 5 (c) showsthe mean absolute positioning error of vehicles e V and CVTs e C over different vehicle densities, which are defined as: [ e V , e C | e V = E ( | ε V | ) , e C = E ( | ε C | ) ] (56)where ε V and ε C denote the vehicle positioning errors andCVT positioning errors in the 100 simulation runs, respec-tively.We can see that the positioning error and its variancehave an obvious decreasing tendency when vehicle densityincreases. Specifically, when there are 4 vehicles among the × square meters’ road, the positioning error of thevehicle has decreased . than single-vehicle scenario.This is because the increasing number of vehicles bringsmore multi-path observations for CVTs, which improves EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 12 (a) Cumulative probability density of CVT positioning error (b) Cumulative probability density of vehicle positioning error
Fig. 6: Cumulative distribution function (CDF) of vehicle positioning error and CVT positioning error. (a) shows the CDF ofvehicle positioning error over different vehicle densities (cid:2) vehicles / (cid:0) × (cid:1)(cid:3) for both TCS and CS [29]. (b) shows theCDF of CVT positioning error over different vehicle densities for both TCS and CS [29].CVT positioning accuracy in the sub-process one of TPFin Algorithm 2. Then this improvement will provide betterestimation for vehicle localization in the sub-process two ofTPF in Algorithm 2. Thus a positive feedback is constitutedbetween the two sub-processes in TPF to improve the positionof vehicles and CVTs. What’s more, higher precision ofCVTs and vehicles will make newly observed VTs moreprecisely associated into their corresponding CVT clusters inAlgorithm 1, which also improves the accuracy of the CVTformation and cluster maintenance process in section IV. Thusanother positive feedback is then constituted between TPF andCVT formation and cluster maintenance mechanism to futherimprove the precision of multi-vehicle localization and CVTmapping.
2) Positioning Error over Time:
Fig. 5 (b) show the confidence level of vehicle positioning error over time underdifferent vehicle densities, p ( ε V ≤ ε ) = 0 . (57)which indicates that for each value ε in the figure, there is confidence that the vehicle positioning error ε V is nobigger than ε .We can see from Fig. 5 (b) that the positioning error ofvehicle decreases over time and finally converge to a lowvalue, which means that TCS can help the vehicle particlesconverge to their real position. The reason behind this isthat TCS will extract more and more information from addedobservations to make the CVT estimations get closer to theirreal positions gradually, which in return helps to improve theaccuracy of vehicle positioning as described in Section VI-C-1).
3) CDF of Positioning Error:
Fig. 6 (a) and Fig. 6 (b) showthe cumulative probability density (CDF) of the positioningerror of vehicles and CVTs. Both and confidencelevel errors are labeled in the figure.
4) Team Channel-SLAM and Channel-SLAM:
Seen fromFig. 5 (c), the VT positioning error of Channel-SLAM is al-most the same as TCS when ρ V = 1 vehicle / (cid:0) × (cid:1) ,however, the vehicle positioning error of Channel-SLAM is higher than TCS when ρ V = 1 vehicle / (cid:0) × (cid:1) , whichmeans that TCS have a better performance in particle con-vergence due to the CVT formation and cluster maintenancemechanism.Additionally, we can found that the positioning error doesn’tdecrease over time when ρ V = 1 vehicle / (cid:0) × (cid:1) inFig. 5 (b). This is because in this situation the CVT estimationis based on the relative observation (ToA and AoA) and theinitial position of just one vehicle. So the initial error of thatvehicle will contaminate the CVT estimations for there is onlyone observation to each CVT. However, TCS is also usefulin this situation, because it will eliminate the accumulativeerror from motion information and also provide a continuousand stable positioning for vehicles in case that the GPS orother positioning sensors get limited by the weather or otherhard conditions. However, with the increasing of the vehicledensity, the CVTs get gradually better estimated, which resultsin higher precision of vehicle positioning. D. Performance over Building Density
Fig. 7 shows the performance of vehicle positioning overdifferent building densities. In this simulation, we set thelength of each building constant as D = 12m , and then changethe distance between the buildings as d = 6m , d = 24m , d =60m as well as d = ∞ (indicates that there is no building).So the building density changes while the value d changes,the value d/D can then be used to indicate the density of thebuildings. In detail, the larger the value, the less dense of thebuildings.The different densities of buildings will then lead to differ-ent possibilities for a vehicle to observe a multi-path reflectedby those buildings, and then influence the number of observedVTs. Thus we plot the average number of VTs observedby a vehicle at a time slot in the figure by green lines(referring to the right axis), which can be indicated that withthe decreasing of the building density, the average number ofthe VTs observed by each vehicle decreases.At the same time, we also test the performance of vehiclepositioning under those different densities of buildings over EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 13
Fig. 7: Vehicle positioning error and the average number ofVTs observed by a vehicle at a time slot over different densi-ties of buildings. The histogram shows the vehicle positioningerror over different densities of buildings referring to the leftaxis, and the green line shows the average number of VTsover different densities of buildings referring to the right axis.Note that d refers to the distance between buildings and D refers to the length of buildings, so the larger the value d/D ,the less dense of the buildings.different vehicle densities, where the result can be seen inthe figure by the histogram (referring to the left axis). Wecan see that the denser the buildings, the more accurate of thevehicle positioning. Specifically, the positioning error when thebuilding density d/D = 0 . is averagely 25.67% better thanthe situation when the building density d/D = ∞ (indicatesthat there is no building). So it can be then indicated thathaving more virtual transmitters will lead to more informationregarding the vehicle position and therefore improve localiza-tion accuracy, which is rather reasonable as the explanation inSection VI-C-1). VII. C ONCLUSION
Team Channel-SLAM firstly establishes and maintains CVTclusters dynamically through CVT Formation and ClusterMaintenance mechanism and then estimates the position ofmultiple vehicles and CVTs simultaneously through TeamParticle Filters. Simulation results show that TCS can improvethe precision of vehicle localization for its better estimation forCVTs in multiple vehicle scenario. As from expectation, thevehicle positioning error keeps decreasing with the increasingdensity of vehicles and buildings in road traffic.A
CKNOWLEDGMENT
This work is partially supported by Research on the keytechnology of seamless handover in cloud-RAN for net-work assisted automatic driving, NSFC project, under grant61801047. It is also supported by the Beijing Nova Programof Science and Technology under grant Z191100001119028.R
EFERENCES[1] X. Chu, Z. Lu, L. Wang, X. Wen, and D. Gesbert, “Team channel-slam: A cooperative mapping approach to vehicle localization,” in , 2020, pp. 1–6. [2] D. for Transport, “Research on the impacts of connected and autonomousvehicles on traffic flow,” https://trid.trb.org/view/1448451, 2016.[3] S. Kuutti, S. Fallah, K. Katsaros, M. Dianati, F. Mccullough, andA. Mouzakitis, “A survey of the state-of-the-art localization techniquesand their potentials for autonomous vehicle applications,”
IEEE Internetof Things Journal , vol. 5, no. 2, pp. 829–846, 2018.[4] M. Ulmschneider, R. Raulefs, C. Gentner, and M. Walter, “Multipathassisted positioning in vehicular applications,” in . IEEE, 2016,pp. 1–6.[5] H. Wymeersch, G. Seco-Granados, G. Destino, D. Dardari, andF. Tufvesson, “5G mmWave positioning for vehicular networks,”
IEEEWireless Communications , vol. 24, no. 6, pp. 80–86, 2017.[6] S. Choudhary, L. Carlone, C. Nieto, J. Rogers, H. I. Christensen,and F. Dellaert, “Distributed mapping with privacy and communicationconstraints: Lightweight algorithms and object-based models,”
The Inter-national Journal of Robotics Research , vol. 36, no. 12, pp. 1286–1311,2017.[7] P.-Y. Lajoie, B. Ramtoula, Y. Chang, L. Carlone, and G. Beltrame,“DOOR-SLAM: Distributed, online, and outlier resilient slam for roboticteams,”
IEEE Robotics and Automation Letters , vol. 5, no. 2, pp. 1656–1663, 2020.[8] P. D. Groves and Z. Jiang, “Height aiding,
C/N weighting andconsistency checking for GNSS NLOS and multipath mitigation in urbanareas,” The Journal of Navigation , vol. 66, no. 5, pp. 653–669, 2013.[9] H. Wymeersch, N. Garcia, H. Kim, G. Seco-Granados, S. Kim, F. Went,and M. Fr¨ohle, “5G mmWave downlink vehicular positioning,” in . IEEE, 2018,pp. 206–212.[10] M. Z. Win, R. M. Buehrer, G. Chrisikos, A. Conti, and H. V. Poor,“Foundations and trends in localization Technologies—Part I [scanningthe issue],”
Proceedings of the IEEE , vol. 106, no. 6, pp. 1019–1021,2018.[11] M. Z. Win, R. M. Buehrer, G. Chrisikos, A. Conti, H. V. Poor et al. ,“Foundations and trends in localization technologies-part II,” 2018.[12] B. H. Fleury, M. Tschudin, R. Heddergott, D. Dahlhaus, and K. I.Pedersen, “Channel parameter estimation in mobile radio environmentsusing the SAGE algorithm,”
IEEE Journal on Selected Areas in Com-munications .IEEE, 2015, pp. 1–6.[17] ——, “Position and orientation estimation through millimeter-wavemimo in 5G systems,”
IEEE Transactions on Wireless Communications ,vol. 17, no. 3, pp. 1822–1835, 2018.[18] J. A. del Peral-Rosado, R. Raulefs, J. A. L´opez-Salcedo, and G. Seco-Granados, “Survey of cellular mobile radio localization methods: From1G to 5G,”
IEEE Communications Surveys & Tutorials , vol. 20, no. 2,pp. 1124–1148, 2017.[19] N. Bulusu, J. Heidemann, D. Estrin et al. , “GPS-less low-cost outdoorlocalization for very small devices,”
IEEE personal communications ,vol. 7, no. 5, pp. 28–34, 2000.[20] M. Z. Win, Y. Shen, and W. Dai, “A theoretical foundation of networklocalization and navigation,”
Proceedings of the IEEE , vol. 106, no. 7,pp. 1136–1165, 2018.[21] S. Marano, W. M. Gifford, H. Wymeersch, and M. Z. Win, “NLOSidentification and mitigation for localization,”
IEEE Journal on SelectedAreas in Communications , vol. 28, no. 7, pp. 1026–1035, 2010.[22] S. Mingyang, T. Xiaofeng, X. Yongtai, and H. Xiao, “A distributedmulti-antenna based NLOS error elimination algorithm for mobilelocalization,” in . IEEE, 2008, pp. 411–415.[23] A. Dammann, R. Raulefs, and S. Zhang, “On prospects of positioningin 5G,” in . IEEE, 2015, pp. 1207–1213.
EHICLE LOCALIZATION VIA COOPERATIVE CHANNEL MAPPING 14 [24] M. Koivisto, M. Costa, J. Werner, K. Heiska, J. Talvitie, K. Lepp¨anen,V. Koivunen, and M. Valkama, “Joint device positioning and clocksynchronization in 5G ultra-dense networks,”
IEEE Transactions onWireless Communications , vol. 16, no. 5, pp. 2866–2881, 2017.[25] M. Koivisto, A. Hakkarainen, M. Costa, P. Kela, K. Leppanen, andM. Valkama, “High-efficiency device positioning and location-awarecommunications in dense 5G networks,”
IEEE Communications Maga-zine , vol. 55, no. 8, pp. 188–195, 2017.[26] V. Savic and E. G. Larsson, “Fingerprinting-based positioning in dis-tributed massive mimo systems,” in . IEEE, 2015, pp. 1–5.[27] E. Leitinger, F. Meyer, P. Meissner, K. Witrisal, and F. Hlawatsch, “Be-lief propagation based joint probabilistic data association for multipath-assisted indoor navigation and tracking,” in . IEEE, 2016, pp. 1–6.[28] E. Leitinger, P. Meissner, C. R¨udisser, G. Dumphart, and K. Witrisal,“Evaluation of position-related information in multipath components forindoor positioning,”
IEEE Journal on Selected Areas in communications ,vol. 33, no. 11, pp. 2313–2328, 2015.[29] C. Gentner, T. Jost, W. Wang, S. Zhang, A. Dammann, and U.-C.Fiebig, “Multipath assisted positioning with simultaneous localizationand mapping,”
IEEE Transactions on Wireless Communications , vol. 15,no. 9, pp. 6104–6117, 2016.[30] E. Leitinger, P. Meissner, M. Lafer, and K. Witrisal, “Simultaneouslocalization and mapping using multipath channel information,” in .IEEE, 2015, pp. 754–760.[31] E. Leitinger, F. Meyer, F. Hlawatsch, K. Witrisal, F. Tufvesson, and M. Z.Win, “A belief propagation algorithm for multipath-based SLAM,”
IEEEtransactions on wireless communications , 2019.[32] H. W. Sorenson and D. L. Alspach, “Recursive bayesian estimation usingGaussian sums,”
Automatica , vol. 7, no. 4, pp. 465–479, 1971.[33] G. Casella and C. P. Robert, “Rao-Blackwellisation of samplingschemes,” 1996.[34] M. Horiba, E. Okamoto, T. Shinohara, and K. Matsumura, “An accurateindoor-localization scheme with nlos detection and elimination exploit-ing stochastic characteristics,”
IEICE Transactions on Communications ,vol. 98, no. 9, pp. 1758–1767, 2015.[35] L. Jiao, F. Y. Li, and Z. Xu, “LCRT: A TOA based mobile terminallocalization algorithm in NLOS environment,” in
VTC Spring 2009-IEEE69th Vehicular Technology Conference . IEEE, 2009, pp. 1–5.[36] K. Witrisal, P. Meissner, E. Leitinger, Y. Shen, C. Gustafson, F. Tufves-son, K. Haneda, D. Dardari, A. F. Molisch, A. Conti et al. , “High-accuracy localization for assisted living: 5G systems will turn multipathchannels from foe to friend,”
IEEE Signal Processing Magazine , vol. 33,no. 2, pp. 59–70, 2016.[37] Z. Wang and S. A. Zekavat, “Omnidirectional mobile nlos identificationand localization via multiple cooperative nodes,”
IEEE Transactions onMobile Computing , vol. 11, no. 12, pp. 2047–2059, 2011.[38] K. Han, S.-W. Ko, H. Chae, B.-H. Kim, and K. Huang, “Hidden vehiclespositioning via asynchronous V2V transmission: A multi-path-geometryapproach,” arXiv preprint arXiv:1804.10778 , 2018.[39] R. Mendrzik, H. Wymeersch, and G. Bauch, “Joint localization andmapping through millimeter wave MIMO in 5G systems,” in . IEEE, 2018, pp.1–6.[40] J. Palacios, P. Casari, and J. Widmer, “JADE: Zero-knowledge devicelocalization and environment mapping for millimeter wave systems,” in
IEEE INFOCOM 2017-IEEE Conference on Computer Communications .IEEE, 2017, pp. 1–9.[41] J. Palacios, G. Bielsa, P. Casari, and J. Widmer, “Communication-driven localization and mapping for millimeter wave networks,” in
IEEE INFOCOM 2018-IEEE Conference on Computer Communications .IEEE, 2018, pp. 2402–2410.[42] M. Aladsani, A. Alkhateeb, and G. C. Trichopoulos, “LeveragingmmWave imaging and communications for simultaneous localizationand mapping,” in
ICASSP 2019-2019 IEEE International Conferenceon Acoustics, Speech and Signal Processing (ICASSP) . IEEE, 2019,pp. 4539–4543.[43] A. Yassin, Y. Nasser, A. Y. Al-Dubai, and M. Awad, “Mosaic: Simul-taneous localization and environment mapping using mmWave withouta-priori knowledge,”
IEEE Access , vol. 6, pp. 68 932–68 947, 2018.[44] H. Kim, K. Granstr¨om, L. Gao, G. Battistelli, S. Kim, and H. Wymeer-sch, “5G mmWave cooperative positioning and mapping using multi-model phd filter and map fusion,”
IEEE Transactions on WirelessCommunications , 2020. [45] A. Blanco, N. Ludant, S. Zhenyu, W. Yi, and J. Widmer, “Performanceevaluation of single base station ToA-AoA localization in an LTEtestbed,” 2019.[46] G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, “Simultaneous localizationand mapping: A survey of current trends in autonomous driving,”
IEEETransactions on Intelligent Vehicles , vol. 2, no. 3, pp. 194–220, 2017.[47] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-ping: part I,”
IEEE robotics & automation magazine , vol. 13, no. 2,pp. 99–110, 2006.[48] M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, andM. Csorba, “A solution to the simultaneous localization and map build-ing (SLAM) problem,”
IEEE Transactions on robotics and automation ,vol. 17, no. 3, pp. 229–241, 2001.[49] J. Mullane, B.-N. Vo, M. D. Adams, and B.-T. Vo, “A random-finite-setapproach to bayesian SLAM,”
IEEE Transactions on Robotics , vol. 27,no. 2, pp. 268–282, 2011.[50] B. J. Frey and D. Dueck, “Clustering by passing messages between datapoints,” science , vol. 315, no. 5814, pp. 972–976, 2007.[51] W. Liu, G. Qin, Y. He, and F. Jiang, “Distributed cooperative re-inforcement learning-based traffic signal control that integrates V2Xnetworks’ dynamic clustering,”
IEEE Transactions on Vehicular Tech-nology , vol. 66, no. 10, pp. 8667–8681, 2017.[52] Y. Han, H. Wu, M. Jia, Z. Geng, and Y. Zhong, “Production capacityanalysis and energy optimization of complex petrochemical industriesusing novel extreme learning machine integrating affinity propagation,”
Energy Conversion and Management , vol. 180, pp. 240–249, 2019.[53] R. Van Der Merwe, A. Doucet, N. De Freitas, and E. A. Wan, “Theunscented particle filter,” in
Advances in neural information processingsystems , 2001, pp. 584–590.[54] B. Siciliano and O. Khatib,
Robotics and the Handbook . Springer,2016.[55] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson,R. Karlsson, and P.-J. Nordlund, “Particle filters for positioning, nav-igation, and tracking,”
IEEE Transactions on signal processing , vol. 50,no. 2, pp. 425–437, 2002.[56] A. Shapiro, “Monte Carlo sampling methods,”
Handbooks in operationsresearch and management science , vol. 10, pp. 353–425, 2003.[57] L. Yin, Q. Ni, and Z. Deng, “A GNSS/5G integrated positioning method-ology in D2D communication networks,”
IEEE Journal on SelectedAreas in Communications , vol. 36, no. 2, pp. 351–362, 2018.[58] R. Mendrzik, F. Meyer, G. Bauch, and M. Z. Win, “Enabling situationalawareness in millimeter wave massive mimo systems,”