Multi-modal cooperative awareness of connected and automated vehicles in smart cities

—Cooperative autonomous driving in 5G and smart cities environment is expected to further improve safety, security and efﬁciency of transportation systems. To this end, involved vehicles is imperative to have accurate knowledge of both their own and neighboring vehicles’ location, a task known as cooperative awareness. In this paper, we have formulated two novel distributed localization and tracking schemes, based on Gradient Descent and Extended Kalman Filter algorithms, to cope with erroneous GPS location. Sensor-rich vehicles exploit Vehicle-to-Vehicle communications and a multitude of integrated sensors, like LIDAR and Cameras, to generate and fuse heterogeneous data. Each vehicle interacts only with its own connected neighboring vehicles, formulating individual star topologies. Extensive simulation studies using CARLA autonomous driving simulator, verify the signiﬁcant reduction of GPS error achieved by the two methods in various experimental conditions. Distributed tracking proves to be much superior than Gradient descent algorithm, both in the case of self (58% reduction of GPS error) and neighboring vehicles location estimation (38% reduction of average GPS error).


I. INTRODUCTION
Connected and Automated Vehicles (CAVs) have the potential to enhance road safety and the overall performance of transportation sector, through the strict control of their positions and motion planning actions [1]. Furthermore, they are able to achieve increased scene analysis ability through the exchange of information by using wireless Vehicle-to-Vehicle or Infrastructure (V2V and V2I) communication technologies. For a better perception of surrounding environment, the members of a 5G Vehicular Ad Hoc Network (VANET) is required to have exact and timely knowledge, apart from their own position, of neighboring vehicles' positions, too. This task is known as cooperative awareness and is critical enough for efficient path and motion planning of CAVs. Usually, Localization module through the GPS sensor is responsible for providing absolute position information. However, GPS is less reliable in challenging urban environments [2]. To obtain highly accurate positioning solutions, vehicles exploit V2V and integrated sensors like LIDAR, Cameras, IMU, etc., to generate, exchange and fuse heterogeneous measurements. This paper has received funding from the European Union's H2020 research and innovation programme CPSoSAware under grant agreement No 871738.
This promising technique is known as Cooperative Localization (CL) and it has received increasing interest during the past few years.
Distributed and Bayesian/tracking CL methods are usually more attractive [3], since the processing and computations are assigned to each individual vehicle (instead of an overall fusion center) which in addition exploits different motion models and patterns for location estimation. Maximum likelihood estimator (MLE) [4] is also a popular choice for the multi-modal fusion, due to its consistency and asymptotic optimality and normality properties. Based on that estimator, a distributed cooperative Gradient Descent (GD) algorithm has been developed in [4], addressing only inter-nodes distance measurements. 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. Kalman and Extended Kalman Filter (KF and EKF) are also popular tracking schemes [6], [7]. For example, a distributed method has been developed in [7], formulating a Bayesian approach which reduces the location estimation uncertainty. A KF is employed, fusing absolute self and neighbors' GPS positions, motion sensors and V2V range measurements. An approach similar to ours, is described in [8]. Authors developed a distributed Bayesian CL method for localizing vehicles in the presence of Non-Line-of-Sight range measurements and malicious (caused by GPS cyber-attack) vehicles. However, they focused primarily on ego vehicle location estimation and abnormal vehicles detection rates, rather than evaluating how accurate they estimate neighbors' location. The notion of centralized and distributed Laplacian localization, utilizing the graph Laplacian operator of the VANET, was introduced in [9], [10]. Finally, since 5G and V2V is crucial for (distributed) CL, [11] discusses these communication aspects and provides a framework for realising cooperative perception.
The aforementioned distributed CL methods were primarily evaluated for the case of ego vehicle location estimation, without considering the cooperative awareness side of localization. Furthermore, only a small number of involved CAVs was taken into account. Therefore, this work focus on proposing and formulating novel cooperative awareness distributed algorithms, considering a large number of CAVs with varying connectivity topologies. In summary, the main contributions of this work are outlined as follows: • Proposed distributed mutlimodal localization and tracking methods are performed in the context of star topologies of involved CAVs, in which each ego vehicle acts as its center. • Ego vehicle estimates both its own and connected neighbors' location. • We formulate a distributed mutlimodal cooperative GD algorithm based on MLE, which takes into account three different pair-wise measurement models. • We propose a novel distributed tracking EKF-based scheme, which exploits the inter-vehicular measurements of individual star topologies. • We generate realistic random traffic trajectories using CARLA autonomous driving simulator [12]. The rest of this paper is organized as follows: Section II describes the system model and provides the cooperative GD approach for CL; Section III formulates the distributed tracking scheme for cooperative awareness task; Section IV presents numerical results and evaluation, while Section V concludes the paper.

II. COOPERATIVE LOCALIZATION VIA MAXIMUM LIKELIHOOD ESTIMATION
In this Section, we formulate the system model for the problem at hand. Afterwards, the proposed cooperative GD algorithm for distributed CL of CAVs will be derived.

A. System Model
Consider the VANET of Fig. 1, with N collaborating CAVs. Connection links imply V2V communication. Usually, a fixed communication range r c (much lower than typical 200m of V2V) is employed as in [5], for reduced computational load. Vehicles exploit also a V2X communication protocol allowing them to broadcast and receive the necessary information in distinct time slots. The absolute position of i-th vehicle at time instant t is given by the vector z (t) T ∈ R 2 , while distance and angle between connected vehicles i and j are equal to: z Usually, integrated sensors like GPS, LIDAR and Cameras can provide those multi-modal measurements. Thus, the following measurement models are defined for each vehicle, assuming also additive white Gaussian noise [3], with G(µ, Σ) as the Gaussian distribution and µ, Σ its mean and covariance: • Absolute position measurement (from GPS): Σ p is a diagonal matrix equal to diag(σ 2 x , σ 2 y ). • Distance measurement (from LIDAR and Cameras): • Angle measurement (from LIDAR and Cameras): Instead of treating the VANET as a whole, it is more convenient to design distributed algorithms at the level of individual vehicles and their direct neighbors, avoiding the deployment of a fusion center. Distributed approaches are highly beneficial and cost effective since they: a) require only local processing and information exchange between neighbors, b) yield robustness against central node failures (malfunctioning or cyber-attacks). By inspecting Fig. 1 it can be easily realized that the yellow (ego) vehicle requires to estimate both its position along with that of its connected neighbors, forming a star like topology, that corresponds to a sub-graph of the overall network. Therefore, cooperative awareness in the context of CL could be seen through individual star topologies, with the corresponding ego vehicle acting as the local center of each star.

B. Cooperative Gradient Descent algorithm
The set N i | , describing the x and y-positions of each own star topology, with P according to MLE criterion, and to minimize it with respect to locations in order to reduce GPS error. For each vehicle (note that either k or l must be the ego vehicle i), the following Probability Density Functions (PDFs) of measurement models are derived: a ) The likelihood function of the measurement models can now be written as the product of all PDFs. Finally, if we take the negative logarithm of the likelihood function, then the objective cost function (same in [13]) is given by: The second term of angle measurement model has to be reformulated in order to facilitate the feasible minimization of (1) using GD algorithm (as we discuss below): . Note that ego vehicle must have access to the GPS position of neighbors and also to the related range measurements. Consider for example the vehicle pair (i, j) ∈ N (t) i . Both vehicles estimate (z d,ij ,z a,ij ) and (z d,ji ,z a,ji ). Therefore, j must transmit to i the range measurements pair (z d,ji ,z a,ji ), so as to the latter effectively minimizes the cost function. Since j estimates actually (|N (t) j | − 1)(z d,ji ,z a,ji ) pairs from LIDAR, a data association step is required from i in order to "discover" the right measurements. Transmitted GPS positions may also facilitate the association process. In this paper, we assume that optimal data association is given to us as a preprocessing step.
The objective cost function can be minimized by the corresponding ego vehicle employing a distributed Alternating Direction Method of Multipliers [13], cooperative GD [4] or the interior point methods of CVX software. We choose the GD due to its simplicity. The already established cooperative GD minimizes a cost function comprising of only the relative distance measurements. We extend it in order to account also for angle and absolute position model. In each iteration of this algorithm (K max the maximum number of iterations), an update step ∆ is calculated to obtain the next estimate: P being the optimal. The GD uses the negative gradient of C (·) to identify the direction of steepest descent: Step size µ > 0 needs to be carefully chosen to balance between safe updates and reasonable convergence speed. The main steps of the proposed cooperative awareness method of Maximum Likelihood based Localization using GD or MLL-GD, are summarized on Algorithm 1. The update step for estimating y-position has been omitted, since it can be derived with the same way as for x-position.
Therefore, we have formulated a distributed localization algorithm implemented at the level of individual vehicles, which are trying to estimate both their position and that of their connected neighbors. Each vehicle is required also to transmit the GPS position and the related range measurements. Either k or l must be the ego vehicle i ;

III. EXTENDED KALMAN FILTER FOR INCREASED COOPERATIVE AWARENESS
In this Section, the proposed approach of EKF for Cooperative Awareness or EKF-CA will be derived. It is in fact a distributed localization and tracking scheme, since each vehicle performs spatiotemporal multi-modal fusion relying only to its connected neighbors. The main novelty is related to the simultaneous estimation of ego and neighbors location at the ego vehicle, exploiting the tracking properties of the EKF, as well as the communication capabilities between the vehicles participating in VANET with star topologies.
The method of MLL-GD ignores the motion model for the vehicles, while at the same time requires a large number of iterations before converging to the optimal solution. These limitations, motivated us to design a more efficient distributed localization and tracking system, exploiting the tracking prop-erties of the well-known EKF within the cooperative awareness concept. Our approach was inspired by the functionalities executed at the traditional multi-modal Simultaneous Localization and Mapping (SLAM) back-ends [14], [15], where the ego vehicle tries to estimate its own position and the position of neighboring landmarks utilizing pair-wise range measurements.
The EKF algorithm [14], utilized by each vehicle in order to estimate both its own position and the position of its neighbors, exploits a non-linear state transition and measurement model to track the desired statex The state transition and measurement functions f (·) and h(·) are in general non-linear, while i |−4) are the diagonal covariance matrices of state and measurement noise. The IMU sensor provides control inputs. In the proposed approaches we deploy the bicycle kinematic model of [14] (known also as Constant Turn Rate and Velocity model), which is a widely adopted model that simulates the state transition function, assuming that the 3D state of i-th vehicle consists of a 2D location and the yaw angle θ (t) i : where ∆T is the sampling interval and v are the linear velocity and yaw rate, respectively. The last two quantities constitute the control input vector. Note that in realistic conditions, zero mean white Gaussian noise is added to individual IMU measurements [6]. As a matter of fact, the statex i | will consist of 2D location and yaw of vehicles of the corresponding star topology:x Moreover, the control input vector u i | is formed by the linear velocities and yaw rates: u contains all the inter-vehicular measurements and the yaw angles of the vehicles participating in the star topology VANET: z i + n θ is the yaw measurement vector of topology, with n θ ∼ G(0, σ 2 θ ). Consider for example that vehicles i, j, k constitute the i-th star topology. Then, (y,t) p,k T . Therefore, neighboring vehicles j and k have to transmit to i, apart from GPS position, their range measurements towards i. Furthermore, they have to broadcast their control inputs, i.e. linear velocity, yaw rate and yaw, since it is required by the state transition and measurement model of i. However, in urban traffic conditions the members of individual star topologies is probable to change as time evolves. In that case, EKF should be reset and re-initialized with MLL-GD solution, since the state vector is no longer the same as before. The main steps of the proposed EKF-CA are summarized on Algorithm 2, based on EKF algorithm [14]. It is important to notice that due to the non-linearity of (2) and (3), EKF approximates the two functions via Taylor expansion, using the corresponding Jacobian matrices: Location vector and its covariance matrixΣ (t) i are predicted and estimated in Lines 8-9 and 11-12.
Vehicles transmit their 2D GPS position, the corresponding 2D range measurements to the "right" neighbors, linear velocity, yaw rate, and yaw angle;  Initialize locations with MLL-GD; To sum up, we have derived a distributed tracking scheme utilized by each vehicle. The latter upon receiving the necessary broadcasted information, corresponding to the intervehicular measurements (e.g., GPS and range measurements extracted from LiDAR data), as well as the control inputs of its own star topology, estimates self and neighbors' position. An indicative execution scenario of our approach is shown in Fig. 2. In this Section, we validate the proposed approaches using Python and CARLA simulator.

A. Experimental Setup
We extracted the trajectories of N = 200 vehicles, along with their control inputs, from CARLA. Ground truth trajectory for (a randomly chosen) ego vehicle, together with the different VANETs which belongs to at four distinct time slots, are shown in Fig. 3. Velocities of vehicles range between 0 (e.g. waiting at traffic lights) and 74km/h. Black and blue dots in Fig. 3-(b) represent ego and neighboring vehicles, while red links the V2V connections. V2V neighbors of ego vehicle during the simulation were between 1 and 16 vehicles. Simulation horizon was set to T = 573 time instances, while ∆T = 0.3sec. The GPS noise was generated with σ x = 3m and σ y = 2.5m. Maximum number of iterations and step size for MLL-GD were set to K max = 300 and µ = 0.001. We assume that both methods don't have any knowledge of noise variance. Therefore, covariance matrices, as well as range measurements noise variance, are initialized to identity/one. During the experiments, we studied the impact to ego and neighboring vehicles location estimation, of VANET size and range measurements uncertainty. The former depends upon communication range r c and dictates the connectivity representation of CAVs. The latter has a direct impact on the pair-wise range measurements and is common aspect of highly complex and occluded urban environments. As evaluation metrics, we measured at each time instant Localization Mean Square Error (LMSE) of all vehicles, Localization Error (LE) of ego and Average Localization Error (ALE) of individual star topology, and constructed the corresponding Cumulative Distribution Function (CDF).
B. Evaluation Study 1) Impact of connectivity topology and communication range: Connectivity topology of CAVs, i.e. which vehicles are V2V connected, can be seen through the communication range p,j . In Fig. 4-(a),(b), the CDF of LMSE is depicted with r c = 20m and r c = 30m, and σ d = 1m, σ a = 4 • . That error has actually been computed using the output of two algorithms and GPS for each ego vehicle. Clearly, EKF-CA exhibits superior performance over MLL-GD, highlighting the benefits of tracking for location estimation. More specifically, the former achieved 61% reduction of GPS LMSE, while the latter only 49%. By increasing the communication range to r c = 30m (as a matter of fact more neighbors are taken into account), higher location estimation accuracy has been achieved, especially for MLL-GD. In Fig. 4-(b), EKF-CA achieved 67% reduction of GPS LMSE, while MLL-GD 61%. We conclude that the integration of information from a large number of neighbors, resulted in higher estimation accuracy. Furthermore, EKF-CA proved to be much better than MLL-GD. Small figures coupled with error values, indicate maximum LMSE (LE and ALE, respectively) achieved by the two methods with 80% probability.
2) Effect of range measurements uncertainty: In occluded urban conditions it is expected that LIDAR sensor may provide range measurements highly contaminated by noise, which seriously affects algorithms' performance. The impact of range measurements uncertainty is investigated in Fig. 4-(c), with σ d = 4m, σ a = 7 • and communication range r c = 20m. It is evident that performances have been seriously degraded. The EKF-CA reduced GPS LMSE by 41%, while MLL-GD by 38%. That deteriorated performance could be seen contrary to Fig. 4-(a) (with σ d = 1m and σ a = 4 • ). For example, with 80% probability the LMSE with EKF-CA can reach 9.79 m 2 (instead of 6.35 m 2 ). Therefore, we conclude that increased range measurements uncertainty seriously affects the performance of the two methods. Distributed tracking has been verified to be slightly more robust to noisy pair-wise range measurements.
3) Cooperative awareness evaluation: In Fig. 4-(d), we demonstrate the location estimation accuracy for the chosen ego vehicle, with communication range r c = 20m, σ d = 1m and σ a = 4 • . Once again EKF-CA outperforms MLL-GD. More specifically, EKF-CA achieved 58% reduction of LE, with contrast to 54% of MLL-GD. Furthermore, EKF-CA succeeded in reducing maximum GPS LE from nearly 10m, to 6m (instead of 8.3m using MLL-GD). The indicative results seem to verify the overall evaluation for the total number of vehicles in Fig. 4-(a). Cooperative awareness accuracy of the proposed methods has been evaluated in Fig. 4-(e), by plotting the ALE of different star topologies (containing both ego and neighbors). The proposed EKF-CA, once again, proved to be much superior, exhibiting 38% reduction of GPS ALE. Finally, in Fig. 4-(f) we demonstrate a snapshot from CARLA simulator containing both ego vehicle and its neighbors, along with the location estimation accuracy achieved by the ego. Therefore, proposed EKF-CA is also useful for effectively performing the cooperative awareness task.
V. CONCLUSION This paper has contributed towards the design of a robust localization system enhanced with increased cooperative awareness ability, which is considered fundamental for automated vehicle's reliable operation. For that purpose, we have formulated two novel distributed and cooperative localization and tracking schemes. Each vehicle, acting as the center of its own V2V connectivity topology, is in place to accurately estimate its position, as well as those of its connected neighbors. Under a variety of different experimental conditions in CARLA renowned simulator, we have highlighted the feasibility and benefits of the two methods. Distributed tracking has proven to be superior than GD-based scheme, especially in the case of cooperative awareness task.