A Distributed Cooperative Localization Strategy in Vehicular-to-Vehicular Networks

This work develops a distributed message-passing approach to cooperative localization for autonomous mobile vehicles that communicate via mm-wave wireless connection in vehicle-to-vehicle networks. Vehicles in the network obtain the measurement information about the relative distance and the angle of arrival from the mm-wave connections made with each other. Some vehicles may obtain knowledge about their absolute position information of different quality, for example, via additional localization feature. The main objective is to estimate the locations of all vehicles using reciprocal exchanges of simple information called a message in a distributed and autonomous way. A simulation is developed to examine the performance of the localization and navigation of vehicles under various network configurations. The results show that it does provide better positioning results in most cases and there are also several cases where the use of the cooperative technique adapts to design parameters such as accuracies of measurement equipment, and initial position estimates, that can affect the localization performance.


Introduction
With the recent introduction of 5th generation (5G) communication with low latency, large capacity, and high reliability characteristics, actual implementation of autonomous vehicles has been rendered more realistic [1]. Among the most important aspects of an autonomous vehicle, one very essential feature is the estimation of its position in a reliable and autonomous manner. This feature requires the robustness to inter-node connection failure, real-time updates, and high accuracy [2][3][4]. The localization of a group of vehicles can be conducted in a centralized manner; all vehicles' locations can be estimated by a single computationally superior unit. While this approach alleviates computational demands of most vehicles in the network, its scalability has a limit since it is responsible for all processing steps such as collection, calculation, and distribution of the positioning information. Furthermore, the amount of devoted computation efforts scales directly with the population of vehicles in the network. Since the constraint on the scalability may cause significant challenges in applying the localization technique in an imminent large-scale vehicle-to-everything network, a distributed localization method where the positions of individual vehicles are identified locally and independently by themselves can be considered. A distributed method of the cooperative localization can adapt to a network of varying sizes because its local estimations involve only observations of a few neighboring units. In most cases, in order to obtain accurate location information, the global navigation satellite system (GNSS) is supported. However, its systems exploit essentially satellites and its effectiveness does not prove sufficient for autonomous driving in harsh environments with obstacles undermining the satellite signals, such as tunnels and undergrounds [5][6][7]. Therefore, one of the most promising

•
A mathematical formulation is constructed for vehicular self-localization and identifying vehicles' relative positions based on a V2X network where all vehicles do not necessarily obtain the exact position information from peripheral devices. • A novel message-passing framework is developed to determine the positions of all vehicles. The algorithm carries out the exchange of only the estimate and accuracy of the position to save computation costs spent to obtain the improved representation of messages. In addition, a broadcast protocol is designed to decrease message calculations and communication overloads for networking. • A GUI simulation platform is developed to test the vehicular network based on 3GPP TR 37.885 specifications. The performance comparison among various distributed strategies is made to justify the localization performance of the proposed algorithm.
The rest of this paper is organized as follow: Section 2 presents the system model in a mm-wave connection based vehicular network. An MP algorithm is derived to handle the position estimates and their accuracies for cooperative localization in Section 3. Section 4 evaluates the performance of the proposed algorithm, and Section 5 concludes the paper.

Vehicular Network Model
This section introduces a system model for cooperative localization in a vehicle-to-vehicle (V2V) network. The vehicles in the network move along the road, and they can communicate with at least one of their neighbors. Among the vehicles, at least one vehicle, called an anchor, is capable of obtaining different quality measurements received from peripheral devices over other vehicles, called agents [18].
2k ] be two-dimensional coordinate vectors of vehicle j and vehicle k at time t, respectively. Vehicles make measurements about their movement and communicate in mm-wave with surrounding vehicles to obtain the estimate about the relative distance and the angle of arrival (AoA) [2,20]. The relative distance measured by vehicle j from vehicle k at time t is given by where · is an Euclidean distance. Also, the AoA value measured by vehicle j from vehicle k is given by .
(2)  To estimate the vector x (t) j for positioning at time t, a Bayesian technique can be used to find the posterior probability distribution for x (t) j from available information. For the information about the movement of vehicle j, vehicle j estimate its position denoted by q (t) j only using the previous position x (t−1) j and its noisy measurement of the speed of driving. Subsequently, the mobility model for vehicle j available to itself is characterized by a prior probability distribution denoted by p(x Furthermore, the measurement model about the collection of parameters received from a surrounding vehicle k denoted by w j ). With those probabilistic functions, the identification of the MAP estimate of x j , denoted byx j , is pursued. For the MAP estimation, the marginal posterior p( j ) is necessary and is represented as Several reasonable assumptions can be made in identifying the positions of vehicles: All vehicles move independently of each other and their movement are modeled in terms of Markov chains. In other words, the knowledge about the mobility obtained internally by each vehicle is independent and is not affected by other vehicles. Furthermore, the relative measurement results are also independent of each other, while the relative measurement results are only affected by the current condition of the vehicle.

Mobility Model
With the knowledge of the mobility model p( denote the estimated velocities of vehicle j along the x-axis and the y-axis of the domain at time instant t. This can be modeled as a pair of random variables, since it is subject to corrupted measurement incurred by sensory hardware imperfection, which is characterized by noise varianceσ 2 . The interval between consecutive time instants is denoted by ∆t. In addition, the mean and variance of the mobility model p(x j and an accuracy vector ϕ q j (t), which can be expressed, respectively, as Note that each component of the accuracy vector [ϕ q 1j , ϕ q 2j ] corresponds to the variation of the velocity in the x-axis and the y-axis directions, respectively.

Measurement Model
Vehicles j uses measurement model p(w j ) to estimate the relative distance and the AoA from neighboring vehicle k. The noisy measurement obtained by vehicle j about the distance to vehicle k is expressed as w being a zero-mean Gaussian random variable with variance σ 2 n d . Likewise, the noisy AoA measurement is given by w is also a zero-mean Gaussian random variable with variance σ 2 n θ .
If vehicle j measures two parameters from vehicle k, likelihood function p(w where p(w k ) are given, respectively, by Note that these mobility and measurement distribution models enforce two different types of constraint functions in deriving a message-passing algorithm in the following section.

Proposed Message-Passing Algorithm
To develop a distributed algorithm via a message-passing framework, a factor graph [21] is introduced. The factor graph consists of function nodes and variable nodes. A function node is usually represented in a square, while a variable node is denoted in a circle in the graph. Each variable node is connected by an edge with its associated function node. In this problem, each variable node is associated with the coordinates of a vehicle, and two types of function nodes are introduced to reflect the internal mobility model and the measurement model. Since the measurement model is based on the information between each pair of adjacent vehicles, the corresponding function node has two connections to two variable nodes. On the other hand, a function node associated with mobility model is connected to a single variable node since the mobility model involves only its internal state of each vehicle. The messages are calculated using the sum-product rule [21] and are exchanged along connected edges. The repeated exchanges of the messages continue until all messages converge to respective fixed values.
The algorithm proceeds in three steps: First, a message which evidences the next position of a vehicle goes from a mobility function node to the corresponding variable node. The second-step message that contains the information about the relative distance and the AoA between each pair of adjacent vehicles also goes from a measurement function node to a variable node. The last message corresponding to the estimated current position of an individual vehicle goes back from the associated variable node to all neighboring function nodes. A single iteration consists of three steps of the message calculation. The iteration, indexed by l, of message transfers at a time instant is repeated until the value of messages stop changing. To be specific, the messages of vehicle j corresponding to three steps at a time instant are denoted by µ(x j , l), respectively. Among those values, the last two message vectors are expressed as where N(j) is a set of indices for neighboring vehicles that can communicate with vehicle j. Consider the first-step message update rule defined with respect to the mobility model. Let j , t) be the message associated with internal mobility model for vehicle j at time t. According to the sum-product update rule, message µ(x (t) j , t) is simply given by mobility probability distribution j ) is simply represented by mean and variance, µ(x j , t) can be characterized with position vector q (t) j and accuracy vector ϕ q j (t), which are given, respectively, as The second-step message update rule is associated with the measurement model which vehicle j obtains the distance and the azimuth from neighboring vehicle k. Message j , l) is calculated by integrating the product of the measurement model function and the incoming message as Note here that likelihood functions p(w kj are nonlinear functions of coordinates of vehicle as in (1) and (2). Thus, the corresponding message can be evaluated only by numerical integration and is not simple to handle in a real-time manner.
To resolve unwieldy handling of this message, we introduce a new parameterized measurement model as shown in Figure 2, Figure Figure 2b, where horizontal and vertical axes are aligned toward the moving direction of vehicle j. By applying for this model with Cauchy-Schwarz inequality followed by some algebra, a new measurement model is characterized as a function of a two-dimensional coordinate difference vector of vehicle j and k. The resulting measurement model with estimate vector w and respectively. Figure 3 shows the factor graph representing the new measurement model corresponding to the example of Figure 1. Now we can rewrite the message update equation as Note here that the message sent from the measurement function node to variable node x (t) j at iteration l evaluates the convolution with respect to x where components of estimate vector ∆ 2kj ] and accuracy vector ϕ ∆ kj (l) = [ϕ ∆ 1kj (l), ϕ ∆ 2kj (l)] are simply given, respectively, by and Since the message with component distributions in (21) and (22) can be fully described using estimate vectors and accuracy vectors, it suffices to send them for the message transfer. Finally, the message which a vehicle updates about its position and sends to a neighboring vehicle is calculated. The message sent from the variable node is updated using the sum-product update rule with all incoming messages from the measurement function nodes and a single message from the mobility function node. The resulting message update rule accounts for multiplying those incoming messages as where N(j)\k is the set of neighboring vehicles that can send or receive the message from vehicle j except vehicle k. Since all incoming messages are represented with position estimate and accuracy vectors, the assumption of Gaussian shape distributions leads to their product being also of a Gaussian shape. Therefore, estimate and accuracy vectors of the outgoing message j , l) can also be represented as the function of estimates and accuracy values of incoming messages. Each component of the resulting message is described in a probability distribution given by where components of estimate vectorx 2j ] and accuracy vector ϕx j (l) = [ϕx 1j (l), ϕx 2j (l)] are calculated, respectively, asx x (l) and If a vehicle estimates its position and sends the corresponding message to neighboring vehicles, it calculates distinct forms of messages according to neighboring vehicles since the message update rule in (27) does not include the message coming from the direction of the message going out. However, as the network size scales up, the evaluation of all different messages incurs significant number of computations. Furthermore, for transmission of those messages, a sophisticated scheduling strategy that makes connections, transmits messages, and closes connections to respective neighbors is necessary, and its associated communication loads readily become critical for real-time operation. To resolve this scheduling challenge, identical messages are desired to send to all neighbors. If there are a sufficiently number of vehicles in the network, outgoing messages make little difference for all vehicles. Thus, the vehicles need to send a common message to surrounding vehicles to indicate its position. This makes little impact on the accuracy of cooperative positioning and boils down to a broadcast protocol for handling messages. Estimate vectorx 2j ] and accuracy vector ϕ x j (l) = [ϕx 1j (l), ϕx 2j (l)] of the probability distribution corresponding to a common message are described, respectively, asx and Note that, in fact, out of three-step message update rules, the first two update rules require only the information internally acquired within a vehicle. The last update rule, however, evaluates the messages that are externally exchanged with neighboring vehicles. It suffices for a vehicle to transmit only the messages calculated using (34)-(37). Thus, each vehicle calculates its position once with the common outgoing messages and broadcasts them to neighboring vehicles. By doing so, the total number of message calculations decreases, thereby lifting the communication burden. In addition, it has an advantage that vehicles can be learn simply all positions of surrounding vehicles.
In light of the structural discrepancy between the developed message-passing algorithm and its physical deployment of distributive positioning, additional consideration is required about handling of messages. The mm-wave technologies are used for conveying the messages computed by the message-passing operation. The signal strengths of the mm-wave signals can be measured by individual vehicles to acquire the estimates of the relative distances and AoAs of the neighboring vehicles. In practical implementation, the steps illustrated in Figure 4a-c are repeated while vehicles estimate their positions at time t via mm-wave technologies. In Figure 4a, vehicle j updates its new position estimate with the mobility model output. In Figure 4b, the signal strengths are measured to estimate the relative distance and AoA of the neighboring vehicle k and the function-to-variable messages in (23)-(26) are updated. Finally, in Figure 4c, vehicle j updates its position by collecting messages calculated in Figure 4a,b and, in turn, broadcasts it to surrounding vehicles. For practical deployment, the computation of messages in Figure 4a,b is carried out internally at each vehicle, while the communication with surrounding vehicles occurs only in Figure 4c. Algorithm 1 provides a summary of the proposed protocol for the distributed positioning algorithm. for nodes j ∈ N do in parallel 5: calculate the estimate and the accuracy of the message, µ(x j , t) in (11) and (12) 6: end for 7: for l = 1 to L max do {iteration index} 8: for j = 1 to N in parallel do 9: calculate the estimate and the accuracy of the message → µ (x j , l) in (23)-(26) 10: calculate the estimate and the accuracy of the message ← µ (x j , l) in (34)-(37) 11: end for 12: broadcast the estimate and the accuracy of the message

Performance Evaluation
In this section, we test the performance of the proposed MP algorithm (denoted by "SMP") and compare with several algorithmic options, such as a different type of the message-passing algorithm [11] (denoted by "CSMP"), variational message-passing algorithm (denoted by "VMP") [22], and extended Kalman filter (denoted by "EKF") [13]. These algorithms run essentially in a distributed manner that requires similar levels of computational efforts and communication burdens, which allows fair comparison in the performance test. known to have similar computational costs for fair comparison.

Cooperative Localization Algorithm
Simulation has been conducted under vehicular network scenarios described in 3GPP TR 37.885 specification to verify the performance and strategy of the proposed algorithm. A V2V environment of 3GPP is a highway with 5 lanes. The width of the lanes is 3.5 m, and the moving distance is 2000 m. A single anchor is assumed to exist that can obtain its position with relatively improved accuracy with peripheral devices. The mobility model of each vehicle has the average speed of [0 kph, 72 kph] with variations ranging in [0.1 (kph) 2 , 1 (kph) 2 ]. In the measurement model, the uncertainty with variation of 1 m and 3 • are incurred in d and θ respectively. Each vehicle measures its own speed every 100 ms and broadcasts a message every 10 ms. All vehicles obtain their initial positions corrupted by additive noises which cause vehicles to consider as if they are in a false lane. The simulation parameters are summarized in Table 1.   Figure 6 shows the performances of the proposed algorithm in comparison with several distributed algorithms in cases whether an anchor does or does not exist. The performance is presented with the cumulative distribution function (CDF) of the localization error with respect to observation parameters. SMP has superior performance over existing algorithms. CSMP has the degraded performance but outperforms other algorithms. In addition, VMP performs similarly but slightly worse than CSMP, while EKF shows additional performance degradation. If an anchor vehicle exists, the SMP shows improved performance with average error of 0.31 m, while CSMP, VMP and EKF have average errors of 1.64 m, 1.73 m and 5.16, respectively. Therefore, if at least one anchor exists in the network, even if the proposed algorithm has some level of the uncertainty in AoA, it can greatly improve the cooperative localization performance. On the other hand, in case where anchor vehicle lacks, the performance of SMP depends on the quality of the AoA. If the deviation of the AoA measurement is within 1 • , SMP still has performance improvement over existing techniques. However, if the uncertainty in AoA measurement amounts up to 3 • , SMP may be subject to a large absolute error. In case of the localization error value of 5 m, the deviation in the AoA measurement of 5 • causes the inversion of the localization error with respect to the unavailable AoA information measurement case. Since the distance between vehicles can be considered to be proportional to the localization error, the positioning error value can be regarded as the distance between vehicles. The farther vehicles are apart, the larger deviation on the information of the angles is received. Therefore, inaccurate information of θ can increase the localization error, thereby impairing the localization performance. To analyze the noise limit on the distance and AoA in the model based on 3GPP TR 37.885 specification, a limit of the standard deviation of the AoA is obtained from the Crammer-Rao Lower Bound with respect to the signal-to-noise ratio (SNR). The theoretical bound of the angle is 0.46 • which is strictly smaller than 1 • . According to this result, it is expected that SMP has relatively good performance with the error incurred in AoA available from reasonably practical measurement equipment.  Figure 7 represents absolute and relative errors of the proposed algorithm with respect to the driving distance of vehicles. The total driving distance is 2000 m, and 10 vehicles exchange messages with each other. If no anchor exists, the absolute average error value continues to increase. Since a vehicle which has relatively high accuracy information of position lacks, the uncertainty of the positions continuously propagates over the network for their inaccurate operations while running along the road. On the other hand, if there is at least one anchor, the absolute error value is kept below 40 cm all the way to the driving distance of 2000 m. As a result, if only a small number of anchor vehicles is available, even a single one, the absolute positioning error of the entire network is greatly improved since messages that convey relatively accurate information helps locating all vehicles correctly. Meanwhile, the relative error maintains low regardless of the presence of the anchor vehicle. This indicates that the error is only affected by the measurement quality of observation parameters d and θ. Since the AoA information is available from other vehicles, the relative error value is kept as low as 20 cm on average. This means that all vehicles are affected by the same level of the bias in positioning. Therefore, there rarely happens a chance of dangerous situations caused by the collision among vehicles.  Table 2 shows trends of the absolute error with respect to the number of vehicles and the distance from an anchor vehicle to the group of agent vehicles. The total absolute error of the vehicular network increases as the distance increases, or as the number of vehicles in the network decrease. The minimum absolute error becomes down to 10 cm if a single anchor vehicle is close to 30 vehicles. On the other hand, the maximum error amount to 2.82 m if an anchor vehicle is apart from 5 agent vehicles. According to these observations, if the network size is large, accurate positioning is possible even if the anchor vehicle is relatively distant. By contrast, if the number of vehicles is small, an anchor is necessarily located nearby to maintain certain levels of the localization error. Therefore, this provides the information about the density of the anchor vehicle in the network required to guarantee the low level of the positioning error. For example, if the absolute positioning error value of the network should be kept within 1 m, there must be at least one anchor within a radius of 50 m for 5 vehicles, 150 m for 20 vehicles, 300 m for 30 vehicles in the network, and so on. Table 2. Absolute error trends depending on the number of vehicles and the radius of an anchor.

V2V Network Strategy with Corrupted Mobility Model
The uncertainty of the initial position of vehicles is already considered along with the communication strategy when measuring the relative distance and azimuth angle between vehicles. For practical deployment, the uncertainty caused by hardware imperfection is now addressed. This is referred to as a noisy mobility model. The uncertainty of measuring mobility measurement is a zero-mean Gaussian noise incurred in the existing mobility model in (11) and (12). Letσ denote the standard deviation of the added noise. If a vehicle is in motion, there is some discrepancy between the actual speed of the vehicle and the speed displayed on the dashboard [23]. The uncertainty of the speed can be quantified with respect to the vehicular motion. Figure 9 shows the average absolute errors with respect to the number of message-passing iterations ifσ ranges from 0 m to 0.5 m. The simulation results indicate that cooperative positioning is essential ifσ is 0.2 m or greater, in that the localization error converges to the value strictly less than the initial value. If theσ ranges below than 0.2 m, that is, the accuracy of the sensor is sufficiently high, cooperative positioning can be unnecessary, and the vehicles may prefer to estimate their positions based on their own mobility models. This indicates that in-vehicle networks of inexpensive hardware devices require cooperative positioning with surrounding vehicles. We can also envision how much the positioning performance degrades in an unfavorable communication environment from Figure 9. The failure to the detection of message-conveying signals causes the loss of the message information along with the lack of the measurement information, thereby preventing from proceeding with another iteration of message-passing operations. In particular, in case of employing 10 iterations of message-passing operations, 20% packet loss, corresponding to 8 iterations, degrades the performance only by 2%. This shows that such an extreme packet loss results in negligible damages to the performance. Thus, this proves the robustness of the proposed algorithm in communication environments with typical qualities.

Conclusions
This paper proposes a message-passing algorithm that enables real-time distributed positioning in cooperation with surrounding vehicles if mm-wave communication is available. The proposed algorithm increases the possibility of the real-time communication by parameterizing complex messages into parameter pairs of estimate and accuracy. The broadcast strategy that allows for non-directional messages is employed so that the computational costs for radio resource allocation and scheduling tasks are relieved. According to the simulation results that have been analyzed with real-time errors, the performance of the proposed algorithm can be improved over existing distributed positioning algorithms.
In extensive numerical simulations under the highway environment provided from 3GPP TR 37.885 specification, the average absolute error below 40 cm and relative error below 20 cm are achieved, respectively. For various vehicular environments, the network size and the number of anchor vehicles are identified to maintain a certain level of the localization error. In addition, a proper condition that enables cooperative positioning is suggested under the uncertainty of vehicles' mobility measurement. The analysis of the measurement equipment uncertainty indicates that cooperative positioning is essential if the standard deviation of the noise is greater than 0.2 m, whereas, if relatively accurate speed estimation is available, cooperative positioning may not be necessary. The proposed framework offers several future research directions for the deployment in a measurement model for urban area scenarios, such as in 3GPP TR 37.885 specification, with only non-line-of-sight (NLOS) measurement available and the experiments with actual mm-wave based radio technology.