ASSESSMENT OF CAR COLLABORATIVE POSITIONING WITH UWB AND VISION

During the last decades the role of positioning and navigation systems is drastically changed in the everyday life of common people, influencing people behavior even multiple times each day. One of the most common applications of this kind of systems is that of terrestrial vehicle navigation: the use of GPS in the automotive navigation sector started thirty years ago, and, nowadays, it commonly assists drivers in reaching most of their non-standard destinations. Despite the popularity of global navigation satellite systems (GNSS), their usability is quite limited in certain working conditions, such as in urban canyons, in tunnels and indoors. While the latter case is typically not particularly interesting for the automotive sector, the first two scenarios represent important cases of interest for automotive navigation. In addition to the market request for increasing the usability of navigation systems on consumer devices, the recent increasing eagerness for autonomous driving is also attracting a lot of researchers’ attention on the development of alternative positioning systems, able to compensate for the unavailability or unreliability of GNSS. In accordance with the motivations mentioned above, this paper focuses on the development of a positioning system based on collaborative positioning between vehicles with Ultra Wide-Band devices and vision. To be more specific, this work focuses on assessing the performance of the developed system in successfully accomplishing three tasks, associated to different levels of gathered information: 1) assessing distance between vehicles, 2) determining the vehicle relative positions, 3) estimating the absolute car positions. The obtained results show that a) UWB can be reliably used (error of few decimeters error) to assess distances when vehicles are relatively close to each other (e.g. less than 40 m), b) the combination of UWB and vision allows to obtain good results in the computation of relative positions between vehicles, c) UWB-based collaborative positioning can be used for determining the absolute vehicle positions if a sufficient number of UWB range measurements can be ensured (sub-meter error for vehicles connected with a static UWB infrastructure, whereas error at meter level for those exploiting only vehicle-to-vehicle UWB communications).


INTRODUCTION
Ultra Wide-Band (UWB) positioning systems have already been used in several applications to, at least partially substitute, global navigation satellite systems (GNSS). In particular, they have already been successfully used in several positioning applications, indoors (Dabove et al., 2018, Sakr et al., 2020, sometimes also in combination with vision , for Unmanned Aerial Vehicles (UAVs) (Tiemann et al., 2015, Goel et al., 2017. This paper focuses on the development of a terrestrial vehicle positioning system able to compensate the absence of GNSS, in particular in the case of urban scenarios, e.g. urban canyons. To such aim, the combined use of UWB transceivers and of vision is investigated: the rationale is that of taking advantage of the good UWB ranging performance at relatively short distances (based on two-way time-of-flight ranging (TW-ToF)), while exploiting vision to compute angular measurements related to the relative position of other vehicles.
In most of the UWB positioning system on the market two types of devices can be distinguished: rovers, which are allowed to move, and anchors, which are fixed on certain static and known locations. In this standard case, rovers exploit range measurements from the anchors in order to compute their posi- * Corresponding author tion. Hereafter, this working condition will be named hereafter vehicle-to-infrastructure (V2I) positioning.
However, in the automotive case, determining the relative distance (and position) between close vehicles is as important as determining each vehicle position. Consequently, in this work UWB vehicle-to-vehicle (V2V) range measurements have also been collected, by means of TW-ToF measurements between UWB devices mounted on the moving vehicles.
The, UWB range measurements are combined by means of an Extended Kalman filter, properly designed to deal with a centralized collaborative positioning approach: the proposed system, which can be considered as a generalization of the approach presented in (Gabela et al., 2019), effectively integrates GNSS measurements, V2I and V2V range measurements.
Furthermore, a deep learning object detection approach (LeCun et al., 2015) has been developed to detect vehicles on the video frames acquired by a camera mounted on the front of one of the cars involved in the experimental test conducted to validate the proposed approaches. Then, angles of the detected cars with respect to the camera optical axis are used, in combination with the corresponding UWB ranges, to determine the relative position of the other cars with respect to the vehicle provided with a camera.
The rest of the paper is organizer as follows: Section 2 summarizes the experimental setup of the tests conducted to check The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII- B1-2021XXIV ISPRS Congress (2021 the performance of the considered positioning approaches, Section 3 analyzes the UWB ranging performance (i.e. the ability in determining the distance between vehicles), Section 4 shows the assess the performance of the relative positioning system based on the combined use of UWB devices and vision, Section 5 presents the developed UWB-based collaborative positioning approach, and, finally some discussion and conclusions are drawn in Section 6 and 7.

WORKING SCENARIO
Data collection for testing the proposed approaches has been conducted in a parking lot at OSU West Campus, involving four vehicles ( Fig. 1): • GPSVan, the OSU mobile mapping vehiclere Each vehicle was equipped with several sensors, in particular, the sensor configuration of interest for this paper is depicted in Fig. 2: • two Pozyx UWB transceivers (Pozyx Labs, 2015) were mounted on the two sides of each vehicle (typically on poles, as shown in Fig. 1). Such two UWB devices, named Pozyx L and Pozyx R in Fig. 2, provided the V2V range measurements for the collaborative positioning approach. Furthermore, each vehicle was equipped with at least one GPS/GNSS receiver (two in the GPSVan case), typically positioned on the left side of the vehicle, close to Pozyx L (see Fig. 2).
• in addition to the previously mentioned Pozyx devices, a TimeDomain (TD) UWB transceiver was mounted on the GPSVan, providing V2I range measurements with a set of ten TD anchors distributed along a road in the parking lot (installed on tripods, as partially visible in Fig. 1). A GoPro 5 Black camera was also mounted on the front of the GPSVan, acquiring video frames with 3840 × 2160 pixel resolution at 30 Hz (GPR1 in Fig. 2).
The ten TD anchors are shown as red circular marks in Fig. 3. Fig. 3 shows also the track pattern of the GPSVan during the test (gray solid lines), and the main area of interest (red area in the The test area has been selected in order to ensure good performance of the GPS/GNSS receivers on all the area: in practice, GPS/GNSS receivers provided reliable and accurate position measurements during all the test duration (i.e. expected accuracy of few centimeters). Consequently, such measurements can be reliably used as reference trajectories to validate the proposed positioning approaches. The rationale of the considered V2I installation is that of mimicking the case of a urban canyon, where either none or few vehicles may have quite reliable GPS/GNSS measurements. Hence, the V2I range measurements are used to determine the absolute position of the vehicles able to communicate with the V2I UWB network, whereas the V2V UWB network is exploited to implement collaborative positioning, i.e. to assess the positions of the other vehicles.
It is worth to notice that the UWB V2V absolute position estimation problem is ill posed against rotation when only one vehicle absolute position is known (e.g. provided with V2I measurements), i.e. if other "prior" information is not provided, it makes sense to consider the obtained solution in this case only up to a rotation around the known vehicle position. Since during the test only one car was able to receive V2I measurements (the GPSVan), in the collaborative positioning section an other car is assumed to receive updates of its absolute position (e.g. by means of GPS/GNSS in this case).
The numerical results reported in the following sections refer to the analysis of the datasets acquired by all the considered sensors in a time interval approximately ten minute long. Since the test area was practically planar, only 2D positioning results are reported in this work.

VEHICLE RELATIVE DISTANCE
This section aims at assessing the reliability of UWB ranging, in order to check its potential performance in determining the distance between vehicles. Fig. 4 shows an example of V2V Pozyx ranging compared with the GNSS-based distance between the GPSVan and the Toyota Corolla. UWB measurements are in clear agreement with the GNSS estimates, however, it is quite apparent that the number of available UWB measurements drastically decreases for distances approximately larger than 40 meters. The above observation is also confirmed by Fig. 5, where the success rate of the Pozyx L network is shown as a function of the distance between the two transceivers. It is also worth to notice that, the two Pozyx networks worked on different channels (2 and 5), leading to a different performance in terms of the maximum reachable range, which was 139 m (Pozyx R) and 66 m (Pozyx L), respectively.
Consequently, the Pozyx R success rate curve is similar to that shown in Fig. 5, but the success rate is higher at practically every distance. Nevertheless, similarly to Fig. 5, in both the cases the range success rate quickly reduces for distances above few tens of meters: this is due both to the low transmitting power of the transceivers and to the presence of obstacles in the environment. Finally, Fig. 6 shows the ranging error distribution for Pozyx L, whereas a more detailed statistical characterization is provided for both the Pozyx devices in Table 1. The UWB ranging error is assumed to be Gaussian in significant part of the works on UWB positioning. Nevertheless, it is quite apparent from Fig. 6 that in this case the error distribution has a quite light tail, probably due to non-line-of-sight (NLOS) measurements. This observation motivated the use of robust statistics to evaluate the error characteristics in Table 1.

V2I TD
Furthermore, Fig. 7 provides an evaluation of the number of simultaneously available V2V measurements for the two Pozyx networks, i.e. the availability of a new range measurement is iteratively checked in a loop for all the couples of devices in a network, those ranges found available in the same loop are considered as "simultaneously" available in Fig. 7 (clearly they are not really simultaneous).

VEHICLE RELATIVE POSITION
Relative vehicle positions are coputed in this section by combining information provided by the Pozyx UWB networks, and by the GoPro 5 Black camera mounted on the GPSVan.
The GoPro 5 Black camera is assumed to be calibrated, and its horizontal axis is assumed to lie on the horizontal plane. The extension of the approach to the general case is immediate.
To be more specific, first, a deep learning-based object detection approach is used: a properly finely tuned Yolo v3 Network is used to detect the cars in the GoPro video frames, as shown in Fig. 8. Yolo v3 Network is known to be a very effective real-time object detection tool. The description of the network training and performance is beyond the scope of this paper: the reader is referred to the literature specific on this topic (Redmon and Farhadi, 2018).
Then, the central horizontal coordinates of the box corresponding to a detected car is used to compute the angle α (shown in Fig. 2): angle α identifies the direction of the detected car with respect to the GPSVan local reference system.   When one V2V UWB range measurement r is available, between the GPSVan and the detected car, the relative position of the detected car with respect to the GPSVan is estimated by computing the intersection between the line passing through the GoPro optical center and identified by the angle α and the circle corresponding to the distance r from the UWB device that provided the measurement r.
Instead, positioning with two V2V UWB measurements allows to estimate also the orientation of the detected car: this is performed by solving an optimization problem, aiming at minimizing the difference between the measured variables (e.g. the two ranges and the angle) and their counterpart computed from the estimated car relative position and orientation (to be precise, the optimization problem is formalized as a least squares adjustment problem).
This approach lead to the positioning results shown in Table 2: the Table shows the 2D positioning error and also its decomposition along the heading direction and its transverse.

COLLABORATIVE POSITIONING FOR VEHICLE ABSOLUTE POSITION ASSESSMENT
This section describes the proposed method for collaborative positioning, which can be considered a generalization of that presented in (Gabela et al., 2019).
Actually, first, subsection 5.1 provides a short characterization of the V2I TD measurements. Then, subsection 5.2 presents the collaborative positioning approach.

V2I TimeDomain measurement characteristics
Similarly to the Pozyx devices, the measurement success rate of the V2I TD transceivers decreases when the distance between the vehicles becomes larger (see Fig. 9). Nevertheless, the TD performance is apparently more similar to that of the Pozyx R network, i.e. larger ranges can be measured with respect to the Pozyx L network. Despite the V2I infrastructure is formed by ten anchors, given the relatively small success rate for large distances, the number of "simultaneously" available measurements was never larger than 6, as shown in Fig. 10.
Then, Fig. 11 shows the ranging error distribution, and the "V2I TD" column in Table 1 reports the error statistics, which, overall, are apparently better than the Pozyx ones.
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2021 XXIV ISPRS Congress (2021 edition)

Collaborative positioning
An Extended Kalman Filter (EKF) approach is used to assess the state value x k at time t k , and in particular the vehicle position, based on all the available measurements.
Let p c i (t) and v c i (t) be the position and velocity of the ith car at time t, and, let x k be the joint state vector at time t k and x c i k the state part corresponding to the ith car, which can be defined as follows Then, The following dynamic model is used to describe the relation between x c i k and x c i k+1 , i.e. the state evolution in ∆t k+1 seconds: where ω k is assumed to be a Gaussian distributed zero-mean white noise process, with covariance matrix Q k , and F k is defined as follows and hence The observation vector z k can be decomposed in three different types of measurements: where p c i 1 k and p c i 2 k are the positions of the two cars when such range measurement is taken.
Then, the linearized observation matrix H k , assuming for simplicity all the measurements available, can be expressed as follows: where h j k and h shall computed by linearizing the cor- Since the range measurements are not acquired in the same time instants, such time difference should be taken into account in the positioning algorithm. In particular, consider for simplicity a V2I range obtained from anchor j (the generalization to V2V is immediate) at time t j k = t k (for simplicity of notation assume t j k > t k ).
Assume a constant velocity in the (t k , t k + t j k ) interval, and let δ j k = t j k − t k , then the position of the GPSVan at time t j k can be computed as follows: and hence where p c 1 k|k−1 (t j k ) is the estimation of p c 1 (t j k ) obtained from measurements available up to the previous iteration of the EKF, andẑ j k is the vehicle-to-anchor distance in accordance with the estimate vehicle position.
The developed approach is applied to the collected data, using the available V2I and V2V measurements, and assuming GNSS available on the Honda Accord. Table 3 shows the obtained positioning results on the main area of interest.
The first column in Table 3 shows the GPSVan 2D positioning error, which, with a slight abuse of notation, has been named "V2I" in the Table. Instead, second and third columns in Table 3 show the results obtained in time instants associated to different number of available ranges. For instance, # rV 2V ≥ 2 refers to the positioning results obtained in all those time instants in which at least two V2V range measurements were available on all the vehicles.

DISCUSSION
The UWB ranging data characterization shows that the system performance has probably been affected by the presence of NLOS measurements, which caused the presence of outliers (and, more in general a lighter tail in the ranging error distribution): outliers were remarkably more frequent in the range measurements of the V2V Pozyx networks than on the V2I TimeDomain one. This is probably also partially caused by the possibility of discarding unreliable measurements in the TimeDomain devices, which surely helped reducing the presence of outliers in such dataset.
For both Pozyx and TimeDomain devices the ranging error is at decimeter level, despite, again, being significantly worse in the Pozyx case. Anyway, the obtained results from this point of view are quite consistent with expectations given the nominal technical specifications of such devices.
The availability of UWB range measurements can probably be considered the critical point, both in the ranging and in the successive positioning analysis: Fig. 5 and Fig. 9 show that the UWB ranging success rate quickly decreases when increasing the distance between vehicles (lower than 10% at 40 m). This observation shows that, despite the maximum UWB range measurement is larger than 100 m, the considered UWB communications are effective only for relatively short distances between the vehicles. Nevertheless, the main case of interest in determining the relative distance and position between vehicles is clearly for short distances between the vehicles.
Overall, when UWB measurements are available, according to the results shown in this paper, they can be considered quite reliable and useful for determining the distances between vehicles.
For what concerns the integration of vision and UWB ranging (Section 4), the proposed approach showed a quite good relative positioning performance, at sub-meter both along the heading direction and its transverse. While this kind of performance along the heading direction may be considered quite acceptable in real automotive applications, the error along the transverse direction is clearly unsatisfactory for autonomous driving applications (e.g. insufficient for properly ensuring the correct lateral distance between cars in urban canyons).
When dealing with the car absolute positioning problem, submeter error was obtained for the car connected with the V2I UWB network (observations quite similar to those mentioned in previous case may be associated to this kind of performance as well), instead, V2V positioning led to meter level 2D positioning error.
As previously mentioned, UWB range measurement availability seems to be a critical factor for the robustness of the positioning performance: the performance is quite consistent when a sufficient number of UWB measurements are available, however, this condition is satisfied only in certain time intervals (mostly when vehicles are quite close to each other).
As of submitting the paper, the analysis and evaluation of the results have not been completed, but we see no problems to completing this part very soon.

CONCLUSIONS
This work showed results of an experiment aimed at assessing the performance of collaborative vehicle navigation. The concept is that sharing navigation data by a group of cooperating vehicles can result in improved vehicle navigation compared to individual navigation solutions. The prerequisite for collaborative navigation is the availability of range data between vehicles and then communication for data sharing. Sensors used on modern assisted and autonomous vehicles provide several ways for range measurements between vehicles, including RADAR, LiDAR, optical camera, etc. The communication is also available in several formats, such as V2V, V2I or V2X. In our effort, we only focused on the positioning aspect, and thus the vehicle dynamics were not considered at all. In the conducted test, we used UWB technology for range measurements with V2V and V2I communication models. In addition, we have tested the feasibility of using a single camera to support the ranging by integrating it to the UWB data.
First, the UWB system performance was analyzed. The results have indicated that the prototype system could provide a good tool for our testing, but the availability of ranging data varied over a larger range, resulting in occasional gaps in the measurements. In other words, the system was not robust, but when data was available the ranging accuracy was consistent and thus could adequately support the objectives of the tests.
The experiences have demonstrated that the collaborative navigation approach can improve the positioning of the vehicles. Using the V2V model, as expected, the travel direction accuracy has been consistently good, while in the lateral direction there was little or no again at all. In contrast, the V2I case produced more comparable accuracies in both directions. For a group of four vehicles, a good collaborative solution can be obtained if two vehicles had good positioning data, as with only one, the solution becomes ill posed against rotation. In the other way, having three vehicles, not along the same line, can provide good solutions for a larger group of vehicles. In terms of numbers, m or typically sub-meter accuracies have been achieved, which is considered adequate along the travel direction.