Graph based Cooperative Localization for Connected and Semi-Autonomous Vehicles

Cooperative Real-time Localization is expected to play a crucial role in various applications in the field of Connected and Semi-Autonomous vehicles (CAVs), such as collision avoidance/warning, cooperative adaptive cruise control, etc. Future 5G wireless systems are expected to enable cost-effective Vehicle-to-Everything (V2X) systems, allowing CAVs to share the measured data with other entities of the network. Typical measurement models usually deployed for this problem, are absolute position from Global Positioning System (GPS), relative distance to neighboring vehicles and relative angle or azimuth angle, extracted from Light Detection and Ranging (LIDAR) or Radio Detection and Ranging (RADAR) sensors. In this paper, we provide a cooperative localization approach that performs multi modal-fusion between the interconnected vehicles, by representing a fleet of connected cars as an undirected graph, encoding each vehicle position relative to its neighboring vehicles. This method is based on the so called Laplacian Processing, a Graph Signal Processing tool, that allows to capture intrinsic geometry of the undirected graph of vehicles rather than their absolute position on global coordinate system, significantly outperforming current state of the art approaches, in terms of localization mean square and maximum absolute error and computational complexity.


I. INTRODUCTION
Real-time Localization is one of the main pillars of Intelligent Transportation Systems (ITS). Although, Global Navigation Satellite Systems (GNSSs), e.g GPS, provide absolute position information, their accuracy is limited to 10 m or higher [8], [10], especially in harsh environments such as urban canyons and tunnels. Since the desired localization error in autonomous driving applications should be no greater than 5 m in the worst case [7], efficient methods for vehicle localization should be developed. In recent years, there is a growing interest in Cooperative Localization (CL) as a means to improve GPS accuracy. CL is based on the 5G communication technology V2X, allowing the vehicles of a Vehicular-Ad-hoc-NETwork (VANET) to share information among them. Useful information could be absolute position from GPS, relative distance or angle to neighboring vehicles from LIDAR/RADAR. Thus, CL consists in fusing different measurement modalities used in VANET, in order to improve the position accuracy.
Recent approaches of CL on VANETs, include [2] where an objective function, based on absolute positions, relative distances and relative angles measurements of the vehicles, is minimized by employing Alternating Direction Method of Multipliers (ADMM). In [3], vehicles share absolute position, relative position and motion state measurements and CL is performed by a covariance intersection filter (CIF), integrating those informations. The VANET of [4], fuses absolute position and range measurements, using Extended Kalman Filter (EKF) and CIF. In [5], a CL method in tunnels, that fuses V2X measurements using particle filtering, is presented. In [6], a CL method in urban canyons, that fuses absolute position and range measurements using EKF is proposed. In [7], CL is achieved by employing V2X measurements between vehicles and infrastructure. In [8], GPS and vehicles dynamics measurements are fed into a Kalman Filter (KF) and combined with any information available about surrounding features such as relative positions of people, traffic lights, etc. In [9], a Bayesian approach that fuses GPS and inter-vehicle distance measurements, is employed in order to perform CL. In [1], CL approaches in Wireless Sensor Networks are distinguished to Bayesian and Non-Bayesian, that rely on Maximum A Posteriori (MAP) estimation and Maximum Likelihood Estimation (MLE), respectively. In [12], distributed solutions that fuse received signal strength and angle of arrival measurements for Internet-of-Things nodes localization are presented. Finally, [10] and [11] provide an overview for cooperative and noncooperative localization techniques in VANETs.
The previously discussed methods, focus only on the pairwise measurements of the interconnected vehicles. To the best of our knowledge, the method proposed here is the first one which, apart from the measurement models, it also takes into account the connectivity properties of the underlying graph formed by the involved vehicles. Moreover, we integrate those properties in our location estimation approach.
Our main contributions can be summarized as follows: • A novel method for efficient CL in VANETs is proposed. The method performs cooperative multi-modal fusion exploiting the intrinsic geometric properties. This is achieved by encoding each vehicle's position, relative to its neighboring vehicles. • The new method can be implemented both in a central-ized (assuming the existence of a fusion center) and local ('distributed') fashion. In the latter case, each vehicle interacts directly only with its immediate neighborhood. • As shown via extensive experiments, the new method outperforms existing state-of-the-art method in terms of both accuracy and execution time. The gain in execution time is as high as 2 or 3 orders of magnitude. Note that the method chosen for comparison is the one in [2] because it exploits exactly the same types of measurements. The rest of the paper is organized as follows: Section II provides the background of traditional CL in VANETs; Section III presents the proposed method while Section IV is dedicated to the experimental setup and simulation results and Section V concludes our work.

II. COOPERATIVE NETWORK LOCALIZATION
Consider a 2-D region where N connected vehicles collect measurements while moving. An example of such a VANET, is shown in Fig. 1. The location of the i-th vehicle at k-th . Each vehicle knows its absolute position from GPS and measures its relative distances and angles with respect to neighboring vehicles using LIDAR or RADAR. The true relative distance z (k) d,ij between connected vehicles i and j is given by: , where ∥·∥ is the l 2 norm. The true relative angle (shown in Fig. 2 a,ij between neighboring and connected vehicles i and j is given by: The acquired measurements are assumed to be described by the following models: • Relative Distance measurement: • Relative Angle measurement: • Absolute position measurement: Covariance matrix Σ p is a diagonal matrix equal to diag(σ 2 x , σ 2 y ). Note that the noise introduced in the measurements is assumed to be Gaussian, as commonly done in relevant literature [2], [3], [4], [5].
A typical approach in CL is to formulate an objective cost function C ( x (k) ) according to MLE [1], [2] and to minimize it with respect to locations x (k) i , in order to reduce the error of absolute position measurement. The probability density functions of the respective measurement models are defined below: The likelihood function of the measurement models can be written as: , where N (i) denotes the set of neighbors of the i-th vehicle. If we take the logarithm of (9), then the objective cost function (same as in [2] and similar to that of [1]) is given by: For the minimization of (10), distributed ADMM [2], cooperative gradient descent [1] or interior point methods (provided by CVX software) can be applied.

III. SPARSE LAPLACIAN BASED LOCALIZATION
The method that will be derived in this Section is based on a proper extension of the Laplacian Processing technique [13] that was originally proposed for surface representation. The motivation was that by modelling a VANET as an undirected graph, one can exploit not only the different measurement modalities between connected vehicles, but also the connectivity properties of vehicles.

A. Centralized Scheme
Let G (k) = (V (k) , E (k) ) be an undirected graph, which changes over time, where V (k) is the set of vertices and E (k) the set of edges. Each vertex v (k) i is represented by absolute cartesian coordinates as v . The differential coordinates δ . Relying on the previously defined δ (k) coordinates, one can recover the true absolute coordinates of the vertices, represented by the vectors x (k) ∈ R N ×1 and y (k) ∈ R N ×1 , by solving the systems: (11) Each vehicle, using LIDAR or RADAR, can also exploit, the Azimuth Angle (shown in Fig. 2) measurement between vehicle observer i and vehicle target j: For example, in Fig. 2, λ = 1 2 . We can also notice from By considering the vehicles of the network as vertices of a graph (example in Fig. 3) and the communication link between the neighbors as its edges, we can create matrices D (k) , A (k) , L (k) . Moreover, each vehicle utilizing measurement models (3) and (12), can define the δ (k) i coordinates and sent them to a fusion center. The latter will try to solve the systems of (11) and estimate the true locations of vehicles. However, L (k) is a singular matrix, which implies that systems in (11) are not solvable. Thus, we need to add into the systems some anchor points c with known absolute coordinates. Furthermore, if we assume as anchors After adding the anchor points, the systems in (11) can be re-written as: The solution of (15) and (16) corresponds to the minimizing of: The required location vectors x (k) and y (k) can be computed as follows: In practice, as anchor points the noisy GPS positions (measurement model (5)) of the vehicles of the network can be used. This differentiate us with the majority of CL methods, where the location of anchors is considered to be the true and without any ambiguity. Furthermore, each δ i (k) must be multiplied with d (k) i , so as to remain in accordance with (11).
Moreover, due to the limited communication range of vehicles' transceivers, matrixL (k) is actually sparse. Thus, one may use a sparse least-squares solver [14], in order to solve (17) and (18). We named our centralized CL approach Graph based CL-Centralized Laplacian or GCL-CLapl. The main steps of GCL-CLapl are summarized on Algorithm 1.

B. Local Laplacian Implementation
In a local Laplacian processing approach, each vehicle receives GPS measurements from neighboring vehicles and solves a local Laplacian problem. This may offer some advantages as compared to the centralized technique, such as: a) no need to define a central node, which is difficult in such a varying topology of VANETs and b) robustness against failure of the central node. In the local Laplacian implementation presented here, instead of using the overallL (k) , we use local L Fig. 3 is equal to:  (26). The presented approach, offers resilience against potential cyber-failures, while as it will be also shown in the following section its accuracy is close enough to that of the global centralized method. As a future step we plan to extend this method to a fully distributed scheme, though the gain margins seems to be limited, compared to the communication and processing overheads that will be potentially required. We named the local (or 'distributed') approach of this Section Graph based CL-Distributed Laplacian or GCL-DLapl. GCL-DLapl is summarized on Algorithm 2. Note that the az,ij ; Last element of x local scheme, could also be useful for the self-localization task, where instead of cooperating vehicles, visually detected road landmarks (e.g. poles, facades) [15] can be used. Distances and angles between the landmark and the vehicle or the relative position of landmark (determined by LIDAR or Camera), could be exploited.

A. Experimental Setup
The conducted experiments were based on different levels of noise of measurement models and different number of vehicles. The initial locations of vehicles were determined assuming uniform distribution for x and y coordinates. We generate the trajectories of vehicles for 500 time instances according to the vehicle motion model of [5]. For a reduced computational load, active communication links are assumed to exist between the vehicles, only if their distance is lower than 20 m, and the number of connected neighbors is at most 6. Moreover, each vehicle always has a connected neighbor. Based on the aforementioned assumptions we consider three different approaches: i) the Traditional CL of (10) based on MLE or TCL-MLE, ii) the proposed centralized scheme GCL-CLapl, and iii) the proposed local ('distributed') scheme GCL-DLapl. We apply the three approaches each time instant. We minimize (10)

B. Evaluation Study
The performances of the proposed method, as well as that of the method in [2], depend on the error introduced in range measurements. The variance of the range measurements error is usually much smaller than the variance of the GPS error, allowing an accurate estimation of the differential coordinates, which seems to be a requirement for estimating accurately the actual vehicles position. In Fig. 4, it is evident that there is a significant reduction of GPS error. In Fig. 4-(a) the reduction of error is 69% for GCL-CLapl, 60% for GCL-DLapl and 60% for TCL-MLE. In Fig. 4-(b) the reduction of error is 56% for GCL-CLapl, 53% for GCL-DLapl and 50% for TCL-MLE. GCL-CLapl outperforms TCL-MLE and GCL-DLapl in Fig. 4-(a),-(b). The increased noise of range measurements ( Fig. 4-(b)) results in deteriorated performance for all three methods. However, our schemes turn out to be more robust to noise. This is apparently due to the fact that the proposed method exploits not only the (noisy) inter-vehicle measurements but it also integrates properly the connectivity representation of VANET graph. Thus, the impact of noise could be further reduced.
We also tested our method on different number of vehicles, namely 5 and 20. In Fig. 5-(a), the reduction of error is 80% for GCL-CLapl, 78% for GCL-DLapl and 71% for TCL-MLE. In Fig. 5-(b), the reduction of error is 87% for GCL-CLapl, 81% for GCL-DLapl and 82% for TCL-MLE. Once again, GCL-CLapl outperforms the 2 others methods. We observe also that the performance of CL methods has increased when the number of vehicles grows larger, i.e 20 vehicles. The explanation is that as the number of vehicles increases, so does the number of neighbors of each vehicle. Thus, vehicles integrate greater amount of information during the location estimation process, either with (10) or our method. We notice also that the distributed scheme GCL-DLapl outperforms TCL-MLE in Fig. 5-(a), while in Fig. 5-(b) have similar performance.
We are planning in the future, to apply our schemes to datasets extracted by CARLA autonomous driving simulator [16] but also to investigate the benefits of integrating the detected road landmarks to our location estimation approach.

C. Complexity
To show the computational complexity of the derived algorithms we demonstrate below in TABLE I the average execution time of 500 time instances of the 3 methods, for 10, 20 and 30 vehicles. It is obvious that GCL-CLapl and GCL-DLapl are much more faster than TCL-MLE. More specifically, GCL-CLapl and GCL-DLapl are 290 and 63, 453 and 63 and 338 and 66 times faster than TCL-MLE, for 10, 20 and 30 vehicles, respectively. As a result, in cases where GCL-DLapl and TCL-MLE have similar performance (Fig. 4-(a), Fig. 5-(b)), GCL-DLapl has the advantage of the lower complexity and execution time. Also, we observe that as the number of vehicles increases, so does the execution time, which is reasonable.
Parentheses into the cells of TABLE I refer to how many times faster than TCL-MLE the 2 proposed schemes are. V. CONCLUSION In this paper, we modeled the vehicles of a VANET as vertices of an undirected graph and the communication links between them as the edges of the graph. Each vehicle is able to know its absolute position and to measure its relative distance and angle or azimuth angle to its neighboring and connected vehicles. Thus, by creating the overall or local Laplacian matrix and defining the δ coordinates, Sparse Laplacian Localization can significantly improve vehicles' GPS accuracy. Finally, we have proven that the centralized or local ('distributed') scheme of our method, outperforms the method of [2], both in terms of accuracy and execution time. Regarding Absolute Localization error can reach 87% and 69% with our method (instead of 82% and 60% of [2]), while the gain in execution time is 3 (GCL-CLapl) and 2 (GCL-DLapl) orders of magnitude.