# A Cooperative Positioning Method of Connected and Automated Vehicles with Direction-of-Arrival and Relative Distance Fusion

Authors: Faan Wang; Liwei Xu; Xianjian Jin; Guodong Yin; Ying Liu
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

The rapid development of science and technology has created favorable conditions for Connected and Automated Vehicles (CAVs). Accurate localization is one of the fundamental functions of CAV to realize some advanced operations such as vehicle platooning. However, complicated urban traffic environments, such as the flyover, significantly influence vehicular positioning accuracy. The inability of CAV to accurately perceive self-localization information has become an urgent issue to be addressed. This paper proposed a novel cooperative localization method by introducing the relative Direction-of-Arrival (DOA) and Relative Distance (RD) into CAV to improve the localization accuracy of CAV in the multivehicle environment. First, the three-dimensional positioning error model of the host vehicle concerning adjacent vehicles in azimuth angle and pitch angle and intervehicle distances under the vehicle-to-vehicle communication was established. Second, two least-squares estimation algorithms, linear and nonlinear, are established to decrease the position errors by combining relative DOA and RD measurement information. To verify the proposed algorithm's effect, the PreScan-Simulink joint simulation is carried out. The results show that the host vehicle's localization accuracy by the proposed method can be improved by 25% compared with direct linearization. Besides, by combining relative DOA and relative RD measurement, the locating capability of the least-square-based nonlinear optimization method can be enhanced by 22%.


---

## Body

## 1. Introduction

Connected and automated vehicles (CAV) are promising methods worldwide to improve traffic safety, enhance driving comfort, and reduce energy consumption [1–3]. As an essential CAV function, accurate localization technology is all-important for CAV to realize high-grade performances such as vehicle platooning [4] and cooperative merging on highway ramps [5, 6] and has been widely studied. The most common localization technique used in CAV is realized by the global navigation satellite system (GNSS). However, the urban forest in downtowns, such as dense buildings, usually weakens the signal quality of GNSS and reduces the positioning precision of vehicles [7–9]. Besides, when cars drive as a group, the small intervehicle distance demands that the vehicular positioning accuracy is centimeter-level [10], which is much higher than the current GNSS road-positioning accuracy level.Various techniques have been proposed to improve vehicular positioning accuracy further to meet multivehicle cooperative control requirements [11–15]. Differential GNSS is proposed in [16] to achieve higher positioning accuracy by eliminating the common biases through a network of fixed reference stations. The real-time kinematic (RTK) technique is employed based on carrier phase measurements in [17] to realize centimeter-level accuracy. Moreover, higher vehicle positioning accuracy is achieved using the inertial navigation system (INS) in [18, 19]. Nonetheless, the methods mentioned above depend on the expensive infrastructures or onboard devices. Some scholars recently studied cooperative localization-based multi-UAVs’ technical possibility to simultaneously reduce costs and achieve lane-level positioning accuracy [20–22].The cooperative localization usually requires two additional pieces of information to enhance the positioning accuracy, i.e., Direction-of-Arrival (DOA) and Relative Distance (RD) between vehicles. Tomic et al. [23] developed a suboptimal estimator to calculate the location of multiple targets in a three-dimensional wireless sensor network by using DOA measurement, whose model is linearized. Wang et al. presented a DOA estimation method based on sparse Bayesian learning (SBL) to improve vehicle localization accuracy [24]. Another study proposed by Yin et al. measured the RD between the base station and target vehicle by combining Time Difference of Arrival (TDOA) and Angle of Arrival (AOA) [25]. Localization errors are reduced by solving the mean square error matrix.Also, some researchers attempt to combine the DOA and RD information by several fusion algorithms to improve the positioning accuracy. Rohani et al. [26] adopt the particle-filter probability-statistics method to conduct information fusion for the distance between the front and rear vehicles to increase relative position precision. Song et al. [27] develop a cooperative localization method based on the Bayesian framework, which uses GPS, distances, and azimuth between vehicles. The effects of vehicle speed, acceleration, and variable spacing on vehicle localization accuracy are analyzed. Naseri et al. [28] propose a Message Passing Hybrid Localization (MPHL) algorithm, a distributed algorithm based on information propagation and Markov chain Monte Carlo sampling. Joint estimation of DOA and RD is conducted to solve the cooperative distributed localization problem. Additionally, Yin et al. [29] propose a cooperative multisensor Edge Cloud Cooperative Localization (ECCL) method, which has relative distance and relative angle observations from neighbor nodes and absolute coordinate positioning systems (such as GPS) to obtain relevant information. A centralized cooperative fusion unit is built in the cloud to fuse the multiple localization information.Although the information of DOA, RD, or DOA-RD is fused by filter algorithms in [23–29], the filter algorithms may not meet real-time localization performance because of the computational complexity. This paper investigates the fusion of DOA-RD information without filter algorithms to achieve the cooperative localization of CAVs in the flyover environment. The localization framework based on least-squares DOA-RD information fusion is also established. The main contributions of this article are threefold. First, host vehicles' three-dimensional positioning error model concerning adjacent vehicles in azimuth angle and pitch angle and intervehicle distances was established. Second, to eliminate the position errors, a least-square-based linear localization method is designed by combining relative DOA and RD measurement information. Then, in light of the positioning results obtained by the linear localization method, a nonlinear calculation method fusing DOA- and RD-related communication between vehicles is proposed to optimize cars' positioning accuracy further. The presented cooperative localization method based on connected and automated vehicles on the localization accuracy is finally analyzed.The remainder of this paper is organized as follows: Section2 introduces the system model and the framework of cooperative localization. In Section 3, the calculation process of the cooperative localization optimization algorithm is presented. Section 4 gives the simulation and results. Section 5 concludes this paper.


## 2. System Model and Cooperative Localization

This paper will focus on the cooperative localization of multiple vehicles by using the DOA and RD information. This section will introduce the modeling of cooperative localization and its fundamental principle.


### 2.1. System Model

The localization in the multistack interchange is challenging due to the intricate positions of the vehicles. This paper will focus on this scenario. Figure1 shows a typical multistack interaction, where the cars drive on different levels.Figure 1
CAV driving on the road scene of a flyover.Figure2 is the simplified model of the cooperative localization for n vehicles. The position of the target vehicle is Vt=vxt,vyt,vztT, and the position of the assistance vehicle is Vi =  vxi,vyi,vziT. Note that only the vehicle with onboard sensors to measure the DOA and RD could be the assistance vehicle. Each car could obtain the basic localization information using the GNSS. The relative DOA between vehicles are azimuth angles α1,α2,α3,…,αi and pitch angles β1,β2,β3,…,βi, while the relative RD between vehicles is D12,D13,D23,…,Dij.Figure 2
Simplified diagram of multiple vehicles.There aren CAVs communicating with the target vehicle to measure the DOA and RD. The DOA and RD between target and assistance vehicles are(1)αi=arctanvyt−vyivxt−vxi,βi=arctanvzt−vzivxt−vxi2+vyt−vyi2,Dij=vxi−vxj2+vyi−vyj2+vzi−vzj2,i,j=1,2,…;i≠j,where αi, βi, and Dij are the azimuth, pitch, and intervehicle distance between the target and assistance vehicles. Besides, the azimuth angle, pitch angle, and distance between vehicles can be expressed as W=αi,βi,DijT. Since sensor noise exists, the system model is depicted as(2)α^iβ^iD^ij=αiβiDij+εαiεβiεDij,i,j=1,2,…,n;i≠j,where W^=α^i,β^i,D^ijT is the measurement value of sensors with noise and εαi∼0,σαi2, εβi∼0,σβi2, and εDij∼0,σDij2 are the measurement noise of azimuth angle, pitch angle, and intervehicle distance, respectively. They are Gaussian white noises and uncorrelated.


### 2.2. Cooperative Localization Principle

The main idea of cooperative localization is using the measured azimuth, pitch angle, and distances between vehicles to improve the positioning accuracy of all cars in the multivehicle system. Assuming that the DOA and RD errors are zero, we will derive the following vehicle positions. Since there exist errors by using GNSS for each vehicle, the position can be expressed as(3)v^xi=vxi+εvxi,v^yi=vyi+εvyi,v^zi=vzi+εvzi,where εvxi, εvyi, and εvzi represent the localization noise of GNSS, which are Gaussian white noises.According to the distance between the CAVs, the location of the assistance vehiclei can be calculated by the target vehicle t, i.e.,(4)vxi=vxt+Δxi,vyi=vyt+Δyi,vzi=vzt+Δzi,where Δxi, Δyi, and Δzi are the distances in three directions. Since the measured values are independent of each other, the measured location of vehicle i is(5)v^xi=vxt+Δxi+εvxi,v^yi=vyt+Δyi+εvyi,v^zi=vzt+Δzi+εvzi.If the locations are measured byn times, the position of the target CAV can be obtained as(6)vxt=1n∑i=1nv^xi−Δxi−εvxi,vyt=1n∑i=1nv^yi−Δyi−εvyi,vzt=1n∑i=1nv^zi−Δzi−εvzi.At the same time, the variances of the least-squares method are(7)Varv^xt=Varv^xi−Δxi−εvxin=σx2n,Varv^yt=Varv^yi−Δyi−εvyin=σy2n,Varv^zt=Varv^zi−Δzi−εvzin=σz2n.The positioning accuracy of the target vehicle is improved with the increase in the cooperation vehicle number.


## 2.1. System Model

The localization in the multistack interchange is challenging due to the intricate positions of the vehicles. This paper will focus on this scenario. Figure1 shows a typical multistack interaction, where the cars drive on different levels.Figure 1
CAV driving on the road scene of a flyover.Figure2 is the simplified model of the cooperative localization for n vehicles. The position of the target vehicle is Vt=vxt,vyt,vztT, and the position of the assistance vehicle is Vi =  vxi,vyi,vziT. Note that only the vehicle with onboard sensors to measure the DOA and RD could be the assistance vehicle. Each car could obtain the basic localization information using the GNSS. The relative DOA between vehicles are azimuth angles α1,α2,α3,…,αi and pitch angles β1,β2,β3,…,βi, while the relative RD between vehicles is D12,D13,D23,…,Dij.Figure 2
Simplified diagram of multiple vehicles.There aren CAVs communicating with the target vehicle to measure the DOA and RD. The DOA and RD between target and assistance vehicles are(1)αi=arctanvyt−vyivxt−vxi,βi=arctanvzt−vzivxt−vxi2+vyt−vyi2,Dij=vxi−vxj2+vyi−vyj2+vzi−vzj2,i,j=1,2,…;i≠j,where αi, βi, and Dij are the azimuth, pitch, and intervehicle distance between the target and assistance vehicles. Besides, the azimuth angle, pitch angle, and distance between vehicles can be expressed as W=αi,βi,DijT. Since sensor noise exists, the system model is depicted as(2)α^iβ^iD^ij=αiβiDij+εαiεβiεDij,i,j=1,2,…,n;i≠j,where W^=α^i,β^i,D^ijT is the measurement value of sensors with noise and εαi∼0,σαi2, εβi∼0,σβi2, and εDij∼0,σDij2 are the measurement noise of azimuth angle, pitch angle, and intervehicle distance, respectively. They are Gaussian white noises and uncorrelated.


## 2.2. Cooperative Localization Principle

The main idea of cooperative localization is using the measured azimuth, pitch angle, and distances between vehicles to improve the positioning accuracy of all cars in the multivehicle system. Assuming that the DOA and RD errors are zero, we will derive the following vehicle positions. Since there exist errors by using GNSS for each vehicle, the position can be expressed as(3)v^xi=vxi+εvxi,v^yi=vyi+εvyi,v^zi=vzi+εvzi,where εvxi, εvyi, and εvzi represent the localization noise of GNSS, which are Gaussian white noises.According to the distance between the CAVs, the location of the assistance vehiclei can be calculated by the target vehicle t, i.e.,(4)vxi=vxt+Δxi,vyi=vyt+Δyi,vzi=vzt+Δzi,where Δxi, Δyi, and Δzi are the distances in three directions. Since the measured values are independent of each other, the measured location of vehicle i is(5)v^xi=vxt+Δxi+εvxi,v^yi=vyt+Δyi+εvyi,v^zi=vzt+Δzi+εvzi.If the locations are measured byn times, the position of the target CAV can be obtained as(6)vxt=1n∑i=1nv^xi−Δxi−εvxi,vyt=1n∑i=1nv^yi−Δyi−εvyi,vzt=1n∑i=1nv^zi−Δzi−εvzi.At the same time, the variances of the least-squares method are(7)Varv^xt=Varv^xi−Δxi−εvxin=σx2n,Varv^yt=Varv^yi−Δyi−εvyin=σy2n,Varv^zt=Varv^zi−Δzi−εvzin=σz2n.The positioning accuracy of the target vehicle is improved with the increase in the cooperation vehicle number.


## 3. Cooperative Localization Algorithm Design

This section will introduce the linear and nonlinear least-squares optimization methods to improve the positioning accuracy by fusing the DOA and RD information between vehicles.


### 3.1. Vehicle Initial Position

The initial position of assistance vehicle is provided by the GNSS, i.e.,Vn0=vx10,vy10,vz10,…,vxn0,vyn0,vzn0T. According to the three-point positioning principle, the position of the target CAV, vt0=vxt0,vyt0,vzt0T, can be calculated by using the positions of two known CAVs and the DOA information between them. Then, if two assistance vehicles are selected, the position of the target CAV is calculated by(8)vxt0=vxi0tanαi−vxj0tanαj−vyi0+vyj0tanαi−tanαj,vyt0=tanαitanαjvxi0−vxj0−vyi0tanαj+vyj0tanαitanαi−tanαj,vzt0=12vzi+vzj−tanβivxt0−vxi2+vxt0−vxi2−tanβivxt0−vxi2+vxt0−vxi2.Therefore, the positions of all vehicles are(9)V0=vx10,vy10,vz10,…,vxn0,vyn0,vzn0,vxt0,vyt0,vzt0T.


### 3.2. Linear Least-Squares Optimization

Assuming that there aren CAVs in a flyover scene, the corresponding coordinate positions in three directions can be set as x⟶, y⟶, and z⟶, which can be represented as(10)x^y^z^=x⟶y⟶z⟶+εx⟶εy⟶εz⟶.The mean values ofεx⟶∼0,σx⟶2, εy⟶∼0,σy⟶2, and εx⟶∼0,σz⟶2 in the three-dimensional coordinates are zero while satisfying the uncorrelated white Gaussian noise.According to equations (2) and (10), we can define linearized solution observation value ω=αi,βi,Dij,x⟶,y⟶,z⟶T; then,(11)ω^=α^iβ^iD^ijx^y^z^=αiβiDijx⟶y⟶z⟶+εαiεβiεRijεx⟶εy⟶εz⟶,i,j=1,2,…,n;i≠j,where ω^=α^i,β^i,D^ij,x^,y^,z^T represents the actual measured value with noise.According to system model (1), it is necessary to perform a first-order Taylor expansion at the initial vehicle value V0 (the error of the second order and above is small and can be ignored). The linearization solution is as follows.Azimuth linearization is(12)αi≈ϕ1ivxi+ϕ2ivyi+ϕ3ivxt+ϕ4ivyt+ϕ5i,where(13)ϕ1i=1γvyt0−vyi0,ϕ2i=−1γvxt0−vxi0,ϕ3i=−1γvyt0−vyi0,ϕ4i=−1γvxt0−vxi0,ϕ5i=arctanvyt0−vyi0vxt0−vxi0,γ=vxt0−vxi02+vyt0−vyi02.Linearization of the pitch angle is(14)βi≈Ψ1ivxi+Ψ2ivyi+Ψ3ivzi+Ψ4ilxt+Ψ5ilyt+Ψ6ilzt+Ψ7i,where(15)Ψ1i=1λvxt−vxjvzi0−vzj0,Ψ2i=1λvyt0−vyj0vzi0−vzj0,Ψ3i=−1λvxt−vxj2+lyt−vyj2,Ψ4i=−1λvxt−vxjvzi0−vzj0,Ψ5i=−1λvyt0−vyj0vzi0−vzj0,Ψ6i=1λvxt−vxj2+vyt−vyj2,Ψ7i=arctanvzi0−vzj0vxt0−vxj02+vyt0−vyj02,A=vxt0−vxj02+vyt0−vyj02+vzi0−vzj02,B=vxt−vxj2+vyt−vyj2,λ=A∗B.Linearization of vehicle distance can be written as(16)Dij≈μ1ivxi+μ2ivyi+μ3ivzi+μ4ivxt+μ5ivyt+μ6ivzi.The positions of vehicles can be solved as(17)V=vx1,vy1,vz1,…,vxn,vyn,vzn,vxt,vyt,vztT.The relationship of the observation equationω^=α^i,β^i,D^ij,x^,y^,z^T is(18)ω^=ηV+ψ,where(19)η=ℛ…℧…⋱…ℱ…ℋ,ℛ=ϕ11ϕ1200……ϕ1iϕ2iϕ3i⋮…Ψ1iΨ1iΨ1i⋮…μ1iμ2iμ3i⋮,℧=000ϕ31ϕ410000ϕ3iϕ4i0000Ψ1iΨ5iΨ6iμ4iμ5iμ6i…00,F=…0μ1iμ2iμ3i1n001n…01n001n001n00,ℋ=μ4iμ5iμ6i0001n00000…1n00001n…1n000,ψ=ϕ51,Ψ71,0,…,0,ϕ5i,Ψ7i,0T.According to Taylor expansion coefficients and constant terms, least-squares equation (18) is solved as(20)V=ηTη−1ηTω^−ψ.The high-precision position coordinates of CAVs can be solved as(21)V=vx1,vy1,vz1,…,vxn,vyn,vzn,vxt,vyt,vztT.The position of the target CAV can be solved byVt=vxt,vyt,vztT.


### 3.3. Nonlinear Least-Squares Optimization

The nonlinear optimization algorithm mainly adopts the minimum residual sum of squares (RSS). LethVt be the functional relationship from the position Vt to the observation W. We can obtain the following equation:(22)W^=hVt+δ,where W^ is the measured value with noise and δ=εαi,εβi,εDijTi,j=1,2,…,n is Gaussian white noise. Assuming that the measurement error of each sensor is zero mean and Gaussian white noise is not correlated with each other, the positioning error formula is(23)δ=H⋅dvt,where dvt=dvxt,dvyt,dvztT and H=∂h/∂vt is the Jacobian matrix as follows:(24)H=vxt−vx1Dt1vyt−vy1Dt1vzt−vz1Dt1vy1−vytd12vxt−vx1d120ABd1Dt12.Also,(25)Α=−vxt−vx1∗vzt−vz1Dt12∗d1,Β=−vyt−vy1∗vzt−vz1Dt12∗d1,d1=vxt−vx12+vyt−vy12.Among them, according toW^=hvxt+δ, when the value of W^−hvxt tends to 0, the residual δ reaches a minimum. When W^−hvxt2 achieves the minimum value, the optimal state quantity is obtained, which can be calculated by using the Gauss–Newton iteration method:(26)W^=HjΔvxtj+hvxtj,Δvxtj=HjTHj−1HjTW^−hvxtj,vxt+1j=vxtj+Δvxtj,where j is the number of iterations.According to the estimation of the least-squares method, the root mean square error (RMSE) matrix is obtained:(27)EdvxtdvxtT=HTH−1HTQHHTH−1,where EdvxtdvxtT, Q is the variance error matrix, and Q=diagσεαi2,σεβi2,σεDij2i,j=1,2,…,n， where σεαi, σεβi, and σεDij are the standard deviation of the error for the azimuth angle, pitch angle, and distance between vehicles. The azimuth angle obtains the initial value of the target CAV V0=vx10,vy10,vz10T, which is treated as the starting point of the least-squares iteration. By the Newton–Raphson method, Δvxt is continuously iterated until W^−hVxt2 reaches the minimum value. The target localization can be obtained as the optimal solution.


## 3.1. Vehicle Initial Position

The initial position of assistance vehicle is provided by the GNSS, i.e.,Vn0=vx10,vy10,vz10,…,vxn0,vyn0,vzn0T. According to the three-point positioning principle, the position of the target CAV, vt0=vxt0,vyt0,vzt0T, can be calculated by using the positions of two known CAVs and the DOA information between them. Then, if two assistance vehicles are selected, the position of the target CAV is calculated by(8)vxt0=vxi0tanαi−vxj0tanαj−vyi0+vyj0tanαi−tanαj,vyt0=tanαitanαjvxi0−vxj0−vyi0tanαj+vyj0tanαitanαi−tanαj,vzt0=12vzi+vzj−tanβivxt0−vxi2+vxt0−vxi2−tanβivxt0−vxi2+vxt0−vxi2.Therefore, the positions of all vehicles are(9)V0=vx10,vy10,vz10,…,vxn0,vyn0,vzn0,vxt0,vyt0,vzt0T.


## 3.2. Linear Least-Squares Optimization

Assuming that there aren CAVs in a flyover scene, the corresponding coordinate positions in three directions can be set as x⟶, y⟶, and z⟶, which can be represented as(10)x^y^z^=x⟶y⟶z⟶+εx⟶εy⟶εz⟶.The mean values ofεx⟶∼0,σx⟶2, εy⟶∼0,σy⟶2, and εx⟶∼0,σz⟶2 in the three-dimensional coordinates are zero while satisfying the uncorrelated white Gaussian noise.According to equations (2) and (10), we can define linearized solution observation value ω=αi,βi,Dij,x⟶,y⟶,z⟶T; then,(11)ω^=α^iβ^iD^ijx^y^z^=αiβiDijx⟶y⟶z⟶+εαiεβiεRijεx⟶εy⟶εz⟶,i,j=1,2,…,n;i≠j,where ω^=α^i,β^i,D^ij,x^,y^,z^T represents the actual measured value with noise.According to system model (1), it is necessary to perform a first-order Taylor expansion at the initial vehicle value V0 (the error of the second order and above is small and can be ignored). The linearization solution is as follows.Azimuth linearization is(12)αi≈ϕ1ivxi+ϕ2ivyi+ϕ3ivxt+ϕ4ivyt+ϕ5i,where(13)ϕ1i=1γvyt0−vyi0,ϕ2i=−1γvxt0−vxi0,ϕ3i=−1γvyt0−vyi0,ϕ4i=−1γvxt0−vxi0,ϕ5i=arctanvyt0−vyi0vxt0−vxi0,γ=vxt0−vxi02+vyt0−vyi02.Linearization of the pitch angle is(14)βi≈Ψ1ivxi+Ψ2ivyi+Ψ3ivzi+Ψ4ilxt+Ψ5ilyt+Ψ6ilzt+Ψ7i,where(15)Ψ1i=1λvxt−vxjvzi0−vzj0,Ψ2i=1λvyt0−vyj0vzi0−vzj0,Ψ3i=−1λvxt−vxj2+lyt−vyj2,Ψ4i=−1λvxt−vxjvzi0−vzj0,Ψ5i=−1λvyt0−vyj0vzi0−vzj0,Ψ6i=1λvxt−vxj2+vyt−vyj2,Ψ7i=arctanvzi0−vzj0vxt0−vxj02+vyt0−vyj02,A=vxt0−vxj02+vyt0−vyj02+vzi0−vzj02,B=vxt−vxj2+vyt−vyj2,λ=A∗B.Linearization of vehicle distance can be written as(16)Dij≈μ1ivxi+μ2ivyi+μ3ivzi+μ4ivxt+μ5ivyt+μ6ivzi.The positions of vehicles can be solved as(17)V=vx1,vy1,vz1,…,vxn,vyn,vzn,vxt,vyt,vztT.The relationship of the observation equationω^=α^i,β^i,D^ij,x^,y^,z^T is(18)ω^=ηV+ψ,where(19)η=ℛ…℧…⋱…ℱ…ℋ,ℛ=ϕ11ϕ1200……ϕ1iϕ2iϕ3i⋮…Ψ1iΨ1iΨ1i⋮…μ1iμ2iμ3i⋮,℧=000ϕ31ϕ410000ϕ3iϕ4i0000Ψ1iΨ5iΨ6iμ4iμ5iμ6i…00,F=…0μ1iμ2iμ3i1n001n…01n001n001n00,ℋ=μ4iμ5iμ6i0001n00000…1n00001n…1n000,ψ=ϕ51,Ψ71,0,…,0,ϕ5i,Ψ7i,0T.According to Taylor expansion coefficients and constant terms, least-squares equation (18) is solved as(20)V=ηTη−1ηTω^−ψ.The high-precision position coordinates of CAVs can be solved as(21)V=vx1,vy1,vz1,…,vxn,vyn,vzn,vxt,vyt,vztT.The position of the target CAV can be solved byVt=vxt,vyt,vztT.


## 3.3. Nonlinear Least-Squares Optimization

The nonlinear optimization algorithm mainly adopts the minimum residual sum of squares (RSS). LethVt be the functional relationship from the position Vt to the observation W. We can obtain the following equation:(22)W^=hVt+δ,where W^ is the measured value with noise and δ=εαi,εβi,εDijTi,j=1,2,…,n is Gaussian white noise. Assuming that the measurement error of each sensor is zero mean and Gaussian white noise is not correlated with each other, the positioning error formula is(23)δ=H⋅dvt,where dvt=dvxt,dvyt,dvztT and H=∂h/∂vt is the Jacobian matrix as follows:(24)H=vxt−vx1Dt1vyt−vy1Dt1vzt−vz1Dt1vy1−vytd12vxt−vx1d120ABd1Dt12.Also,(25)Α=−vxt−vx1∗vzt−vz1Dt12∗d1,Β=−vyt−vy1∗vzt−vz1Dt12∗d1,d1=vxt−vx12+vyt−vy12.Among them, according toW^=hvxt+δ, when the value of W^−hvxt tends to 0, the residual δ reaches a minimum. When W^−hvxt2 achieves the minimum value, the optimal state quantity is obtained, which can be calculated by using the Gauss–Newton iteration method:(26)W^=HjΔvxtj+hvxtj,Δvxtj=HjTHj−1HjTW^−hvxtj,vxt+1j=vxtj+Δvxtj,where j is the number of iterations.According to the estimation of the least-squares method, the root mean square error (RMSE) matrix is obtained:(27)EdvxtdvxtT=HTH−1HTQHHTH−1,where EdvxtdvxtT, Q is the variance error matrix, and Q=diagσεαi2,σεβi2,σεDij2i,j=1,2,…,n， where σεαi, σεβi, and σεDij are the standard deviation of the error for the azimuth angle, pitch angle, and distance between vehicles. The azimuth angle obtains the initial value of the target CAV V0=vx10,vy10,vz10T, which is treated as the starting point of the least-squares iteration. By the Newton–Raphson method, Δvxt is continuously iterated until W^−hVxt2 reaches the minimum value. The target localization can be obtained as the optimal solution.


## 4. Simulation Results and Discussion

### 4.1. Simulation Method

We use PreScan-Simulink joint simulation software to perform Monte Carlo simulations on the flyover driving scene and analyze the localization accuracy according to the RMSE. The localization accuracy in thex−y−z direction is defined as follows:(28)RMSEx=1M∑i=1M1N∑j=1Nvxit−vxit_true2,RMSEy=1M∑i=1M1N∑j=1Nvyit−vyit_true2,RMSEz=1M∑i=1M1N∑j=1Nvzit−vzit_true2.


### 4.2. Simulation Conditions

We set up a flyover scenario to conduct the simulation analysis. The initial positions, directions, and speeds of multi-CAVs have been given. The accuracy of the onboard sensors is shown in Table1.Table 1
Sensor measurement accuracy.
TypeAzimuth accuracy (°)Ranging accuracy (m)Self-localization accuracy (m)0.150.510


### 4.3. Simulation Results

We can only solve the target CAV's localization when using the relative direction information to obtain the RMSE of the target vehicle. The simulation results show that the single relative direction information cannot improve each connected vehicle's localization accuracy. The RMSE result of the target CAV is shown in Figure3, and the RMSE statistics of the target CAV are shown in Table 2.Figure 3
RMSE of the relative DOA solution for the target CAV.Table 2
RMSE statistics of direction-finding solution for target CAV.
x-label (m)y-label (m)z-label (m)RMSE8.09608.15329.7026In single relative direction information, the RMSE after linearization is solved by the least-squares method. The RMSE obtained by the linear optimization solution method is shown in Figure4, and the RMSE statistics are shown in Table 3.Figure 4
RMSE of linear optimization solution.Table 3
RMSE statistics for linear optimization to solve the target CAV.
x-label (m)y-label (m)z-label (m)RMSE6.3617.05717.3956When the target CAV's initial position is obtained by single relative direction information, the RMSE of the target CAV is obtained as shown in Figure5 through combining the least-squares method with the nonlinear optimization method, and the RMSE statistics are shown in Table 4.Figure 5
The RMSE of the downward nonlinear optimization solution.Table 4
RMSE statistics of the target CAV measuring downward nonlinear solution.
x-label (m)y-label (m)z-label (m)RMSE4.54754.63514.9571When the target CAV's initial position is obtained by relative-direction finding and ranging, the nonlinear optimization algorithm solves the RMSE of the target CAV. The simulation results are shown in Figure6, and the RMSE statistics are shown in Table 5.Figure 6
RMSE of nonlinear optimization solution under direction finding and ranging.Table 5
RMSE statistics of nonlinear optimization solution under direction finding and ranging.
x-label (m)y-label (m)z-label (m)RMSE0.61740.69350.8287


### 4.4. Analysis of Factors Affecting Localization Accuracy

Vehicle speed is one of the most important factors for road safety. Therefore, the reasonable target speeds of CAVs and the distance between the assistance CAVs are necessary to guarantee the flyover scenario's driving safety. The localization accuracies of the target CAV under different conditions are simulated and analyzed by using the linear least-squares fusion algorithm. The simulation results are shown in Figures7–9. In Figure 7, two assistance CAVs are assumed driving on the road with constant speeds of 30 km/h and the distance between them is 100 m. The velocity of target CAV is changed between 10 and 80 km/h. In Figure 8, the target CAV and the assistance CAVs are hypothesized to run along different roads and the relative speeds of assistance vehicles vary from 10 km/h to 80 km/h. The previous figure changes the distance between the assistance CAVs from 20 m to 200 m. Note that the cooperative positioning method proposed in this paper is based on the azimuth and the distance between the assistance CAVs. Therefore, similar to the spatial three-point positioning principle, at least two assistance CAVs are needed to complete the cooperative positioning of the host vehicle in the application.Figure 7
RMSE of speed change to the target CAV.Figure 8
RMSE of the speed change of the multivehicles to the target CAV.Figure 9
Effect of assistance CAV distance on target CAV localization accuracy.From Figures7 and 8, we can see that both the host vehicle speed and the relative speeds of assistance vehicles can affect the position accuracy of the host vehicle and the location accuracy is weakened with augments of these two factors. By observing Figure 9, one can get that the positioning error of the host vehicle decreases first and then increases and reaches the lowest point between 80 km/h and 120 km/h. When the distance between two assistance CAVs is 20 m, a significant positioning error occurs because the GNSS system works. However, with the functioning of the cooperative location method, the positioning error is gradually reduced and the positioning error is less than 5 m at between 80 km/h and 120 km/h. As the distance between the two cars grows, the localization accuracy of the target CAV decreases, possibly due to the phenomena of communication delay and data dropout in vehicle-to-vehicle wireless communication networks. Therefore, the changing trend of target vehicle positioning RMSE is finally shown in Figure 9.


### 4.5. Result Analysis

By Figures3–6, the average value of vehicle localization errors is calculated and a method to reduce the error percentage is proposed. When the self-localization accuracy of CAVs given by the simulation is 10 m, the result shows the following:(1)
When only relative DOA is employed to obtain the localization of the target CAV, the low-accuracy position values of CAVs are directly used for calculation. Due to the low accuracy of the comparative DOA method, the positioning error of the target CAV is about 9–12 m, unable to improve the localization accuracy of the CAVs.(2)
The least-squares method linearization optimization for solving the localization of the target CAV can be concluded that the target CAV's localization error is about 5–7 m, which improves the target's localization accuracy of CAV.(3)
Using the least-squares linearization optimization solution method combining relative DOA and relative RD measurement, the target CAV's localization value is about 4–5 m. Compared with direct linearization, the accuracy is improved by 25%.(4)
Using the nonlinear optimization solution method of the least-squares method combining relative DOA and relative RD measurement, the target CAV's localization value is about 3–4 m. Compared with the linear optimization solution method, the improvement of the least-squares method nonlinear optimization solution method is about 22%.(5)
The localization accuracy of the target CAV among the multi-CAVs decreases and increases as the distance of the assistance multi-CAVs increases. When the assistance CAV space is 100–120 m, the localization error reaches the minimum.


## 4.1. Simulation Method

We use PreScan-Simulink joint simulation software to perform Monte Carlo simulations on the flyover driving scene and analyze the localization accuracy according to the RMSE. The localization accuracy in thex−y−z direction is defined as follows:(28)RMSEx=1M∑i=1M1N∑j=1Nvxit−vxit_true2,RMSEy=1M∑i=1M1N∑j=1Nvyit−vyit_true2,RMSEz=1M∑i=1M1N∑j=1Nvzit−vzit_true2.


## 4.2. Simulation Conditions

We set up a flyover scenario to conduct the simulation analysis. The initial positions, directions, and speeds of multi-CAVs have been given. The accuracy of the onboard sensors is shown in Table1.Table 1
Sensor measurement accuracy.
TypeAzimuth accuracy (°)Ranging accuracy (m)Self-localization accuracy (m)0.150.510


## 4.3. Simulation Results

We can only solve the target CAV's localization when using the relative direction information to obtain the RMSE of the target vehicle. The simulation results show that the single relative direction information cannot improve each connected vehicle's localization accuracy. The RMSE result of the target CAV is shown in Figure3, and the RMSE statistics of the target CAV are shown in Table 2.Figure 3
RMSE of the relative DOA solution for the target CAV.Table 2
RMSE statistics of direction-finding solution for target CAV.
x-label (m)y-label (m)z-label (m)RMSE8.09608.15329.7026In single relative direction information, the RMSE after linearization is solved by the least-squares method. The RMSE obtained by the linear optimization solution method is shown in Figure4, and the RMSE statistics are shown in Table 3.Figure 4
RMSE of linear optimization solution.Table 3
RMSE statistics for linear optimization to solve the target CAV.
x-label (m)y-label (m)z-label (m)RMSE6.3617.05717.3956When the target CAV's initial position is obtained by single relative direction information, the RMSE of the target CAV is obtained as shown in Figure5 through combining the least-squares method with the nonlinear optimization method, and the RMSE statistics are shown in Table 4.Figure 5
The RMSE of the downward nonlinear optimization solution.Table 4
RMSE statistics of the target CAV measuring downward nonlinear solution.
x-label (m)y-label (m)z-label (m)RMSE4.54754.63514.9571When the target CAV's initial position is obtained by relative-direction finding and ranging, the nonlinear optimization algorithm solves the RMSE of the target CAV. The simulation results are shown in Figure6, and the RMSE statistics are shown in Table 5.Figure 6
RMSE of nonlinear optimization solution under direction finding and ranging.Table 5
RMSE statistics of nonlinear optimization solution under direction finding and ranging.
x-label (m)y-label (m)z-label (m)RMSE0.61740.69350.8287


## 4.4. Analysis of Factors Affecting Localization Accuracy

Vehicle speed is one of the most important factors for road safety. Therefore, the reasonable target speeds of CAVs and the distance between the assistance CAVs are necessary to guarantee the flyover scenario's driving safety. The localization accuracies of the target CAV under different conditions are simulated and analyzed by using the linear least-squares fusion algorithm. The simulation results are shown in Figures7–9. In Figure 7, two assistance CAVs are assumed driving on the road with constant speeds of 30 km/h and the distance between them is 100 m. The velocity of target CAV is changed between 10 and 80 km/h. In Figure 8, the target CAV and the assistance CAVs are hypothesized to run along different roads and the relative speeds of assistance vehicles vary from 10 km/h to 80 km/h. The previous figure changes the distance between the assistance CAVs from 20 m to 200 m. Note that the cooperative positioning method proposed in this paper is based on the azimuth and the distance between the assistance CAVs. Therefore, similar to the spatial three-point positioning principle, at least two assistance CAVs are needed to complete the cooperative positioning of the host vehicle in the application.Figure 7
RMSE of speed change to the target CAV.Figure 8
RMSE of the speed change of the multivehicles to the target CAV.Figure 9
Effect of assistance CAV distance on target CAV localization accuracy.From Figures7 and 8, we can see that both the host vehicle speed and the relative speeds of assistance vehicles can affect the position accuracy of the host vehicle and the location accuracy is weakened with augments of these two factors. By observing Figure 9, one can get that the positioning error of the host vehicle decreases first and then increases and reaches the lowest point between 80 km/h and 120 km/h. When the distance between two assistance CAVs is 20 m, a significant positioning error occurs because the GNSS system works. However, with the functioning of the cooperative location method, the positioning error is gradually reduced and the positioning error is less than 5 m at between 80 km/h and 120 km/h. As the distance between the two cars grows, the localization accuracy of the target CAV decreases, possibly due to the phenomena of communication delay and data dropout in vehicle-to-vehicle wireless communication networks. Therefore, the changing trend of target vehicle positioning RMSE is finally shown in Figure 9.


## 4.5. Result Analysis

By Figures3–6, the average value of vehicle localization errors is calculated and a method to reduce the error percentage is proposed. When the self-localization accuracy of CAVs given by the simulation is 10 m, the result shows the following:(1)
When only relative DOA is employed to obtain the localization of the target CAV, the low-accuracy position values of CAVs are directly used for calculation. Due to the low accuracy of the comparative DOA method, the positioning error of the target CAV is about 9–12 m, unable to improve the localization accuracy of the CAVs.(2)
The least-squares method linearization optimization for solving the localization of the target CAV can be concluded that the target CAV's localization error is about 5–7 m, which improves the target's localization accuracy of CAV.(3)
Using the least-squares linearization optimization solution method combining relative DOA and relative RD measurement, the target CAV's localization value is about 4–5 m. Compared with direct linearization, the accuracy is improved by 25%.(4)
Using the nonlinear optimization solution method of the least-squares method combining relative DOA and relative RD measurement, the target CAV's localization value is about 3–4 m. Compared with the linear optimization solution method, the improvement of the least-squares method nonlinear optimization solution method is about 22%.(5)
The localization accuracy of the target CAV among the multi-CAVs decreases and increases as the distance of the assistance multi-CAVs increases. When the assistance CAV space is 100–120 m, the localization error reaches the minimum.


## 5. Conclusion

This paper studies the localization accuracy of the CAV in the flyover scenario when the accurate localization information of the vehicle is hard to obtain. The fusion algorithm that assists the relative DOA and relative RD measurement of multi-CAVs for cooperative localization is also researched. Using nonlinear optimization estimation and linearized least-squares estimation algorithm, the simulation results show that the proposed method improves the localization accuracy of the target CAV in road scenes such as flyover without increasing infrastructure costs. It can also conclude that reducing the CAV speed in the multivehicle cooperative of the CAV can increase the localization accuracy of the vehicle and enable driving. Reducing the distance between the assistance CAVs can increase the localization accuracy of the target CAV. This method is used for the cooperative localization of multivehicles in the flyover road scene. It can also be used for multi-aircraft combined attacks or target tracking in aerospace.



---

*Source: 5340693-2022-01-05.xml*