Graph Laplacian Extended Kalman Filter for Connected and Automated Vehicles Localization

Extended Kalman Filters have been widely applied for tracking the location of moving semi-autonomous vehicles. The latter are equipped with a multitude of sensors generating multi-modal data, while at the same time they are capable of cooperating via Vehicle-to-Vehicle communication technologies. In this paper, we have formulated a cooperative tracking scheme based on Extended Kalman Filter, in order to cope with erroneous GPS location information. It performs multi-modal fusion in a centralized and distributed manner, assuming the existence of an overall fusion center or local interaction among neighbouring and connected vehicles only. It features the property of encoding in a linear form the different measurement modalities, including range and GPS measurements, exploiting the connectivity topology of cooperating vehicles, using the graph Laplacian operator. The extended experimental evaluation using realistic vehicle trajectories extracted by CARLA autonomous driving simulator, verify the significant reduction of GPS error under various realistic conditions. Moreover, both schemes outperform existing cooperative localization methods. Finally, the distributed tracking approach exhibits similar performance and in specific cases outperforms the centralized counterpart.


I. INTRODUCTION
Connected and Automated Vehicles (CAVs) are considered a viable option to face the operating challenges of Intelligent Transportation Systems [1]. CAVs benefit from integrated sensors like Camera or LIDAR, for increased awareness and scene analysis ability. At the same time, the potential of Vehicle-to-Vehicle (V2V) or Vehicle-to-Infrastructure (V2I) wireless communications, enable the cooperation among the members of a Vehicular Ad-Hoc Network (VANET). To this end, CAVs can achieve their goals by the collaborating fusion of their measurements. Such a major task is Localization [2], i.e. exact knowledge of self and others location. Localization improves traffic safety, road congestion and efficient planning manoeuvring. Consider the case of estimating the right velocity in order to keep safety distances among the vehicles. This spacing scheme requires the accurate knowledge of locations. Although GPS sensor is widely employed to provide absolute position information, it exhibits high localization error (even greater than 10m), especially in dense urban conditions [5]. This paper has received funding from the European Union's H2020 research and innovation programme CPSoSaware under grant agreement No 871738. Therefore, efficient cooperative localization (CL) methods for CAVs, based on collaborating multi-modal fusion of intervehicular measurements, have received increasing interest during the last years.
There are several works that study the benefits of CL. The authors in [3] provide an overview of CL in Wireless Sensor Networks. They categorize CL methods into: One-shot vs Tracking, Bayesian vs non-Bayesian and Centralized vs Distributed. Distributed and Bayesian/tracking are in general more attractive, since the processing and computations are assigned to each individual vehicle, which in addition exploits different motion patterns. In [4], a distributed CL method, where each vehicle fuses absolute position from GPS, motion sensor's readings and range measurements utilizing Extended Kalman Filter (EKF), is proposed. It mainly focuses on tunnels, when GPS may not operating due to signal blockage. In [5], a set of detected non-cooperative features, are used as common noisy reference points. Each vehicle performs a Bayesian Gaussian Message Passing algorithm, improving stand-alone GPS accuracy in different urban conditions. In [6], authors build upon and extend their work of [5], by introducing a distributed feature-vehicle data association procedure. The work of [7], presents a robust cubature Kalman Filter (KF) enhanced by Huber M-estimation, in order to improve the performance of the data fusion under the presence of measurement outliers, though not considering the significant role which plays the VANET's size in location estimation. In [8], a Bayesian method is proposed, based on distributed generalized message passing and KF, which utilizes measurements from Inertial Measurement Unit (IMU), GPS, Signals of Opportunity and range measurements using V2V and V2I. A distributed non-Bayesian method is presented in [12], which fuses absolute positions, relative distances and angles of only four vehicles, using the maximum likelihood estimator (MLE). The latter is attractive due to its consistency and asymptotic optimality and normality properties.
While the aforementioned CL schemes offer significant benefits, they are mainly evaluated using trajectories generated by simplified kinematic models and more importantly they focus only on the pair-wise measurements, without considering the topology of a large number of CAVs that form undirected graphs with varying topologies. Although in our previous study in non-Bayesian Centralized and Distributed Laplacian Localization (CLL and DLL) [9], we proved the significance of exploiting this type of connectivity using the graph Laplacian operator, the tracking properties were neglected. Therefore, in this work we focus on proposing novel cooperative tracking schemes which apart from measurements, take also into account the underlying graph of involved vehicles, by efficiently utilizing the graph Laplacian operator. More specifically, the main contributions of this work are summarized as follows: • We formulate two novel graph Laplacian EKF-based cooperative schemes for CAV localization and tracking. • Range measurements between the vehicles, derived either from LIDAR data or visual features, are encoded together with the actual positions through the extended graph Laplacian operator. • The proposed method is performed in a centralized manner. A distributed low cost variant is also proposed that takes into account only the noisy positions of a local neighborhood. • We generated realistic random traffic trajectories using CARLA autonomous driving simulator [10]. Extensive simulation studies verify that: i) centralized and distributed tracking exhibit almost the same location estimation performance, ii) the reduction of GPS Localization Mean Square Error (LMSE) can reach 86% and 84%, respectively. Both schemes significantly outperform the method of [12], named Maximum Likelihood based Localization (MLL). The latter is chosen since it exploits exactly the same multimodal data as we do, while it utilizes the, prominent in CL literature [3], MLE for fusion.

II. CENTRALIZED AND DISTRIBUTED LOCALIZATION
This Section provides some preliminaries about our previous work on graph based CLL and DLL for CAV localization, for self completeness. Both methods were utilized to develop the proposed tracking methods.

A. Centralized Laplacian Localization
T ∈ R 2 , whereas the distance and azimuth angle between connected vehicles i and j are equal to z . Integrated sensors like GPS and LIDAR could provide those measurements, assuming also additive white Gaussian measurement noise [3], [5]. Hence, we define the following three measurement models for each vehicle, with G(µ, Σ) as the Gaussian distribution and µ, Σ its mean and covariance: • Absolute position measurement: We acquire the Laplacian matrix of V2V cluster graph are the degree and the adjacency matrices of cluster graph. The differential coordinates per vehicle δ is the set of neighbouring and connected vehicles to i-th, containing i, and with cardinality |N is singular and non-invertible [13], we create the extended Laplacian vector of x-differentials. The second part of b (t,x) contains the actual GPS positions of vehicles, known as anchor points [13]. As such, the x-positions of vehicles of cluster, in the form of vector x (t) ∈ R |C (t) i | , follow the linear system of: which can be solved in the least-squares sense. The same approach is followed for estimating y-positions

B. Distributed Laplacian Localization
The distributed scheme of DLL exploits the local Laplacian matrix for each neighborhood. Consider the cluster of  (shown in Fig. 1-(b)) implies that yellow and green are not connected, though they are. As such, the local Laplacian matrixL , consisting of neighborhood's GPS measurements (transmitted to i = 1) and the x-differential of i = 1. The x-location estimation of i = 1 is reduced to solving the following linear system, as in (1): 1 | consists of relevant xlocations estimated by 1-th vehicle, while latter's location corresponds to the last element of vector. The same approach is followed for y-location vector y 1 | and the rest of vehicles.

III. CENTRALIZED AND DISTRIBUTED TRACKING
In this Section, the proposed Centralized and Cooperative EKF (CCEKF) and Distributed and Cooperative EKF (DCEKF) for CAV localization will be derived. Both schemes exploit the spatiotemporal properties of vehicles in the context of EKF and perform cooperative multi-modal fusion. The CCEKF approach is implemented in a centralized manner and at the level of individual clusters of CAVs. The distributed or local tracking approach of DCEKF, utilizes the star topology and measurements of each neighborhood, since each vehicle relies only on its own direct neighborhood to estimate its location. The main novelty relies on the fact that the measurement model of both approaches utilizes and fuses GPS positions, differential coordinates and the connectivity representation of vehicles via the linear graph Laplacian operator.

A. Centralized Tracking
Obviously, CLL is a one-shot spatial scheme, without considering any past estimations or kinematic model for vehicles. This a serious limitation towards CAVs localization. The prominent tracking approach of EKF can facilitate the design of an efficient localization system. Moreover, (1) encodes in a linear form the connectivity properties and measurement modalities of cluster through differential coordinates. That useful property is integrated to the proposed method of CCEKF. The latter is in fact a centralized scheme, since vehicles of cluster are required to transmit their measurements (differential coordinates and GPS) and control inputs to a fusion center, i.e 5G cloud, which in turn will estimate and inform them about their locations using e.g. Time-Division Mutliple Access (TDMA) communication protocol. According to that, vehicles broadcast and receive the necessary information in distinct time slots.
Clusters (example shown in Fig. 4-(b)) are created by imposing a fixed communication range r c (much lower than typical 200m of V2V) and maximum number of neighbors N max for each vehicle, as in [5]. Thus, a reduced computational load with permissible localization accuracy can be achieved, avoiding also extreme cases like two vehicles of the same cluster are far (even kilometers) away from each other. The EKF algorithm [11] implemented at the level of individual clusters, exploits a state transition and measurement model to track the desired state x  vectors: The state transition and measurement functions g 1 (·) and h 1 (·) are in general non-linear, while i | are the diagonal covariance matrices of state and measurement noise. The IMU sensor provides control inputs, whilst the measurement vector refers to intercluster measurements (e.g. relative distances). The bicycle kinematic model of [11], is a popular choice to simulate the state transition function, assuming that the 3D state of i-th vehicle consists of 2D location and heading angle θ where ∆T is the time step and s i | will consist of 2D location and heading angle of vehicles of the corresponding cluster: i | will consist of linear and angular velocities of cluster: The main novelty is related to treating the measurement model of EKF according to CLL. As such, the measurement vector z i | will consist of differential coordinates, GPS measurements and heading angles measurements by IMU of cluster's vehicles: whereθ (t) = θ (t) + n θ the heading angles measurement vector of cluster, with n θ ∼ G(0, σ 2 θ ), σ θ = σ ω . Apparently, function h 1 (·) will no longer be non-linear, but the block i | consisting of extended Laplacian matrix of V2V cluster graph and the identity matrix, multiplied by the state: i.e. avoiding the linearization of distance and angle model performed by the standard EKF, which may introduces high localization error. Moreover, in realistic urban traffic scenarios, vehicles may enter or exit individual cluster. Therefore, the topology of the latter is probable to change as the time evolves.
In that case, EKF should be initialized with CLL current solution, since the state, control inputs and measurement model are modified. Furthermore, a vehicle may not belong to any cluster at all for a time period, limiting its own localization ability to noisy GPS. Hence, we exploit an extra EKF, utilizing only GPS and IMU measurements, operating for vehicles which they do not participate in any cluster. The 3D state of those vehicles consist of 2D location and heading angle, the 2D control vector is constituted by linear and angular velocity, while the 3D measurement vector consist of GPS measurements and IMU heading measurement. At the linear measurement model, the identity matrix I 3 is multiplied by the state, as in (8). The main steps of the proposed CCEKF are summarized on Algorithm 1, based on EKF algorithm of [11]. Location vector and its covariance matrix Σ C (t) i are predicted and estimated in Lines 6-7 and 9-10. EKF approximates the non-linear transition function g 1 , with its Jacobian matrix G C (t) To sum up, we have derived a centralized and cooperative tracking approach, which encodes in a linear form the measurement modalities and connectivity properties of CAVs. The proposed CCEKF is executed in a centralized manner on the level of clusters of CAVs. A schematic diagram representing CCEKF approach is provided in Fig. 2.

B. Distributed Tracking
A distributed localization architecture, in which the processing is assigned to individual vehicles, offers benefits related to limited communication and computing requirements. There is no need to define an overall central node, which may Vehicles transmit their ids, differential coordinates, GPS measurements and control inputs to cloud; Intialize locations with CLL [9]; Cloud informs vehicles about their locations x Perform single EKF with GPS and IMU; 16 end be vulnerable to malfunctioning and cyber-attacks or even cost-ineffective. Thus, increased robustness to sensing and communication failures can be achieved. The EKF employed for the distributed (or local) tracking relies on the same state transition and measurement models, as in (3) and (4). Again, we consider the non-linear transition function g 2 (·), known as the bicycle kinematic model, which was described in the previous Section but now applied for the vehicles of N (t) i . As such, the state vector x i | will now consist of 2D locations and heading angles of vehicles belonging to the neighborhood of i: At the same time, control input vector u i | will consist of linear and angular velocities of neighborhood: The key feature of proposed DCEKF relies on the fact of integrating into the measurement model of EKF, the linear system of (2): where the measurement vector z i |+2 will be compromised of GPS and heading measurements transmitted to i and differential coordinate of i. Apparently, linear function h 2 (·), which couples together the star topology and measurement modalities of neighborhood, will be equal to:  Therefore, we derived the distributed (or local) counterpart DCEKF (depicted in Fig. 3) based on our previous work on DLL. The distributed tracking scheme is applied on the level of individual neighborhoods, where each vehicle receives from neighbors the necessary (measurement and control) information, without any central processing involvement. Each vehicle acts now as the fusion center, utilizing the neighborhood's star topology too. We take into account the cases of varying neighborhood topologies just as the same way as in CCEKF.

IV. SIMULATIONS
In this Section, we validate the introduced approaches, by performing computer based simulations using Python and CARLA simulator.

A. Experimental Setup
We extracted the trajectories of N = 200 vehicles from CARLA simulator, along with the related control inputs during their movement. We set σ x = 3m, σ y = 2.5m. Covariance matrices are initialized to identity matrix. Ground truth for a number of vehicles, and clusters formed at a certain time instant, are depicted in Fig. 4. The simulation horizon was set (a) Ground truth (b) Formed clusters Fig. 4. Trajectories and generated clusters on a specific frame generated from CARLA to T = 500, while ∆T = 0.3s. Experiments were based on two main parameters: size of cluster and range measurements noise. The former dictates the connectivity representation, while the latter the feasible estimation of differential coordinates, even in occluded and highly complex environment. Finally, we measured LMSE, Localization Error (LE) and Average LE (ALE) at each time instant and constructed the Cumulative Distribution Function (CDF).
B. Evaluation Study 1) Benefit of cooperation: The effect of participation in clusters is presented in Fig. 5-(c). We set r c = 20m, N max = 6, σ d = 1m and σ az = 4 • . Vehicle 9 is always a member of clusters, while vehicles 191 and 24 only for 72% and 46% of simulation horizon. Apparently, centralized and distributed tracking exhibit similar performances. The reduction of GPS LE of vehicle 9 with DCEKF is 72%, much greater than 57% and 46% of 191 and 24. Superior location estimation accuracy of vehicle 9 is clearly due to constantly being a member of clusters. In Fig. 5-(f), we measured ALE over 1000 iterations of GPS, CLL and CCEKF for vehicle 193 and plotted it for the first 100 time instances. Clearly, CCEKF tracking error is much lower than CLL, while its peaks coincide with CLL, an effect attributed to cluster initialization using the one-shot CL approach. Moreover, tracking error is slightly increased when the vehicle is not a member of clusters, since during those time instances, the traditional EKF is employed using only individual GPS measurements. 2) Impact of cluster size: The effect of cluster size in location accuracy is depicted in Fig. 5-(a),(b), with σ d = 1m, σ az = 4 • . Evidently, tracking schemes outperform all other methods. For example, in Fig. 5-(a) the reduction of GPS LMSE is 78%, 76%, 64%, 60% and 50% with CCEKF, DCEKF, CLL, DLL and MLL, respectively. In Fig. 5-(b), with increased cluster's size (r c = 30m and N max = 8), the methods exhibit superior performance over Fig. 5-(a), since vehicles integrate larger amount of information. CCEKF and DCEKF achieved 86% and 84% reduction of GPS LMSE, i.e. significantly greater than in Fig. 5-(a). Thus, increased cluster's size leads to increased location estimation accuracy. Note that in all cases the CDF of distributed DCEKF almost coincide with its centralized counterpart.
3) Impact of range noise: The impact of uncertainty in range measurements is depicted in Fig. 5-(d),(e), with fixed r c = 20m and N max = 6. For example, in Fig. 5-(d) the reduction of GPS LMSE is 80%, 78%, 67%, 63% and 54% for the CCEKF, DCEKF, CLL, DLL and MLL schemes respectively. Once again, tracking schemes are much for efficient. In Fig. 5-(e), with σ d = 7m, σ az = 10 • , performances have been seriously deteriorated, though tracking proves its robustness. For example, CCKEF and DCEKF achieved 43% and 47% reduction of GPS LMSE, while MLL actually increases it. Moreover, the proposed distributed scheme is more effective than CCEKF, since vehicles utilize only their own differential coordinate, and thus limiting the impact of increased range measurements noise.

V. CONCLUSION
In this paper, we formulated a centralized and distributed cooperative scheme for CAV localization and tracking, based on a novel graph Laplacian EKF algorithm. Extensive results using realistic measurements have proven the proposed schemes efficacy and efficiency under different experimental conditions generated in the CARLA autonomous driving simulator. It is worth mentioning that the distributed tracking outperforms the centralized tracking approach in the presence of range measurement uncertainties.