5G Enabled Cooperative Localization of Connected and Semi-Autonomous Vehicles via Sparse Laplacian Processing

Cooperative Localization has received extensive interest from several scientific communities including Robotics, Optimization, Signal Processing and Wireless Communications. It is expected to become a major aspect for a number of crucial applications in the field of Connected and (Semi-) Autonomous vehicles (CAVs), such as collision avoidance/warning, cooperative adaptive cruise control, safely navigation, etc. 5G mobile networks will be the key to providing connectivity for vehicle to everything (V2X) communications, allowing CAVs to share with other entities of the network the data they collect and measure. Typical measurement models usually deployed for this problem, are absolute position information from Global Positioning System (GPS), relative distance to neighbouring vehicles and relative angle or azimuth angle, from Light Detection and Ranging (LIDAR) or Radio Detection and Ranging (RADAR) sensors. In this paper, we provide a cooperative estimation approach that performs multi modal-fusion between interconnected vehicles. This method is based on a Graph Signal Processing tool, known as Laplacian Graph Processing, and significantly outperforms existing method both in terms of attained accuracy and computational complexity.


INTRODUCTION
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 and deviations up to 10m or higher may arise, especially in harsh environments such as urban canyons. Since the localization error in autonomous driving should be no greater than 5m in the worst case, accurate localization methods 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. Useful information could be absolute position from GPS, relative distance or angle (shown in Fig. 2) to neighbouring vehicles from LIDAR/RADAR. Thus, CL relies on 5G for the fusion of 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). 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. Finally, in [1], CL approaches in Wireless Sensor Networks are categorized as Non-Bayesian, where the relative or self-measurements depend only on the locations of nodes or location of self-node involved, and Bayesian, where the location of node depends only on the measurements of that node. The former relies on Maximum Likelihood Estimation (MLE) and the latter on Maximum A Posteriori (MAP) Estimation.
The previously discussed methods focus only on the pair-wise 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 considers the connectivity properties of the underlying graph formed by the involved vehicles. Moreover, we integrate these 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 neighbouring vehicles. • The new method is implemented in a centralized fashion, assuming the existence of a fusion centre.
• 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 orders of magnitude. Note that the method chosen for comparison is the one in [2] because it exploits the same type of measurements. The rest of the paper is organized as follows: section 2 provides the background of traditional CL in VANETs; section 3 presents the proposed method, while section 4 is dedicated to the experimental setup and simulation results and section 5 concludes our work.

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 -th vehicle at -th time instant is given by = . Each vehicle knows its absolute position from GPS and measures its relative distances and angles with respect to neighbouring vehicles using LIDAR or RADAR. The true relative distance between connected vehicles and is given by = , where is the l 2 norm. The true angle (shown in Fig. 2) between neighbouring vehicles and 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 is a diagonal matrix equal to diag ( ). A typical approach in CL is to formulate an objective cost function (according to MLE [1], [2]) and to minimize it with respect to locations in order to reduce the error of absolute position measurement. The likelihood function of the measurement models can be written as: (4) where denotes the set of neighbours of the -th vehicle and are the probability density functions of the measurement models. If we take the logarithm of Eq. (4), then the objective cost function (same as in [2] and similar to that of [1]) is given by: For the minimization of Eq. (5), interior point methods (e.g., those provided by CVX software) can be applied. The noise in range measurements is assumed to be Gaussian (as in [3], [4]) under the hypothesis of Line-of-Sight (LOS) between the vehicles (Fig. 1). However, in a highly complex environment, it is probable that between two vehicles, an occluding object (e.g. building, vehicle etc.) also exists (Fig. 3), and therefore, LIDAR/RADAR cannot provide an accurate range estimation. This effect is known as Non-LOS (NLOS) and it is a serious challenge of autonomous driving. The work in [7], tackles with detecting hidden objects (e.g. hidden around corners), using LIDAR confocal scanning. Under these assumptions, the noise in range measurements can be modelled as: w , where a heavy-tailed Gaussian Distribution and the probability that the current measurement is NLOS affected.

SPARSE LAPLACIAN BASED LOCALIZATION
The method that will be derived in this section is based on a proper extension of the Laplacian Processing technique [6]. 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 representation of involved vehicles.
Let be an undirected graph, where is the set of vertices and the set of edges. Each vertex is represented by absolute cartesian coordinates as . The differential coordinates for each vertex are defined as , where is the number of neighbors of vertex . We also define the diagonal degree matrix [i,i] = ) and adjacency matrix , with A[i,j] = 1, if and A[i,j] = 0, otherwise. Finally, the symmetric Laplacian matrix of graph is equal to . Relying on the previously defined coordinates, one can recover the true absolute coordinates of the vertices, represented by the vectors and , by solving the systems: and ( 6 ) Each vehicle, using LIDAR or RADAR, can also exploit, the azimuth angle (shown in Fig. 2) measurement between vehicle observer and vehicle target : , For example, in Fig. 2, . We can also notice from Fig. 2, that: By considering the vehicles of the network as vertices of a graph and the communication link between the neighbours as its edges, we can derive the associated matrices , , . Moreover, each vehicle utilizing measurement models of Eq. (1) and Eq. (7), can send its measurements to a fusion centre, which can compute the coordinates. However, L is a singular matrix, which implies that systems in Eq. (6) are not solvable. Thus, we need to add into the systems some anchor points with known absolute coordinates. Furthermore, if we assume as anchors e.g. the vertices and , then the extended is equal to = , where is a vector with 0's and . After adding the anchor points, the systems in Eq. (6) can be re-written as: and (9) The required location vectors and can be computed in the Least Squares sense as follows: and ( 1 0 ) In practice, as anchor points the GPS positions of the vehicles of the network (Eq. (3)) can be used. Furthermore, each must be multiplied with , so as to remain in accordance with Eq. (6). Moreover, due to the limited communication range of vehicles' transceivers, matrix is actually sparse. Thus, one may use a sparse leastsquares solver, in order to solve systems of Eq. (9).
To summarize, in Step 1 we create matrices D, A, L, in Step 2 we define coordinates of vehicles using Eq. (8) and in Step 3, we solve the systems in Eq. (10).

Experimental Setup
Extensive experiments were conducted for different levels of noise of measurement models. The initial locations of vehicles were determined assuming uniform distribution for and coordinates. We generated the trajectories of vehicles for 500-time instances according to the vehicle motion model of [5]. Active communication links were assumed to exist between the vehicles, only if their distance was lower than 20m, and the number of connected neighbours was at most 6. Based on these assumptions we considered two different approaches: i) the Traditional CL (of [2] and Eq. (5)) based on MLE (TCL-MLE), and ii) the proposed centralized scheme as Graph-based CL (GCL-CLapl). We applied the two approaches at each time instant. Equation (5) was minimized using the CVX software. Each time instant, we set = 3m, = 2.5m in order to have an average GPS error equal to 3.4m and we calculated the Mean Square Localization Error (MSLE) of GPS and the 2 aforementioned methods. In Fig. 4

Evaluation study
The performance of the proposed method, as well as that of the method in [2], depends on the error introduced in range measurements. The variance of the range measurements error is usually much smaller than the variance of (a) = 0 Figure 4. CDFs of MSLE 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 using both methods. In Fig. 4(a) the reduction of error is 88% for GCL-CLapl and 78% for TCL-MLE. In Fig. 4(b) the reduction of error is 83% for GCL-CLapl and 65% for TCL-MLE. In Fig. 4(c) the reduction of error is 79% for GCL-CLapl and 52% for TCL-MLE. In Fig. 4(d) the reduction of error is 78% for GCL-CLapl and 48% for TCL-MLE. It is obvious that the performance of the two CL methods is degraded when increases and the NLOS effect is stronger. However, GCL-CLapl outperforms TCL-MLE in all 4 cases and proves 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 the VANET graph. Thus, the impact of noise in range measurements could be further reduced, even in the presence of the NLOS effect.
he average execution time is independent of . It is 0.0015 sec with GCL-CLapl and 0.4357 sec with TCL-MLE. More specifically, our method is 290 times faster than TCL-MLE. Therefore, GCL-CLapl is 2 orders of magnitude faster than TCL-MLE.

CONCLUSION
In this paper, we modelled 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 knows its absolute position and measures the relative distance and angle or azimuth angle to its neighbouring and connected vehicles. Thus, by creating the Laplacian matrix and defining the coordinates, Sparse Laplacian Localization can significantly improve GPS accuracy. Finally, we have proven that our centralized scheme, outperforms the method of [2], both in terms of accuracy and execution time. Regarding the accuracy, the reduction of MSLE can reach 78%-88% with our method (instead of 48%-78% of [2]) under variable NLOS conditions, while the gain in execution time is 2 orders of magnitude.