Cooperative Localization with Angular Measurements and Posterior Linearization

Cooperati v e Localization with Angular Measurements and Posterior Linearization Y ibo W u ∗ , Bile Peng ∗ , Henk W ymeersch ∗ , Gonzalo Seco-Granados ‡ , Anastasios Kakkav as † § , Mario H. Casta ˜ neda Garcia † , and Richard A. Stirling-Gallacher † ∗ Department of Electrical Engineering, Chalmers Univ ersity of T echnology † Munich Research Center , Huawei T echnologies Duesseldorf GmbH ‡ Department of T elecommunications and Systems Engineering, Uni versitat Autonoma de Barcelona § Department of Electrical and Computer Engineering, T echnische Uni versit ¨ at M ¨ unchen Abstract —The application of cooperative localization in ve- hicular networks is attractiv e to impro ve accuracy and cov- erage. Con ventional distance measurements between vehicles are limited by the need for synchronization and pro vide no heading inf ormation of the vehicle. T o address this, we present a cooperative localization algorithm using posterior linearization belief propagation (PLBP) utilizing angle-of-arrival (AoA)-only measurements. Simulation results show that both directional and positional root mean squared error (RMSE) of vehicles can be decreased significantly and con verge to a low value in a few iterations. Furthermore, the influence of parameters for the vehicular network, such as vehicle density , communication radius, prior uncertainty and AoA measurements noise, is analyzed. I . I N T R O D U C T I O N V ehicular localization with high precision is of great im- portance for future autonomous dri ving. Among different pos- sibilities, e.g., global navigation satellite system (GNSS) [1], cooperativ e localization [2] enables the possibility for message passing (MP) between vehicles, which can lead to more accurate positioning and increased positioning cov erage. In cooperativ e localization, vehicles use on-board sensors, in- cluding 5G front-end, radar and stereo cameras [3], to obtain measurements relative to the positions of nearby vehicles. V ehicles exchange information related to relati ve positions and own position estimates to obtain an approximation of their own posterior distrib ution. Belief propagation (BP) [4] is a well-known frame work for Bayesian inference that can be ap- plied for the cooperativ e localization problem [2]. Cooperativ e localization is particularly advantageous when vehicles have different prior localization accuracy , because vehicles with high-quality sensors can help vehicles with low quality sensors to reduce their localization errors. The last point is practical in the foreseeable future because vehicles with dif ferent lev els of sensing precision are expected to coexist [5]. The performance of any localization system is limited by the underlying measurements. Con ventional measurements include distance and angle between vehicles. In terms of dis- tance measurements, radar can provide high accuracy , but does not include identity information of the target, required for MP. Measurements based on the trav el time of radio signals (time- of-arriv al (TO A) and time-dif ference-of-arriv al (TDO A)) can provide such identity information [6]–[8]. Howe ver , TO A and Fig. 1: Geometric model of two v ehicles. V ehicle i measures the AoA [ h ij ( x ij )] 1 from vehicle j , and vehicle j measures [ h ij ( x ij )] 2 . TDO A are challenged by the synchronization requirements [7]. The clocks of two vehicles need to be synchronized such that the delay can be computed. This can lead to significant localization error because of small clock error [9], or to use two-way TO A with round-trip delay time instead of the one-way delay to a void synchronization, which doubles the resource requirement. Achieving a ranging accuracy lower than 10 m by TO A/TDO A is very challenging in vehicular en vironments [10]. In contrast, AoA is readily a v ailable when the receiver is equipped with an antenna array [11]–[14]: [13] has in vestigated the performance of vehicle-to-v ehicle (V2V) relativ e positioning using AoA measurements from multiple receiving arrays on the vehicle, and the achie ved positioning accuracy met requirements of 5G New Radio (NR) vehicle-to- ev erything (V2X) standardization. While AoA measurements are attractive from a practical point of vie w , the integration in MP is non-trivial. Due to the nonlinear relation between the AoA and the vehicle state, analytical computation of the messages in BP is not possible. Approximations include the use of particles [15], [16] or linearization of the measurement model [17]. While the increasing number of particles giv es better approximation performance, it also increases the com- putation complexity . T o address this problem, [14] uses a von Mises-Fisher (VMF) model for the measurement likelihood and performs posterior linearization belief propagation (PLBP) [18], for a scenario with unkno wn positions but known orien- tation. In this paper, we consider a cooperati ve localization prob- lem where vehicles’ positions and orientations are unknown. W e apply Gaussian parametric BP [19] for the MP , which reduces the communication resource overhead and computa- tional comple xity compared to a particle approach. T o pass those messages through the nonlinear angle measurement model, posterior linearization (PL) [18] is applied to linearize the model using statistical linear regression (SLR) with respect to the posterior , which can be calculated by the current messages [20]. Based on the linearized model, the BP is then performed to update the new beliefs. This PLBP procedure can be iterated so that the posterior probability density function (PDF) of the vehicle position and orientation can con ver ge. I I . P R O B L E M S TA T E M E N T W e consider a network comprising a set of vehicles V = { 1 , ..., N } . A set of communication links E ⊂ V × V are considered to connect each vehicle according to a communi- cation radius r . The neighbor set of vehicle i is denoted by N i . Each vehicle i ∈ V has a state x i ∈ R 3 , comprising the 2D position [ x i , y i ] T and the heading θ i ∈ ( − π , π ] . W e denote the joint state of vehicles i and j as x ij = [ x T i x T j ] T . Each vehicle is assumed to hav e kno wledge of its prior state by some accessible positioning techniques, e.g., GNSS, assumed to be a Gaussian density p i ( x i ) = N ( x i ; µ i , P i ) , (1) where N ( x i ; µ i , P i ) denotes a Gaussian distribution in v ari- able x i with mean vector µ i = [ µ x , µ y , µ θ ] | and cov ariance matrix P i . The measurement model between two vehicles is shown in Fig. 1. Each vehicle i is equipped with linear arrays on its two sides, each of which provides a field of view (FO V) ϕ i with 0 < ϕ i ≤ π . Signals with an AoA measurements within the FO V of node can be measured. The AoA measurement v ector z ij between vehicles i and j is defined as a function of x i and x j with additive Gaussian noise z ij = h ij ( x ij ) + η ij , (2) where η ij represents the measurement noise, modeled as η ij ∼ N ( 0 , R ij ) and h ij ( x ij ) is defined as 1 h ij ( x ij ) =  atan2 (( y j − y i ) , ( x j − x i )) − θ i atan2(( y i − y j ) , ( x i − x j )) − θ j  , (3) in which atan2( y , x ) calculate the four-quadrant in verse tan- gent of y and x . Howe ver , the atan2 introduces problems because of its discontinuity at the negati ve semi-axis of x , i.e. ( x, 0) : x < 0 . Instead of modeling the angular measurements 1 For simplicity we consider the center points of the two arrays on each vehicle to coincide. The ef fect of the relative position and orientation of the antenna arrays is outside the scope of this paper and related work can be found in [21]. by VMF distribution, as [14] has done, we adopt a simple ad- hoc correction from [22], which is described in Appendix A. W e denote the vector of all measurements by z = [ z ij ] i,j ∈N i and the vector of all vehicles’ states by x . The goal of the network is to compute p i ( x i | z ) , for each vehicle. I I I . B E L I E F P RO PAG A T I O N A N D P O S T E R I O R L I N E A R I Z A T I O N A. Belief Pr opagation F ormulation The standard approach to solve the localization problem is to use belief propagation. W e first factorize the joint PDF p ( x , z ) = p ( x ) p ( z | x ) (4) = N Y i =1 p i ( x i ) Y j ∈N i ,j >i p ( z ij | x ij ) . (5) A factor graph representation of this joint PDF in combination with loopy BP allows the computation of approximations of the marginal posteriors p i ( x i | z ) . The BP message passing rules at iteration k are as follows (assuming j ∈ N i ) [4] b ( k − 1) j ( x j ) ∝ p j ( x j ) Y i ∈N j m ( k − 1) i → j ( x j ) (6) m ( k ) j → i ( x i ) ∝ Z p ( z ij | x ij ) b ( k − 1) j ( x j ) m ( k − 1) i → j ( x j ) d x j . (7) The approximate marginal posterior at iteration k is p j ( x j | z ) ≈ b ( k ) j ( x j ) . The process is initialized at k = 0 by b (0) j ( x j ) = p j ( x j ) and m (0) i → j ( x j ) = 1 . The joint posterior of x i , x j can also be approximated by [4] b ( k ) ( x ij ) ∝ p ( z ij | x ij ) b ( k ) i ( x i ) b ( k ) j ( x j ) m ( k ) i → j ( x j ) m ( k ) j → i ( x i ) . (8) Howe ver , due to the nonlinear observation model (2), in gen- eral BP cannot be executed in closed form: neither the integral (6) nor the product (7) can be computed exactly , except when the observation model is linear with Gaussian noise [18]. This motiv ates the following linearization procedure. B. Linearization Giv en a belief b ( k ) ( x ij ) , we approximate the observation model by h ij ( x ij ) ≈ C ij ˜ x ij + e ij , (9) where e ij ∼ N ( 0 , Ω i,j ) , and ˜ x ij = [ x T ij 1 T ] T . C ij is selected to minimize the mean square error (MSE) over the giv en joint belief b ( k ) ( x ij ) : arg min C ij E {k h ij ( x ij ) − C ij ˜ x ij k 2 } . (10) Once C ij is determined, we find that Ω i,j = k h ij ( x ij ) − C ij ˜ x ij k 2 . T o solve this optimization problem, the SLR [18] with respect to the posterior PDF is performed, where the de- tails are presented in Appendix A. T o visualize the advantage of posterior SLR, Fig. 2 sho ws the true measurement model (3) and its approximations (9) with respect to posterior and prior . W e observ e that the linearized model by posterior SLR is more accurate and has less uncertainty than the model linearized by prior SLR. 1 0 2 0 3 0 4 0 5 0 x i o f x i ( m ) − 1 . 0 − 0 . 5 0 . 0 0 . 5 1 . 0 h i j ( x i j ) ( r a d ) T r u e m e a n o f x i j T r u e h i j ( x i j ) Pr i o r m e a n o f x i j L i n e a r i z e d h i j ( x i j ) w . r . t p r i o r Po st e r i o r m e a n o f x i j L i n e a r i z e d h i j ( x i j ) b y p o st e r i o r Fig. 2: The true measurement model h ij ( x ij ) and its approximations by SLR with respect to the prior and posterior , as a function of the x -dimension of x i . The length of the red and blue lines represent 2 standard deviations of the prior and posterior linearized models, respectiv ely . C. Belief Pr opagation with Linearized Measurement Models Once a linearization of all measurement models is obtained, BP is performed as follo ws. The likelihood function is now of the form p ( z ij | x ij ) ∝ (11) exp  − 1 2 ( z ij − C ij ˜ x ij ) T Σ − 1 ij ( z ij − C ij ˜ x ij )  , where Σ ij = Ω ij + R ij . This formulation now allows closed- form Gaussian message passing according to (6)–(7) and (8). The details of the implementation are provided in the Appendix B. The overall algorithm thus operates as described in Algo- rithm 1. The algorithm requires a selection of K (the number of linearization iterations) and M (the number of BP iterations per linearization step). The overall complexity per vehicle is approximately O ( K M ¯ N D 3 ) , where D is the state dimension and ¯ N is the average number of neighbors. Algorithm 1 : Iterative Cooperativ e Localization for k = 1 to K do Giv en the current beliefs b ( k − 1) ( x ij ) , solve (10) for each ( i, j ) ∈ E to obtain (11). Run M iterations of BP on the linearized model. Compute joint beliefs b ( k ) ( x ij ) at the current BP itera- tion. end for Return marginal beliefs. T ABLE I: Setup parameters for the vehicular scenario. r [m] ϕ [rad] σ x [m] σ y [m] σ θ [rad] R [rad 2 ] 30 π 5 5 0.35 0.10 Fig. 3: Scenario of the vehicular network. The interactiv e web map can be found in [25]. I V . S I M U L A T I O N R E S U L T S In this section we simulated a vehicular network scenario and analyzed the performance of the designed Algorithm 1. First, the localization and orientation performance of Algo- rithm 1 in the vehicle network is e v aluated by the positional and directional root mean squared error (RMSE). Then, based on this scenario, we analyzed the impact of different network parameters. A. Simulation Scenario The vehicular scenario is based on a road map in cen- tral New Y ork Manhattan (latitude: 40 . 71590 and longitude: − 73 . 99560 ). The map data is generated from Stamen Map [23] at a zoom le vel of 18. W ithin this map, the scenario is sho wn in Fig. 3, where 51 vehicles are possibly connected within the communication radius ( r = 30 m). The priors are set to P i = diag ( σ 2 x , σ 2 y , σ 2 θ ) . Among the vehicles, 6 are chosen as anchors (vehicles or road side units with a very concentrated prior density , set to diag ( σ 2 x , σ 2 y , σ 2 θ ) = diag (0 . 01 , 0 . 01 , 0 . 01) ). The interactiv e web map is also provided 2 [25]. The remaining parameters of this scenario are illustrated in T able I, where R denotes the constant value of the measurement variance (approximately 18 degrees standard deviation). B. Results and Discussion 2 The results of the scenario can be visualized by an interactive web map in [24], where the red, blue, and green dots represent the true, prior and estimated positions, respectively . 0 1 2 3 4 5 6 7 8 9 1 0 L i n e a r i z a t i o n i t e r a t i o n n u m b e r , k 2 4 6 R M S p o si t i o n e r r o r ( m ) Po st e r i o r L F , M = 1 Po st e r i o r L F , M = 2 Po st e r i o r L F , M = 3 Po st e r i o r L F , M = 1 0 Pr i o r L F , M = 1 Pr i o r L F , M = 2 Pr i o r L F , M = 3 Pr i o r L F , M = 1 0 0 1 2 3 4 5 6 7 8 9 1 0 L i n e a r i z a t i o n i t e r a t i o n n u m b e r , k 0 . 0 0 . 1 0 . 2 0 . 3 R M S d i r e c t i o n e r r o r ( r a d ) Po st e r i o r L F , M = 1 Po st e r i o r L F , M = 2 Po st e r i o r L F , M = 3 Po st e r i o r L F , M = 1 0 Pr i o r L F , M = 1 Pr i o r L F , M = 2 Pr i o r L F , M = 3 Pr i o r L F , M = 1 0 Fig. 4: RMS position and direction error against the number of linearization iteration k . The initial position and direction RMSE of vehicles are 7.01m and 0.38 rad, respectively . 0 2 4 6 8 1 0 L o c a t i o n E r r o r ( m ) 0 . 0 0 . 2 0 . 4 0 . 6 0 . 8 1 . 0 C DF Po st e r i o r L F , M = 1 Po st e r i o r L F , M = 2 Po st e r i o r L F , M = 3 Po st e r i o r L F , M = 1 0 Pr i o r L F , M = 1 Pr i o r L F , M = 2 Pr i o r L F , M = 3 Pr i o r L F , M = 1 0 Pr i o r st a t e 0 . 0 0 0 . 0 5 0 . 1 0 0 . 1 5 0 . 2 0 0 . 2 5 0 . 3 0 0 . 3 5 0 . 4 0 O r i e n t a t i o n E r r o r ( r a d ) 0 . 0 0 . 2 0 . 4 0 . 6 0 . 8 1 . 0 C DF Po st e r i o r L F , M = 1 Po st e r i o r L F , M = 2 Po st e r i o r L F , M = 3 Po st e r i o r L F , M = 1 0 Pr i o r L F , M = 1 Pr i o r L F , M = 2 Pr i o r L F , M = 3 Pr i o r L F , M = 1 0 Pr i o r st a t e Fig. 5: CDF of localization and orientation error, K = 10 . 1) Con ver gence Speed: In order to examine the perfor- mance of Algorithm 1, in Fig. 4 we plot the RMS position and direction error against the number of linearization iteration K . Notice the performance gap between the prior linearization filter (LF) (dotted lines) and the posterior LF (solid lines). After each belief propagation iteration, the posterior of each vehicle is closer to the true state than the prior , so the belief propagation has a better performance on the posterior linearization measurement model. Both position RMSE and direction RMSE conv erged for linearization iteration number larger than 4. Meanwhile, increasing M from 1 to 3 provides significant improv ements for both position and orientation estimation accuracy as the beliefs are more accurate. The improv ement becomes very small for M greater than 3. 2) Localization P erformance: While the above results sho w the average RMSE of the position and direction, Fig. 5 shows the cumulativ e distribution functions (CDFs) of the position 2 0 4 0 6 0 C o m m u n i c a t i o n r a d i u s r ( m ) 1 2 3 4 5 6 R M S p o si t i o n e r r o r ( m ) 0 . 0 0 0 . 0 5 0 . 1 0 0 . 1 5 0 . 2 0 R M S d i r e c t i o n e r r o r ( r a d ) 0 . 0 5 0 . 1 0 A O A m e a su r e m e n t n o i se √ R ( r a d ) 0 . 5 1 . 0 1 . 5 2 . 0 2 . 5 3 . 0 R M S p o si t i o n e r r o r ( m ) 0 . 0 2 0 . 0 4 0 . 0 6 0 . 0 8 R M S d i r e c t i o n e r r o r ( r a d ) 0 1 0 2 0 3 0 Po si t i o n u n c e r t a i n t y , σ p ( m ) 0 1 0 2 0 3 0 R M S p o si t i o n e r r o r ( m ) 0 . 0 0 . 1 0 . 2 0 . 3 R M S d i r e c t i o n e r r o r ( r a d ) 0 . 0 0 . 1 0 . 2 0 . 3 Di r e c t i o n u n c e r t a i n t y , σ d ( r a d ) 1 . 0 1 . 2 1 . 4 1 . 6 R M S p o si t i o n e r r o r ( m ) 0 . 0 0 0 . 0 1 0 . 0 2 0 . 0 3 0 . 0 4 R M S d i r e c t i o n e r r o r ( r a d ) Fig. 6: The impact of 4 vehicle netw ork parameters on localization and orientation performance. Lines with square and triangle markers represent the position and orientation RMSE, respectiv ely . K = 10 , M = 10 and posterior LF are applied. and direction errors for K = 10 for different values of M . W e observe that for M = 3 the performance is similar to M = 10 and that nearly all vehicles can be localized with a position error less then 4 meters and an orientation error less than 0.15 radians (8 degrees). The importance of posterior linearization ov er prior linearization is again clear . 3) Impact of Network parameters: Here, we analyze the impact of modifying the scenario parameters in T able I on localization and orientation estimation performance. In Fig. 6, we e valuate 4 parameters separately , namely communication radius ( r ), measurement noise variance ( R ), prior uncertainty in position ( σ p = ( σ 2 x + σ 2 y ) 1 / 2 ) and prior uncertainty in orientation ( σ θ ) in 4 sub-figures, by plotting the position and direction RMSE as functions of one of them, while keeping the rest fixed to the values of T able I. • The top left sub-figure shows the impact of the commu- nication radius r . Both RMSEs are reduced rapidly by increasing r from 10 m to 30 m since each vehicle has more neighbors and the network connectivity increases quickly , up to the point where all vehicles are in each others’ communication range. W e note that with increased connectivity comes increased computational complexity . • In the top right sub-figure, we v ary the AoA measure- ments noise variance R . W e note that both direction and position RMSE increase approximately linearly in √ R . This emphasizes the need for good measurements. • The influence of the prior position uncertainty ( √ σ x , σ y ) is shown in the bottom left sub-figure. The red dashed line describes the prior position RMSE. W e notice the increase of σ p from 0 m to 10 m has small effect on both position and direction performance (less than 2 m/0.05 rad), showing the good performance of the proposed method. For position uncertainty o ver 10 m, Algorithm 1 is still able to improv e performance over the prior RMSE, but leads to progressiv ely lar ger errors. This is in contrast to range-based cooperati ve localization [2], where no prior information was needed. • The influence of the direction uncertainty ( σ θ ) is sho wn in the bottom right sub-figure, where we observe a rapid in- crease in RMSE. This is because the AoA measurements depend on the orientation of the receiving vehicles. For larger prior orientation uncertainty , Algorithm 1 is less affected. V . C O N C L U S I O N W e ha ve applied PLBP to cooperative localization (posi- tion and orientation estimation) of vehicles with AoA-only measurements. Multiple conditions of the vehicular network, including the vehicle density , communication radius, prior un- certainty and measurement noise v ariance hav e been discussed. Numerical results show that the proposed algorithm has good performance in terms of both position and orientation estima- tion, and only a fe w iterations are required for con ver gence. This makes the algorithm attractive for real-time processing. A C K N O W L E D G M E N T This research was supported, in part, by the EU Horizon 2020 project 5GCAR (Fifth Generation Communication Au- tomotiv e Research and innov ation) and the Spanish Ministry of Science, Inno v ation and Uni versities under Grant TEC2017- 89925-R. A P P E N D I X A S T E P S O F T H E P O S T E R I O R L I N E A R I Z A T I O N This section illustrates the procedures of SLR on the measurement model and the approximation of the parameters ( C ij , Ω i,j ) with respect to the joint posterior PDF p ( x ij | z ij ) = N ( x ij ; µ ij ; P ij ) . First, according to the joint posterior of x i , x j , we select L sigma-points X 1 , ..., X L and weights ω 1 , ..., ω L using a sigma-point method such as the unscented transform [26]. Then we calculate the transformed sigma points by Z l = h ij ( X l ) l = 1 , ..., L (12) Howe ver , as mentioned in Section II, the function arctan has discontinuity problem at the negati ve x semi-axis. The sigma points transformation needs an ad-hoc modification so that the difference between angles Z l − z ij must be bounded in ± π . Z l can be corrected to ˆ Z l by the following transformation: ˆ Z l = z ij + π − modulo (( z ij − Z l ) + π ) 2 π (13) where ˆ Z l denotes the corrected sigma point, z ij is the AoA measurements and modulo ( · ) 2 π represents the modulo opera- tion. Introducing C ij = [ A ij b ij ] , so that h ij ( x ij ) ≈ A ij x ij + b ij + e ij , (14) the solution of the approximation of A ij , b ij , Ω i,j is A ij = C T xz P − 1 ij (15) b ij = ¯ z − A ij µ ij (16) Ω i,j = C z z − A ij P ij A T ij (17) where ¯ z , C xz and C z z are approximated using the sigma- points (13) and weights by ¯ z ≈ L X j =1 ω j ˆ Z l (18) C xz ≈ L X j =1 ω j ( X j − µ ij )( ˆ Z l − ¯ z ) T (19) C z z ≈ L X j =1 ω j ( ˆ Z l − ¯ z )( ˆ Z l − ¯ z ) T . (20) A P P E N D I X B I M P L E M E N T A T I O N O F B P I N T H E L I N E A R I Z E D M O D E L This section illustrates the deriv ation of equation (6)–(7) and (8). Once we ha ve the approximated linearization model 9, we can represent the BP message m ( k ) i → j by the Gaussian format [20] m ( k ) i → j ( x j ) ∝ N ( α ( k ) ij ; H ( k ) ij x j , Γ ( k ) ij ) (21) where α ( k ) ij , H ( k ) ij and Γ ( k ) ij are α ( k ) ij = [ z ij ] 1 − A i µ ( k − 1) ij − b ij (22a) H ( k ) ij = A j (22b) Γ ( k ) ij = R ij + Ω ij + A i P ( k − 1) ij A | i (22c) where [ z ij ] 1 is the AoA measurement receiv ed by vehicle i , A i , A j are defined at Section III-B and µ ( k − 1) ij and P ( k − 1) ij are found from the relation N ( µ ( k − 1) ij , P ( k − 1) ij ) ∝ N ( x i ; µ i , P i ) Y j 0 ∈N i \ j m ( k − 1) j 0 → i ( x i ) (23) where the Kalman update step [20, Algorithm 1] is per- formed to update each message m ( k − 1) j 0 → i ( x i ) on the prior state N ( x i ; µ i , P i ) . T o get the local belief (6) at the k -th iteration, we can also use Kalman filter update step to update the vehicle prior with all its incoming messages. b ( k ) j ( x j ) = N ( x j ; µ j , P j ) × Y i ∈N j m ( k ) i → j ( x j ) (24) The k -th iteration joint posterior (8) is expressed as [20] b ( k ) ( x ij ) = N ( x i ; µ i , P i ) Y j 0 ∈N i \ j m ( k ) j 0 → i ( x i ) (25) × N ( x j , µ j , P j ) × Y i 0 ∈N j \ i m ( k ) i 0 → j ( x j ) p ( z ij | x i , x j ) where we can also apply Kalman filter update [20, Algorithm 1] as in (23). R E F E R E N C E S [1] S. Gleason, D. Gebre-Egziabher, and D. G. Egziabher , “GNSS applica- tions and methods, ” 2009. [2] H. W ymeersch, J. Lien, and M. Z. Win, “Cooperative localization in wireless networks, ” Pr oceedings of the IEEE , vol. 97, no. 2, pp. 427– 450, Mar . 2009. [3] F . de Ponte M ¨ uller , “Surve y on ranging sensors and cooperative tech- niques for relati ve positioning of vehicles, ” Sensors , vol. 17, no. 2, p. 271, Jan. 2017. [4] F . R. Kschischang, B. J. Frey , H.-A. Loeliger et al. , “Factor graphs and the sum-product algorithm, ” IEEE T ransactions on information theory , vol. 47, no. 2, pp. 498–519, Jul. 2001. [5] E. Steinmetz, R. Emardson, F . Br ¨ annstr ¨ om, and H. W ymeersch, “Theo- retical limits on cooperative positioning in mixed traffic, ” IEEE Access , vol. 7, pp. 49 712–49 725, 2019. [6] P . H. Mohammadabadi and S. V alaee, “Cooperative node positioning in vehicular networks using inter-node distance measurements, ” in 2014 IEEE 25th Annual International Symposium on P ersonal, Indoor , and Mobile Radio Communication (PIMRC) . IEEE, 2014, pp. 1448–1452. [7] A. Catovic and Z. Sahinoglu, “The Cramer-Rao bounds of hybrid TO A/RSS and TDO A/RSS location estimation schemes, ” IEEE Com- munications Letters , vol. 8, no. 10, pp. 626–628, Jan. 2004. [8] M. R. Gholami, S. Gezici, E. G. Strom, and M. Rydstrom, “Hybrid TW - TO A/TDOA positioning algorithms for cooperative wireless networks, ” in 2011 IEEE International Confer ence on Communications (ICC) . IEEE, Jul. 2011, pp. 1–5. [9] R. M. Buehrer, H. W ymeersch, and R. M. V aghefi, “Collaborati ve sensor network localization: Algorithms and practical issues, ” Proceedings of the IEEE , vol. 106, no. 6, pp. 1089–1114, Jun. 2018. [10] N. Alam and A. G. Dempster , “Cooperati ve positioning for vehicular net- works: Facts and future, ” IEEE transactions on intelligent transportation systems , vol. 14, no. 4, pp. 1708–1717, Jun. 2013. [11] S. Sakagami, S. Aoyama, K. Kuboi, S. Shirota, and A. Ak eyama, “V ehicle position estimates by multibeam antennas in multipath envi- ronments, ” IEEE T ransactions on V ehicular T echnology , vol. 41, no. 1, pp. 63–68, Feb. 1992. [12] A. Fascista, G. Ciccarese, A. Coluccia, and G. Ricci, “ Angle of arriv al- based cooperative positioning for smart vehicles, ” IEEE T ransactions on Intelligent Tr ansportation Systems , no. 99, pp. 1–13, Nov . 2017. [13] A. Kakkav as, M. H. C. Garcia, R. A. Stirling-Gallacher , and J. A. Nossek, “Multi-array 5G V2V relativ e positioning: Performance bounds, ” in 2018 IEEE Global Communications Conference (GLOBE- COM) . IEEE, Dec. 2018, pp. 206–212. [14] A. F . Garcia-Fernandez, F . T ronarp, and S. Sarkka, “Gaussian tar- get tracking with direction-of-arriv al von Mises-Fisher measurements, ” IEEE T ransactions on Signal Processing , Apr . 2019. [15] B. Etzlinger, F . Meyer , A. Springer , F . Hlawatsch, and H. W ymeer- sch, “Cooperative simultaneous localization and synchronization: A distributed hybrid message passing algorithm, ” in Signals, Systems and Computers, 2013 Asilomar Conference on . IEEE, Nov . 2013, pp. 1978– 1982. [16] V . Savic and S. Zazo, “Cooperative localization in mobile networks using nonparametric variants of belief propagation, ” Ad Hoc Networks , vol. 11, no. 1, pp. 138–150, Mar . 2013. [17] E. A. W an and R. V an Der Merwe, “The unscented Kalman filter for nonlinear estimation, ” in Pr oceedings of the IEEE 2000 Adaptive Sys- tems for Signal Pr ocessing, Communications, and Contr ol Symposium (Cat. No. 00EX373) . IEEE, Oct. 2000, pp. 153–158. [18] ´ A. F . Garc ´ ıa-Fern ´ andez, L. Svensson, M. R. Morelande, and S. S ¨ arkk ¨ a, “Posterior linearization filter: Principles and implementation using sigma points, ” IEEE T ransactions on Signal Pr ocessing , vol. 63, no. 20, pp. 5561–5573, Jul. 2015. [19] W . Y uan, N. Wu, B. Etzlinger , H. W ang, and J. Kuang, “Cooperative joint localization and clock synchronization based on Gaussian message passing in asynchronous wireless networks. ” IEEE T rans. V ehicular T echnology , vol. 65, no. 9, pp. 7258–7273, Jan. 2016. [20] ´ A. F . Garc ´ ıa-Fern ´ andez, L. Svensson, and S. S ¨ arkk ¨ a, “Cooperativ e localization using posterior linearization belief propagation, ” IEEE T ransactions on V ehicular T echnology , vol. 67, no. 1, pp. 832–836, Jan. 2018. [21] Y . Shen and M. Z. W in, “On the accuracy of localization systems using wideband antenna arrays, ” IEEE T ransactions on Communications , vol. 58, no. 1, pp. 270–280, Jan. 2010. [22] D. F . Crouse, “Cubature/unscented/sigma point Kalman filtering with angular measurement models, ” in 2015 18th International Conference on Information Fusion (Fusion) . IEEE, Jul. 2015, pp. 1550–1557. [23] Stamen-Map, “Map of central Manhattan, ” http://maps.stamen.com/m2i/ #toner/256:256/18/40.71590/- 73.99560, May . 13, 2019, [May . 27, 2019]. [24] ——, “Conv ergence map of the vehicular network scenario, ” https:// hhsly .github.io/paper plbp fig3/, May . 13, 2019, [May . 27, 2019]. [25] ——, “Initialization map of the vehicular network scenario, ” https:// hhsly .github.io/paper plbp fig2/, May . 13, 2019, [May . 27, 2019]. [26] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation, ” Proceedings of the IEEE , vol. 92, no. 3, pp. 401–422, 2004.