# Relative Positioning Method for UAVs Based on Multi-Source Information Fusion

Authors: He Song; Shaolin Hu; Qiliang Guo; Wenqiang Jiang
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

To improve the accuracy and reliability of the relative positioning for unmanned aerial vehicles (UAVs), a relative positioning method based on multi-source information fusion is proposed. An integrated positioning scheme is constructed by the Beidou Navigation Satellite System (BDS) receivers, Global Positioning System (GPS) receivers, Vision-Based Navigation System (VisNav), and Inertial Navigation System (INS). The BDS pseudorange relative difference equation, the GPS relative difference equation, the relative line-of-sight vector equation, and the INS measurement equation are established, respectively. The least squared (LS) method is used to realize information fusion, and the Gauss-Newton method is used to iteratively solve the relative position results. Finally, numerical simulation and result analysis are conducted with different sensor configurations. Simulation results show that BDS/INS/GPS/VisNav relative positioning result is obviously better than that of INS/GPS, INS/VisNav, and INS/GPS/VisNav, and the proposed method reduces the relative positioning errors and has a higher accuracy and excellent robustness. The research result is suitable for application scenarios with high navigation accuracy requirement such as AAR and intelligent swarm formation control.


---

## Body

## 1. Introduction

The relative positioning technology of UAVs is used to determine the relative position relationship between UAVs. In the processes of the coordinated formation flight [1] and automation aerial refueling (AAR) [2, 3], UAVs need to know each other’s relative navigation information such as relative position, relative attitude, and relative velocity. What is more, the relative navigation information is solved to be the first key problem [4, 5]. Therefore, in order to improve the coordination efficiency and capabilities of UAVs, researching some high-precision relative positioning methods is definitely crucial.At present, the sensors that measured the relative navigation information usually include radars, VisNav, BDS, GPS, INS, and optical devices [6–12]. For the relative navigation method based on radars [13–15], the external electromagnetic interference could affect the quality of the radar measurement data, thereby reducing the positioning accuracy. For the relative navigation method based on GPS [16, 17], due to the shortcomings of unstable navigation signals and susceptibility to external interference, the relative navigation positioning accuracy could be affected [18]. For the relative navigation method based on INS [19, 20], due to the accumulated deviation of measurement equipment such as gyroscopes and accelerometer, the relative navigation positioning errors were relatively bigger. For the relative navigation method based on VisNav [21], although it no longer relies on satellite navigation signals, due to the time delay in processing a large amount of data and the susceptibility to the influence and restriction of the external environment, the relative navigation accuracy could be reduced, and the method might even be inapplicable in long-distance situations [22, 23]. In order to improve the effect of the relative navigation, the navigation methods of integrated sensors were studied and discussed. Wang et al. designed a relative navigation scheme including VisNav/INS/Differential GPS (DGPS) and used a distributed filtering architecture to fuse information. Simulation results showed that VisNav/INS/DGPS relative navigation scheme had a higher accuracy compared with VisNav/INS and INS/DGPS schemes [24]. On the basis of GPS/INS, Wilson et al. designed an integrated scheme with infrared cameras and active infrared Light-Emitting Diodes (LEDs) to assist the formation flight positioning. Experimental results showed that the root mean square error (RMSE) of the relative position estimation is 1.2 m in the horizontal direction and 0.44 m in the vertical direction [25]. In order to eliminate the influence of the DGPS, Gross et al. designed a relative navigation scheme integrated with DGPS/INS/Ultra Wide Band (UWB) and used the relative measurement data of UWB devices to assist the restoration of ambiguity over the entire week. Simulation results showed that UWB assisted relative navigation scheme had higher precision and stability [26]. According to the abovementioned integrated navigation methods, on the one hand, the GPS, INS, VisNav, and other equipment are mostly used for combined positioning, and the BDS developed by China is not used. On the other hand, the Kalman filter (KF) method and its improvement methods, as well as particle filtering and robust filtering methods are mostly used for information fusion, the fusion method based on the least squares (LS) method is rare in the literature. Therefore, the BDS is combined with GPS, INS, and VisNav for relative positioning, and the least squares method is used for multi-source information fusion in this paper, which can further improve the relative positioning accuracy and reduce relative positioning errors.The structure of this paper is as follows: In section2, the BDS pseudorange relative difference equation, GPS relative difference equation, relative line-of-sight vector equation, and INS measurement equation are built; the LS method based on the Gauss-Newton iteration is used to calculate the relative position results. In section 3, computational environment and simulation parameters are set to conduct numerical simulation for different sensor configurations. Finally, the conclusions are drawn in section 4.


## 2. Relative Positioning Method Based on Multi-Source Information Fusion

In this paper, we mainly study the relative positioning method between UAVs, and the two involved UAVs are equipped with INS, BDS, GPS, and VisNav. INS, BDS and GPS are usually used to measure the velocity and position of a single UAV, and VisNav is used to obtain the relative velocity and position between UAVs. The sensor-measured information is transmitted to the airborne computer through the data link, and then the information fusion method is used to achieve the high-precision relative positioning results. The integrated scheme is designed as shown in Figure1.Figure 1
Schematic of relative positioning for UAVs.


### 2.1. Coordinate Transformation

In order to calculate the relative positioning results, we need to establish an auxiliary coordinate system. Considering the requirement of the relative positioning, we establish the geographic coordinate system and the coordinate system of the UAV, respectively. The body geographic coordinate systemS (OS-XSYSZS) is also known as the navigation coordinate system. We select the northeast celestial coordinate system as the navigation coordinate system. The origin O of the coordinate system is the mass center of the UAV, the Z-axis points to the zenith, the X-axis points to the northbound, and the Y-axis points to the eastbound. The body coordinate system b (Ob-XbYbZb) has the same origin as the body geographic coordinate system. We select the upper right coordinate system as the body coordinate system. The origin O of the coordinate system is the mass center of the UAV, the Z-axis points to the upward, the X-axis points to the forward direction, and the Y-axis points to the right side of the UAV.During the flight of the UAV, the attitude angle changes. INS can calculate the three-axis attitude angle. The three-axis attitude angle (φ, θ, and ψ) is the rotation angle around the X-axis, Y-axis, and Z-axis, respectively, and which are called roll angle, pitch angle, and yaw angle. The body coordinate system can be obtained by rotating the three-axis attitude angle of the navigation coordinate system. According to the rotation sequence of “Z-X-Y,” the coordinate transformation formula can be expressed [27, 28]:(1)xbybzbT=CSbψ,ϕ,θxsyszsT=cosθcosψ−sinϕsinθsinψcosθsinψ+sinϕsinθcosψ−cosϕsinθ−cosθsinψcosϕcosψsinϕsinθcosψ+sinϕcosθsinψsinθsinψ−sinϕcosθcosψcosϕcosθxsyszs.


### 2.2. BDS Pseudorange Relative Difference Measurement

In this section, we will establish the BDS pseudorange relative difference measurement model. The pseudorangeρASi between the UAV A and the satellite Si (i = 1,2, …, N) can be obtained [29].(2)ρASi=rASi+cδtrA−δtSi+εA1Si+εA2Si+εA3Si+εA4Si,where rASi is the distance between UAV A and the satellite Si, δtrA is the clock difference of BDS, δtSi is the clock difference of the satellite Si, εA1Si, εA2Si, εA3Si, and εA4Si are the observation noise errors of the ionosphere, the troposphere, the ephemeris, and BDS, respectively.Similarly, the pseudorangeρBSi between the UAV B and the satellite Si can be obtained.(3)ρBSi=rBSi+cδtrB−δtSi+εB1Si+εB2Si+εB3Si+εB4Si.Assuming that the flight environment of the UAV is not change, the difference between (2) and (3) can eliminate the common error term such as the ionospheric deviation, the troposphere deviation, and the ephemeris deviation; therefore, the difference ΔρABSi can be obtained.(4)ΔρABSi=ρASi−ρBSi=rASi−rBSi+cδtrA−δtrB+εAB4Si.Considering the close flight, the difference in the cosine of the direction of the vector from the satelliteSi to the UAV A and the UAV B is very small. We denote the relative position vector between UAVs as r⟶AB=xAB,yAB,zABT, and the space position vector of the satellite Si as s⟶i=xsi,ysi,zsiT. The unit vector of the vector from the satellite Si to the midpoint of UAVs is μ⟶si=s⟶i−1/2r⟶AB/s⟶i−1/2r⟶AB, then, we can get(5)rASi−rBSi≈μ⟶siTr⟶AB.The derivation process of (5) is as follows: Figure 2 shows the position vector chart of the satellite Si, UAV A, and UAV B. According to the vector chart of the parallelogram, we get(6)SiA⟶−SiB⟶=r⟶BA,SiA⟶+SiB⟶=SiC⟶.Figure 2
The position vector chart of the satelliteSi, UAV A, and UAV B.According to the result of (6), we get(7)SiA⟶SiA⟶−SiB⟶=SiA⟶⋅r⟶BA,SiB⟶SiA⟶−SiB⟶=SiB⟶⋅r⟶BA.According to the result of (7), we get(8)SiA⟶2−SiB⟶2=SiA⟶+SiB⟶r⟶BA,therefore, we can get(9)SiA⟶−SiB⟶=SiA⟶+SiB⟶SiA⟶+SiB⟶r⟶BA=SiA⟶+SiB⟶SiA⟶+SiB⟶⋅SiA⟶+SiB⟶SiA⟶+SiB⟶⋅r⟶BA,where(10)SiA⟶+SiB⟶SiA⟶+SiB⟶=SiA⟶2+SiB⟶2+2SiA⟶SiB⟶cosαSiA⟶+SiB⟶,where α is the angle between vector SiA⟶ and vector SiB⟶.Due to the UAVs is close flight, the angleα≈0, so(11)SiA⟶+SiB⟶SiA⟶+SiB⟶≈SiA⟶+SiB⟶SiA⟶+SiB⟶≈1.According to the result of (11), we get(12)SiA⟶−SiB⟶≈e⟶⋅r⟶BA.The vectorμ⟶si is the unit vector of the satellite Si to the midpoint of the UAVs, so, we get(13)rASi−rBSi≈μ⟶siTr⟶AB.According to the above results, we can obtain the (5).According to the result of (4) and (5), the single difference ΔρABSi between the satellite Si and UAVs can be obtained.(14)ΔρABSi=μ⟶siTr⟶AB+cδtrA−δtrB+εAB4Si.Similarly, the single differenceΔρABSj between the satellite Sj (j = 1, 2, …, N, i ≠ j) and UAVs is(15)ΔρABSj=μ⟶sjTr⟶AB+cδtrA−δtrB+εAB4Sj.Calculating the difference between (14) and (15) to eliminate the clock deviation, the double difference ΔΔρABSiSj of different UAV relatives to different satellite can be obtained.(16)ΔΔρABSiSj=ΔρABSi−ΔρABSj=μ⟶siT−μ⟶sjTr⟶AB+εAB4SiSj,where εAB4SiSj is the noise which is existed in the pseudorange double difference calculation of UAVs.


### 2.3. GPS Relative Difference Measurement

We denote the position vector output by the GPS of the UAV A and B asr⟶AGPS and r⟶BGPS, respectively; then, the relative position vector r⟶ABGPS can be obtained(17)r⟶ABGPS=r⟶AGPS−r⟶BGPS+w⟶AB=r⟶AB+w⟶AB,where w⟶AB=wx,wy,wzT is the measurement noise.


### 2.4. Relative Line-of-Sight Vector Measurement

The vision-based navigation system installed on the UAV is mainly used to measure the relative line-of-sight vector between UAVs. In this section, three characteristic light spots on the UAV A are observed by the vision-based navigation system installed on the UAV B. The measurement principle is shown in Figure3.Figure 3
The measurement principal of relative line-of-sight.The position vector of the characteristic light spot in the body coordinate system of the UAV A isp⟶i=xi,yi,ziT, (i = 1, 2, 3), the relative position vector between UAVs is r⟶AB=xAB,yAB,zABT, and then, the unit line-of-sight vector l⟶i can be expressed(18)l⟶i=CIBCAIr⟶i+η⟶AB=CABr⟶i+η⟶AB,r⟶i=r⟶AB+p⟶ir⟶AB+p⟶i=xi+xAB,yi+yAB,zi+zABTxi+xAB2+yi+yAB2+zi+zAB2,where η⟶AB=ηx,ηy,ηzT is the measurement noise.


### 2.5. INS Relative Measurement

The INS mainly includes three-axis gyroscope and three-axis accelerometer in this paper. The three-axis gyroscope is used to measure the rotational angular velocityω⟶=ωx,ωy,ωzT of the UAV, and the attitude angle (φ, θ, and ψ) can be obtained by integrating the measured angular velocity. The three-axis accelerometer is used to measure the linear acceleration a⟶=ax,ay,azT of the UAV, and the position can be obtained by integrating the measured acceleration twice. The relative position vector between UAVs A and B is as follows:(19)r⟶ABINS=∫tst∫t0tsa⟶Atdtdt−∫tst∫t0tsa⟶Btdtdt+ξ⟶AB=r⟶AB+ξ⟶AB,where t0, ts and t represent the integration time; ξ⟶AB=ξx,ξy,ξzT is the measurement noise.


### 2.6. Multi-Source Information Fusion Method Based on LS

The LS can find the best function match by minimizing the sum of squares of error. When applying LS for multi-source data fusion, the accuracy of each measurement value is usually required to be the same. Due to the different types of measurement equipment used in this paper, the measurement accuracy is also different. Therefore, the weighted least squares (WLS) method is used in this paper. The basic idea of WLS is to give a higher degree of trust to reliable data sources, so that the accuracy of the whole result is higher. According to the measurement models (16)–(20), the WLS is used to achieve information fusion and calculate the relative positioning results between UAVs. An objective function is defined as(20)JxAB,yAB,zAB=w1μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2+w2r⟶AB−r⟶ABGPS2+w3CABr⟶i−l⟶i2+w4r⟶AB−r⟶ABINS2,where w1, w2w3 and w4 are the weight value.Generally, if the error of the measured valuevi is small, the corresponding weight value vi should be large. According to the principle of maximum likelihood, when the weight of each measurement value is the inverse of the standard deviation of the measurement error, the maximum likelihood solution can be obtained, namely:(21)wi=1σi,where σi is the standard deviation of random error of measurement value νi. In this paper, the standard deviation σi can be replaced by the root mean value of the error between the measurement sample value and the sample mean value. Therefore, the weight of each measurement value can be expressed as:(22)wi=11/n−1∑i=1nvi−1/n∑i=1nvi,where n is the measurement sample number.The specific expressions of the three terms on the right side of (20) are as follows:(23)μ⟶SiT−μ⟶SjTr⟶AB−ΔΔρABSiSj2=xsi−xAB/2,ysi−yAB/2,zsi−zAB/2xsi−xAB/22+ysi−yAB/22+zsi−zAB/22−xsi−xAB/2,ysi−yAB/2,zsi−zAB/2xsi−xAB/22+ysi−yAB/22+zsi−zAB/22xAByABzAB−ΔΔρABSiSj2=2xsi−xABxAB+2ysi−yAByAB+2zsi−zABzAB2xsi−xAB2+2ysi−yAB2+2zsi−zAB2−2xsj−xABxAB+2ysj−yAByAB+2zsj−zABzAB2xsj−xAB2+2ysj−yAB2+2zsj−zAB2−ΔΔρABSiSj2,r⟶AB−r⟶ABGPS2=r⟶AB−r⟶ABGPSTr⟶AB−r⟶ABGPS=xAB−xABGPS2+yAB−yABGPS2+zAB−zABGPS2,(24)CABr⟶i−l⟶i2=CABr⟶i−l⟶iTCABr⟶i−l⟶i=r⟶iTCABTCABr⟶i−2l⟶iTCABr⟶i+η⟶AB+l⟶iTl⟶i=2−2l⟶iTCABr⟶i=2−2lixc11+liyc21+lizc31xi+xAB+lixc12+liyc22+lizc32yi+yAB+lixc13+liyc23+lizc33zi+zABxi+xAB2+yi+yAB2+zi+zAB2,where the rotation matrix CAB is(25)CAB=c11c12c13c21c22c23c31c32c33=cosθBcosψB−sinϕBsinθBsinψBcosθBsinψB+sinϕBsinθBcosψB−cosϕBsinθB−cosθBsinψBcosϕBcosψBsinϕBsinθBcosψB+sinϕBcosθBsinψBsinθBsinψB−sinϕBcosθBcosψBcosϕBcosθB⋅cosθAcosψA−sinϕAsinθAsinψAcosθAsinψA+sinϕAsinθAcosψA−cosϕAsinθA−cosθAsinψAcosϕAcosψAsinϕAsinθAcosψA+sinϕAcosθAsinψAsinθAsinψA−sinϕAcosθAcosψAcosϕAcosθA.The attitude angle (φA, θA, and ψA) and (φB, θB, and ψB) can be calculated by the measured information from the INS.(26)r⟶AB−r⟶ABINS2=r⟶AB−r⟶ABINSTr⟶AB−r⟶ABINSS=xAB−xABINS2+yAB−yABINS2+zAB−zABINS2.In order to obtain the minimum value of the objective functionJ, the following conditions need to be satisfied:(27)∂JxAB,yAB,zAB∂xAB,yAB,zAB=∂μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2∂xAB,yAB,zAB+∂CABr⟶i−l⟶i2∂xAB,yAB,zAB+∂r⟶AB−r⟶ABGPS2∂xAB,yAB,zAB+∂r⟶AB−r⟶ABINS2∂xAB,yAB,zAB=0.According to the result of equation (24)–(28), equation (29) is a nonlinear function and not solve its analytical solution directly, therefore, we can use the Gauss-Newton iteration method [30] to find the linear optimal solution.For the convenience of description, letJ1=μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2, J2=r⟶AB−r⟶ABGPS2, J3=CABr⟶i−l⟶i2, and J4=r⟶AB−r⟶ABINS2, and their partial derivative can be obtained, respectively.(28)∂J1∂xAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K12K3K1K2K3K1K3K3xsiysizsi−2L12L3L1L2L3L1L3L3xsjysjzsj−1K−1LxAB,∂J1∂yAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K1K2K3K22K3K2K3K3xsiysizsi−2L1L2K3L22K3L2L3K3xsjysjzsj−1K−1LyAB,∂J1∂zAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K1K3K3K2K3K3K32K3xsiysizsi−2L1L3K3L2L3K3L32K3xsjysjzsj−1K−1LzAB.where K1=2xsi−xAB, K2=2ysi−yAB, K3=2zsi−zAB, K=K12+K22+K32, L1=2xsj−xAB, L2=2ysj−yAB, L3=2zsj−zAB, and L=L12+L22+L32.(29)∂J2∂xAB=2M3lixc12+liyc22+lixc32M2+lixc13+liyc23+lixc33M3−lixc11+liyc21+lixc31M2−lixc11+liyc21+lixc31M3TM1M2M3,∂J2∂yAB=2M3−lixc11+liyc21+lixc31M1lixc11+liyc21+lixc31M1+lixc13+liyc23+lixc33M3−lixc11+liyc21+lixc31M3TM1M2M3∂J2∂zAB=2M3−lixc11+liyc21+lixc31M1−lixc11+liyc21+lixc31M2lixc11+liyc21+lixc31M1+lixc12+liyc22+lixc32M2TM1M2M3.,where M1=xAB+xi, M2=yAB+yi, M3=zAB+zi, M=M12+M22+M32.(30)∂J3∂xAB=2xAB−xABGPS,∂J3∂yAB=2yAB−yABGPS,∂J3∂zAB=2zAB−zABGPS,(31)∂J4∂xAB=2xAB−xABINS,∂J4∂yAB=2yAB−yABINS∂J4∂zAB=2zAB−zABINS.,According to the result of equation (30)–(33), we can get the Jacobian matrix H.(32)H=w1w2w3w4T∂J1∂xAB∂J1∂yAB∂J1∂zAB∂J2∂xAB∂J2∂yAB∂J2∂zAB∂J3∂xAB∂J3∂yAB∂J3∂zAB∂J4∂xAB∂J4∂yAB∂J4∂zAB,therefore, according to the Gauss-Newton iteration method, the relative positioning results of UAVs can be obtained.(33)r⟶ABk+1=r⟶ABk−HTH−1HT,k=0,1,2,⋯,where ζ⟶ is the iterative vectors, k is the iterative number.The complexity of the relative positioning calculation time is related to the LS method based on Gauss-Newton iteration used in this paper as shown in equation (33). Equation (33) is a process of matrix multiplication and the iterative number is k. Therefore, the complexity of the calculation time changes from O (n3) to O (kn3). In addition, in the process of the actual calculation, we can control the iterative number and the iteration vectors to ensure that the iterative calculation is convergent.


## 2.1. Coordinate Transformation

In order to calculate the relative positioning results, we need to establish an auxiliary coordinate system. Considering the requirement of the relative positioning, we establish the geographic coordinate system and the coordinate system of the UAV, respectively. The body geographic coordinate systemS (OS-XSYSZS) is also known as the navigation coordinate system. We select the northeast celestial coordinate system as the navigation coordinate system. The origin O of the coordinate system is the mass center of the UAV, the Z-axis points to the zenith, the X-axis points to the northbound, and the Y-axis points to the eastbound. The body coordinate system b (Ob-XbYbZb) has the same origin as the body geographic coordinate system. We select the upper right coordinate system as the body coordinate system. The origin O of the coordinate system is the mass center of the UAV, the Z-axis points to the upward, the X-axis points to the forward direction, and the Y-axis points to the right side of the UAV.During the flight of the UAV, the attitude angle changes. INS can calculate the three-axis attitude angle. The three-axis attitude angle (φ, θ, and ψ) is the rotation angle around the X-axis, Y-axis, and Z-axis, respectively, and which are called roll angle, pitch angle, and yaw angle. The body coordinate system can be obtained by rotating the three-axis attitude angle of the navigation coordinate system. According to the rotation sequence of “Z-X-Y,” the coordinate transformation formula can be expressed [27, 28]:(1)xbybzbT=CSbψ,ϕ,θxsyszsT=cosθcosψ−sinϕsinθsinψcosθsinψ+sinϕsinθcosψ−cosϕsinθ−cosθsinψcosϕcosψsinϕsinθcosψ+sinϕcosθsinψsinθsinψ−sinϕcosθcosψcosϕcosθxsyszs.


## 2.2. BDS Pseudorange Relative Difference Measurement

In this section, we will establish the BDS pseudorange relative difference measurement model. The pseudorangeρASi between the UAV A and the satellite Si (i = 1,2, …, N) can be obtained [29].(2)ρASi=rASi+cδtrA−δtSi+εA1Si+εA2Si+εA3Si+εA4Si,where rASi is the distance between UAV A and the satellite Si, δtrA is the clock difference of BDS, δtSi is the clock difference of the satellite Si, εA1Si, εA2Si, εA3Si, and εA4Si are the observation noise errors of the ionosphere, the troposphere, the ephemeris, and BDS, respectively.Similarly, the pseudorangeρBSi between the UAV B and the satellite Si can be obtained.(3)ρBSi=rBSi+cδtrB−δtSi+εB1Si+εB2Si+εB3Si+εB4Si.Assuming that the flight environment of the UAV is not change, the difference between (2) and (3) can eliminate the common error term such as the ionospheric deviation, the troposphere deviation, and the ephemeris deviation; therefore, the difference ΔρABSi can be obtained.(4)ΔρABSi=ρASi−ρBSi=rASi−rBSi+cδtrA−δtrB+εAB4Si.Considering the close flight, the difference in the cosine of the direction of the vector from the satelliteSi to the UAV A and the UAV B is very small. We denote the relative position vector between UAVs as r⟶AB=xAB,yAB,zABT, and the space position vector of the satellite Si as s⟶i=xsi,ysi,zsiT. The unit vector of the vector from the satellite Si to the midpoint of UAVs is μ⟶si=s⟶i−1/2r⟶AB/s⟶i−1/2r⟶AB, then, we can get(5)rASi−rBSi≈μ⟶siTr⟶AB.The derivation process of (5) is as follows: Figure 2 shows the position vector chart of the satellite Si, UAV A, and UAV B. According to the vector chart of the parallelogram, we get(6)SiA⟶−SiB⟶=r⟶BA,SiA⟶+SiB⟶=SiC⟶.Figure 2
The position vector chart of the satelliteSi, UAV A, and UAV B.According to the result of (6), we get(7)SiA⟶SiA⟶−SiB⟶=SiA⟶⋅r⟶BA,SiB⟶SiA⟶−SiB⟶=SiB⟶⋅r⟶BA.According to the result of (7), we get(8)SiA⟶2−SiB⟶2=SiA⟶+SiB⟶r⟶BA,therefore, we can get(9)SiA⟶−SiB⟶=SiA⟶+SiB⟶SiA⟶+SiB⟶r⟶BA=SiA⟶+SiB⟶SiA⟶+SiB⟶⋅SiA⟶+SiB⟶SiA⟶+SiB⟶⋅r⟶BA,where(10)SiA⟶+SiB⟶SiA⟶+SiB⟶=SiA⟶2+SiB⟶2+2SiA⟶SiB⟶cosαSiA⟶+SiB⟶,where α is the angle between vector SiA⟶ and vector SiB⟶.Due to the UAVs is close flight, the angleα≈0, so(11)SiA⟶+SiB⟶SiA⟶+SiB⟶≈SiA⟶+SiB⟶SiA⟶+SiB⟶≈1.According to the result of (11), we get(12)SiA⟶−SiB⟶≈e⟶⋅r⟶BA.The vectorμ⟶si is the unit vector of the satellite Si to the midpoint of the UAVs, so, we get(13)rASi−rBSi≈μ⟶siTr⟶AB.According to the above results, we can obtain the (5).According to the result of (4) and (5), the single difference ΔρABSi between the satellite Si and UAVs can be obtained.(14)ΔρABSi=μ⟶siTr⟶AB+cδtrA−δtrB+εAB4Si.Similarly, the single differenceΔρABSj between the satellite Sj (j = 1, 2, …, N, i ≠ j) and UAVs is(15)ΔρABSj=μ⟶sjTr⟶AB+cδtrA−δtrB+εAB4Sj.Calculating the difference between (14) and (15) to eliminate the clock deviation, the double difference ΔΔρABSiSj of different UAV relatives to different satellite can be obtained.(16)ΔΔρABSiSj=ΔρABSi−ΔρABSj=μ⟶siT−μ⟶sjTr⟶AB+εAB4SiSj,where εAB4SiSj is the noise which is existed in the pseudorange double difference calculation of UAVs.


## 2.3. GPS Relative Difference Measurement

We denote the position vector output by the GPS of the UAV A and B asr⟶AGPS and r⟶BGPS, respectively; then, the relative position vector r⟶ABGPS can be obtained(17)r⟶ABGPS=r⟶AGPS−r⟶BGPS+w⟶AB=r⟶AB+w⟶AB,where w⟶AB=wx,wy,wzT is the measurement noise.


## 2.4. Relative Line-of-Sight Vector Measurement

The vision-based navigation system installed on the UAV is mainly used to measure the relative line-of-sight vector between UAVs. In this section, three characteristic light spots on the UAV A are observed by the vision-based navigation system installed on the UAV B. The measurement principle is shown in Figure3.Figure 3
The measurement principal of relative line-of-sight.The position vector of the characteristic light spot in the body coordinate system of the UAV A isp⟶i=xi,yi,ziT, (i = 1, 2, 3), the relative position vector between UAVs is r⟶AB=xAB,yAB,zABT, and then, the unit line-of-sight vector l⟶i can be expressed(18)l⟶i=CIBCAIr⟶i+η⟶AB=CABr⟶i+η⟶AB,r⟶i=r⟶AB+p⟶ir⟶AB+p⟶i=xi+xAB,yi+yAB,zi+zABTxi+xAB2+yi+yAB2+zi+zAB2,where η⟶AB=ηx,ηy,ηzT is the measurement noise.


## 2.5. INS Relative Measurement

The INS mainly includes three-axis gyroscope and three-axis accelerometer in this paper. The three-axis gyroscope is used to measure the rotational angular velocityω⟶=ωx,ωy,ωzT of the UAV, and the attitude angle (φ, θ, and ψ) can be obtained by integrating the measured angular velocity. The three-axis accelerometer is used to measure the linear acceleration a⟶=ax,ay,azT of the UAV, and the position can be obtained by integrating the measured acceleration twice. The relative position vector between UAVs A and B is as follows:(19)r⟶ABINS=∫tst∫t0tsa⟶Atdtdt−∫tst∫t0tsa⟶Btdtdt+ξ⟶AB=r⟶AB+ξ⟶AB,where t0, ts and t represent the integration time; ξ⟶AB=ξx,ξy,ξzT is the measurement noise.


## 2.6. Multi-Source Information Fusion Method Based on LS

The LS can find the best function match by minimizing the sum of squares of error. When applying LS for multi-source data fusion, the accuracy of each measurement value is usually required to be the same. Due to the different types of measurement equipment used in this paper, the measurement accuracy is also different. Therefore, the weighted least squares (WLS) method is used in this paper. The basic idea of WLS is to give a higher degree of trust to reliable data sources, so that the accuracy of the whole result is higher. According to the measurement models (16)–(20), the WLS is used to achieve information fusion and calculate the relative positioning results between UAVs. An objective function is defined as(20)JxAB,yAB,zAB=w1μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2+w2r⟶AB−r⟶ABGPS2+w3CABr⟶i−l⟶i2+w4r⟶AB−r⟶ABINS2,where w1, w2w3 and w4 are the weight value.Generally, if the error of the measured valuevi is small, the corresponding weight value vi should be large. According to the principle of maximum likelihood, when the weight of each measurement value is the inverse of the standard deviation of the measurement error, the maximum likelihood solution can be obtained, namely:(21)wi=1σi,where σi is the standard deviation of random error of measurement value νi. In this paper, the standard deviation σi can be replaced by the root mean value of the error between the measurement sample value and the sample mean value. Therefore, the weight of each measurement value can be expressed as:(22)wi=11/n−1∑i=1nvi−1/n∑i=1nvi,where n is the measurement sample number.The specific expressions of the three terms on the right side of (20) are as follows:(23)μ⟶SiT−μ⟶SjTr⟶AB−ΔΔρABSiSj2=xsi−xAB/2,ysi−yAB/2,zsi−zAB/2xsi−xAB/22+ysi−yAB/22+zsi−zAB/22−xsi−xAB/2,ysi−yAB/2,zsi−zAB/2xsi−xAB/22+ysi−yAB/22+zsi−zAB/22xAByABzAB−ΔΔρABSiSj2=2xsi−xABxAB+2ysi−yAByAB+2zsi−zABzAB2xsi−xAB2+2ysi−yAB2+2zsi−zAB2−2xsj−xABxAB+2ysj−yAByAB+2zsj−zABzAB2xsj−xAB2+2ysj−yAB2+2zsj−zAB2−ΔΔρABSiSj2,r⟶AB−r⟶ABGPS2=r⟶AB−r⟶ABGPSTr⟶AB−r⟶ABGPS=xAB−xABGPS2+yAB−yABGPS2+zAB−zABGPS2,(24)CABr⟶i−l⟶i2=CABr⟶i−l⟶iTCABr⟶i−l⟶i=r⟶iTCABTCABr⟶i−2l⟶iTCABr⟶i+η⟶AB+l⟶iTl⟶i=2−2l⟶iTCABr⟶i=2−2lixc11+liyc21+lizc31xi+xAB+lixc12+liyc22+lizc32yi+yAB+lixc13+liyc23+lizc33zi+zABxi+xAB2+yi+yAB2+zi+zAB2,where the rotation matrix CAB is(25)CAB=c11c12c13c21c22c23c31c32c33=cosθBcosψB−sinϕBsinθBsinψBcosθBsinψB+sinϕBsinθBcosψB−cosϕBsinθB−cosθBsinψBcosϕBcosψBsinϕBsinθBcosψB+sinϕBcosθBsinψBsinθBsinψB−sinϕBcosθBcosψBcosϕBcosθB⋅cosθAcosψA−sinϕAsinθAsinψAcosθAsinψA+sinϕAsinθAcosψA−cosϕAsinθA−cosθAsinψAcosϕAcosψAsinϕAsinθAcosψA+sinϕAcosθAsinψAsinθAsinψA−sinϕAcosθAcosψAcosϕAcosθA.The attitude angle (φA, θA, and ψA) and (φB, θB, and ψB) can be calculated by the measured information from the INS.(26)r⟶AB−r⟶ABINS2=r⟶AB−r⟶ABINSTr⟶AB−r⟶ABINSS=xAB−xABINS2+yAB−yABINS2+zAB−zABINS2.In order to obtain the minimum value of the objective functionJ, the following conditions need to be satisfied:(27)∂JxAB,yAB,zAB∂xAB,yAB,zAB=∂μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2∂xAB,yAB,zAB+∂CABr⟶i−l⟶i2∂xAB,yAB,zAB+∂r⟶AB−r⟶ABGPS2∂xAB,yAB,zAB+∂r⟶AB−r⟶ABINS2∂xAB,yAB,zAB=0.According to the result of equation (24)–(28), equation (29) is a nonlinear function and not solve its analytical solution directly, therefore, we can use the Gauss-Newton iteration method [30] to find the linear optimal solution.For the convenience of description, letJ1=μ⟶siT−μ⟶sjTr⟶AB−ΔΔρABSiSj2, J2=r⟶AB−r⟶ABGPS2, J3=CABr⟶i−l⟶i2, and J4=r⟶AB−r⟶ABINS2, and their partial derivative can be obtained, respectively.(28)∂J1∂xAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K12K3K1K2K3K1K3K3xsiysizsi−2L12L3L1L2L3L1L3L3xsjysjzsj−1K−1LxAB,∂J1∂yAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K1K2K3K22K3K2K3K3xsiysizsi−2L1L2K3L22K3L2L3K3xsjysjzsj−1K−1LyAB,∂J1∂zAB=2K1K−L1LK2K−L2LK3K−L3LxAByABzAB−ΔΔρABSiSj⋅2K1K3K3K2K3K3K32K3xsiysizsi−2L1L3K3L2L3K3L32K3xsjysjzsj−1K−1LzAB.where K1=2xsi−xAB, K2=2ysi−yAB, K3=2zsi−zAB, K=K12+K22+K32, L1=2xsj−xAB, L2=2ysj−yAB, L3=2zsj−zAB, and L=L12+L22+L32.(29)∂J2∂xAB=2M3lixc12+liyc22+lixc32M2+lixc13+liyc23+lixc33M3−lixc11+liyc21+lixc31M2−lixc11+liyc21+lixc31M3TM1M2M3,∂J2∂yAB=2M3−lixc11+liyc21+lixc31M1lixc11+liyc21+lixc31M1+lixc13+liyc23+lixc33M3−lixc11+liyc21+lixc31M3TM1M2M3∂J2∂zAB=2M3−lixc11+liyc21+lixc31M1−lixc11+liyc21+lixc31M2lixc11+liyc21+lixc31M1+lixc12+liyc22+lixc32M2TM1M2M3.,where M1=xAB+xi, M2=yAB+yi, M3=zAB+zi, M=M12+M22+M32.(30)∂J3∂xAB=2xAB−xABGPS,∂J3∂yAB=2yAB−yABGPS,∂J3∂zAB=2zAB−zABGPS,(31)∂J4∂xAB=2xAB−xABINS,∂J4∂yAB=2yAB−yABINS∂J4∂zAB=2zAB−zABINS.,According to the result of equation (30)–(33), we can get the Jacobian matrix H.(32)H=w1w2w3w4T∂J1∂xAB∂J1∂yAB∂J1∂zAB∂J2∂xAB∂J2∂yAB∂J2∂zAB∂J3∂xAB∂J3∂yAB∂J3∂zAB∂J4∂xAB∂J4∂yAB∂J4∂zAB,therefore, according to the Gauss-Newton iteration method, the relative positioning results of UAVs can be obtained.(33)r⟶ABk+1=r⟶ABk−HTH−1HT,k=0,1,2,⋯,where ζ⟶ is the iterative vectors, k is the iterative number.The complexity of the relative positioning calculation time is related to the LS method based on Gauss-Newton iteration used in this paper as shown in equation (33). Equation (33) is a process of matrix multiplication and the iterative number is k. Therefore, the complexity of the calculation time changes from O (n3) to O (kn3). In addition, in the process of the actual calculation, we can control the iterative number and the iteration vectors to ensure that the iterative calculation is convergent.


## 3. Numerical Simulation and Results Analysis

### 3.1. Simulation Parameters Initial

We select MATLAB 2019 version as simulation software. The computer configurations are as follows: Windows 10 operation system with 64 bit, the central processing unit (CPU) is Intel Xeon Silver 4215 R/Tray (8 c, 3.2 G), the graphics card is NVIDIA GeForce Titan RTX, 32G DDR4 memory, and the hard disk driver (HDD) is SSD Samsung 480 GB 2.5″SATA. All numerical simulation results shown in this paper are based on the above configuration environment.The two UAVs are equipped with INS, BDS, GPS, and VisNav, and we assume that the measurement noise of the measured sensors meets Gaussian white noise. The sensors configuration and simulation parameters are shown in Table1.Table 1
Sensors configuration and simulation parameters.
SensorsParametersValueGyroscopeConstant drift0.2 (°)/hAngle random walk0.01 (°)/hAccelerometerConstant bias100μgAcceleration random walk15μg/hBDSPseudorange measurement accuracy [31]0.3 mGPSPositioning accuracy5 mVISIONStandard deviations3500μradWe select the three characteristic light spots and all spots are assumed to be visible. The locations of all spots are given in Table2.Table 2
List of characteristic light spot locations.
Light spot orderXi/kmYi/kmZi/km102.25024.25003–3.7502.5The initial positions of UAVs A and B are (60 km, 20 km, and 16 km) and (52 km, 20 km, and 16 km), respectively, and their flight trajectory are given by(34)XA=30000+30000cost/600,YA=20000+30000cost/600,ZA=16000+16t,XB=20000+32000cost/600,YB=20000+36000cost/600,ZB=16000+12t.Simulation timet is 3000 s. The flight trajectory of UAV A and UAV B is shown in Figure 4. The true relative position change curve between UAVs and the true relative position change curve in the X-axis, Y-axis, and Z-axis directions are shown in Figures 5 and 6.Figure 4
Flight trajectory of UAV A and B: The true relative position.Figure 5
The true relatives position.Figure 6
The triaxial component of true relative position.


### 3.2. Simulation Result and Analysis

Figure7 shows the change curve and errors curve of the true value and the solved value of the relative position in the X-axis, Y-axis, and Z-axis directions. It can be seen that the method proposed in this paper can accurately calculate the relative position of the UAVs.Figure 7
Comparison of relative trajectories. (a) Relative position inX direction, (b) relative position error in X direction, (c) relative position in Y direction, (d) relative position error in Y direction, (e) relative position in Z direction, and (f) relative position error in Z direction.
(a)(b)(c)(d)(e)(f)Figure8 shows the convergence curve of the iteration calculation. It can be seen that after 30 iterations, the iteration error starts to convergence to the error reference value, which means that the LS based on Gauss-Newton iteration can quickly calculate the relative positioning results.Figure 8
The convergence curve of the iteration calculation.The relative positioning results in theX-axis, Y-axis, and Z-axis directions using the INS/GPS, INS/VisNav, INS/GPS/VisNav, and BDS/INS/GPS/VisNav relative positioning scheme are showed in Figures 9–11.Figure 9
Comparison of relative trajectories in (X) direction. (a) Relative positions, (b) relative position errors.
(a)(b)Figure 10
Comparison of relative trajectories in (Y) direction. (a) Relative positions, (b) relative position errors.
(a)(b)Figure 11
Comparison of relative trajectories in (Z) direction. (a) Relative positions, (b) relative position errors.
(a)(b)From Figures9–11, it can be seen that the BDS/INS/GPS/VisNav relative positioning scheme is the best among the above relative positioning schemes, the next one is the INS/GPS/VisNav relative positioning scheme, and the effect of relative positioning using the INS/GPS or INS/VisNav relative positioning scheme is the worst.Tables3 and 4 show the RMSE and the average solved time of the relative positioning results, respectively. From Tables 3 and 4, it can be seen that the BDS/INS/GPS/VisNav relative positioning scheme has the highest accuracy and robustness among the above relative positioning schemes, although its average solved time is the longest, it still satisfies the allowable time error range compared to the other two schemes. Therefore, the relative positioning method based on multi-source information fusion in this paper is feasible and effective.Table 3
Comparison of relative positioning results.
Relative positioning errorsRMSE/mThe method proposed (BDS/INS/GPS/VisNav)INS/GPS/VisNavINS/VisNavINS/GPSX21.845925.067529.830739.9574Y24.670925.149129.643640.1866Z24.408024.811929.869540.0397Table 4
Average solved time of relative positioning.
The method proposed (BDS/INS/GPS/VisNav)INS/GPS/VisNavINS/VisNavINS/GPSAverage solved time/s6.7785.4764.1893.252


## 3.1. Simulation Parameters Initial

We select MATLAB 2019 version as simulation software. The computer configurations are as follows: Windows 10 operation system with 64 bit, the central processing unit (CPU) is Intel Xeon Silver 4215 R/Tray (8 c, 3.2 G), the graphics card is NVIDIA GeForce Titan RTX, 32G DDR4 memory, and the hard disk driver (HDD) is SSD Samsung 480 GB 2.5″SATA. All numerical simulation results shown in this paper are based on the above configuration environment.The two UAVs are equipped with INS, BDS, GPS, and VisNav, and we assume that the measurement noise of the measured sensors meets Gaussian white noise. The sensors configuration and simulation parameters are shown in Table1.Table 1
Sensors configuration and simulation parameters.
SensorsParametersValueGyroscopeConstant drift0.2 (°)/hAngle random walk0.01 (°)/hAccelerometerConstant bias100μgAcceleration random walk15μg/hBDSPseudorange measurement accuracy [31]0.3 mGPSPositioning accuracy5 mVISIONStandard deviations3500μradWe select the three characteristic light spots and all spots are assumed to be visible. The locations of all spots are given in Table2.Table 2
List of characteristic light spot locations.
Light spot orderXi/kmYi/kmZi/km102.25024.25003–3.7502.5The initial positions of UAVs A and B are (60 km, 20 km, and 16 km) and (52 km, 20 km, and 16 km), respectively, and their flight trajectory are given by(34)XA=30000+30000cost/600,YA=20000+30000cost/600,ZA=16000+16t,XB=20000+32000cost/600,YB=20000+36000cost/600,ZB=16000+12t.Simulation timet is 3000 s. The flight trajectory of UAV A and UAV B is shown in Figure 4. The true relative position change curve between UAVs and the true relative position change curve in the X-axis, Y-axis, and Z-axis directions are shown in Figures 5 and 6.Figure 4
Flight trajectory of UAV A and B: The true relative position.Figure 5
The true relatives position.Figure 6
The triaxial component of true relative position.


## 3.2. Simulation Result and Analysis

Figure7 shows the change curve and errors curve of the true value and the solved value of the relative position in the X-axis, Y-axis, and Z-axis directions. It can be seen that the method proposed in this paper can accurately calculate the relative position of the UAVs.Figure 7
Comparison of relative trajectories. (a) Relative position inX direction, (b) relative position error in X direction, (c) relative position in Y direction, (d) relative position error in Y direction, (e) relative position in Z direction, and (f) relative position error in Z direction.
(a)(b)(c)(d)(e)(f)Figure8 shows the convergence curve of the iteration calculation. It can be seen that after 30 iterations, the iteration error starts to convergence to the error reference value, which means that the LS based on Gauss-Newton iteration can quickly calculate the relative positioning results.Figure 8
The convergence curve of the iteration calculation.The relative positioning results in theX-axis, Y-axis, and Z-axis directions using the INS/GPS, INS/VisNav, INS/GPS/VisNav, and BDS/INS/GPS/VisNav relative positioning scheme are showed in Figures 9–11.Figure 9
Comparison of relative trajectories in (X) direction. (a) Relative positions, (b) relative position errors.
(a)(b)Figure 10
Comparison of relative trajectories in (Y) direction. (a) Relative positions, (b) relative position errors.
(a)(b)Figure 11
Comparison of relative trajectories in (Z) direction. (a) Relative positions, (b) relative position errors.
(a)(b)From Figures9–11, it can be seen that the BDS/INS/GPS/VisNav relative positioning scheme is the best among the above relative positioning schemes, the next one is the INS/GPS/VisNav relative positioning scheme, and the effect of relative positioning using the INS/GPS or INS/VisNav relative positioning scheme is the worst.Tables3 and 4 show the RMSE and the average solved time of the relative positioning results, respectively. From Tables 3 and 4, it can be seen that the BDS/INS/GPS/VisNav relative positioning scheme has the highest accuracy and robustness among the above relative positioning schemes, although its average solved time is the longest, it still satisfies the allowable time error range compared to the other two schemes. Therefore, the relative positioning method based on multi-source information fusion in this paper is feasible and effective.Table 3
Comparison of relative positioning results.
Relative positioning errorsRMSE/mThe method proposed (BDS/INS/GPS/VisNav)INS/GPS/VisNavINS/VisNavINS/GPSX21.845925.067529.830739.9574Y24.670925.149129.643640.1866Z24.408024.811929.869540.0397Table 4
Average solved time of relative positioning.
The method proposed (BDS/INS/GPS/VisNav)INS/GPS/VisNavINS/VisNavINS/GPSAverage solved time/s6.7785.4764.1893.252


## 4. Conclusions

In this paper, we propose a relative positioning method that can integrate three different types of data such as satellite navigation data, visual navigation data, and inertial navigation data, which can significantly improve the positioning accuracy. The proposed method uses the LS to fuse these three different types of data. The simulation results show that comparing the relative positioning results of BDS/GPS and VISION/INS, the proposed method can reduce the RMSE and verify its effectiveness.In future work, the proposed method can be expanded to integrate more types of measurement data, such as Galileo system data, GLONASS system data, and other types of inertial navigation data, and is particularly suitable for the relative positioning of UAVs.



---

*Source: 3755861-2022-09-15.xml*