# Multi-UAV Factor Graph Colocation Algorithm

Authors: YueXia Zhang; Pengfei Jia
Journal: Wireless Communications and Mobile Computing
 (2022)

---

## Abstract

To tackle the difficulty and high cost of UAV positioning in a 5G disaster environment, this study proposes a multi-UAV coordinated positioning algorithm based on a factor graph (MUAV-FGC). A factor graph model is designed for coordinated positioning of the master-slave UAVs. In the positioning process, the influence of the speed and heading errors of the UAV on the positioning accuracy was analyzed, and the expected and variance values of the variables were used as the information transmitted between the factors. The error of the cooperative positioning algorithm was derived based on the factor graph. The coordinated positioning of multiple drones and the positioning accuracy improved. The simulation results show that, compared with the traditional extended Kalman filter (EKF) algorithm, the proposed MUAV-FGC positioning algorithm reduces the mean positioning and root mean square error by approximately 18.86% and 17.54%, respectively, thus proving its effectiveness.


---

## Body

## 1. Introduction

In recent years, various types of natural disasters affecting a wide area have occurred frequently, causing huge losses to life and property. With the continuous maturity of 5G technology and the research and development of 6G technology in the future [1], the advanced technology represented by UAV is more and more applied to the field of disaster prevention, reduction, and disaster relief and emergency management [2–4], which opens the “eye of the air” for disaster prevention, reduction, and disaster relief. In the event of a severe natural disaster, the drone can quickly cross the mountain and river barriers and get to the disaster site unaffected by the information interruptions and traffic obstructions; obtain the disaster information through photography, video, or other remote sensing methods; and provide accurate information for emergency rescue and relief in a timely manner [5, 6]. Thus, drone technologies present substantial technical advantages. When performing missions, drones need to be able to accurately determine their own location to successfully perform tasks such as disaster situation detection, search and rescue, and delivery of rescue supplies [7, 8] at the correct location in scenarios including nuclear disasters, where investigation and rescue by humans is not possible. Therefore, at present, obtaining information on the precise location of the UAV has become one of the most significant core technologies to be developed and improved [9, 10].The traditional methods of obtaining the position of the UAVs primarily include the global positioning system (GPS) [11–13] and inertial navigation and positioning system (INS) [14–16]. Although the GPS positioning system has the advantages of globalization and real time, the signal could be easily blocked by obstructions (such as high mountains and valleys and remote areas). Moreover, electromagnetic interference, which affects its position accuracy, can easily occur. If the drone cannot receive the GPS signals or if the frequency update of the GPS signal could not meet the requirements of the aircraft to update the information, the drone will lose its position. Conversely, INS has the advantages of small size, low cost, and independent positioning without relying on external information [17]. However, when an INS moves in a large area for a long time, the error of pose estimation will accumulate over time, leading to positioning, and the error increases sharply and deviates significantly from the actual position. When drones are used for disaster relief, they are often required to work for long hours in harsh environments, such as remote mountain areas. Thus, the GPS and INS systems cannot meet the requirements of drone positioning.At present, scholars at home and abroad have conducted a significant amount of research on the positioning of UAVs [18–20]. Padhy et al. [21] proposed a model for autonomous navigation and collision avoidance of drones in corridor environments where GPS cannot be located; detailed experiments were conducted in different corridors to prove the effectiveness of the experimental plan. Zhang et al. proposed an autonomous navigation and positioning method for UAVs based on electric field array detection [22]. Their experiments showed the positioning effect of the positioning algorithm, and the positioning error was within the range of the user error; therefore, this positioning algorithm had good usability and application value. Liu et al. [23] proposed an interactive multimodel positioning algorithm based on a robust Cuban Kalman filter and analyzed the performance of the UAV positioning and navigation algorithm. These positioning methods are only for single UAV positioning research, and there is no research on multi-UAV coordinated positioning. However, in disaster environments, multiple UAVs often work collaboratively; thus, coordinated positioning is required to improve the positioning accuracy. Therefore, the coordinated positioning of multiple drones has become a crucial issue that must be addressed.Regarding the research on multi-UAV colocation [24–29], Sivaneri and Gross [30] proposed a UAV colocation algorithm in a global navigation satellite system (GNSS) environment, which uses a platform where the satellite signals are interfered, except for on the ground level, for navigation. However, the limitations of the ground control station technology posed a problem. Meyer et al. [31] proposed a sigma point belief propagation positioning algorithm, which uses sampling points to characterize the state and can quickly perform collaborative positioning; this method is more suitable for indoor positioning and navigation and does not consider the use of measurement information devices such as satellites. However, a large number of coordinated drones lead to a large computational load, which remains an issue yet to be solved. Vetrella et al. [32] proposed a collaborative positioning method that integrates inertial, magnetometer, available satellite pseudo-range, cooperative drone position, and monocular camera information, which effectively improves the positioning performance of the drone swarm under the GPS-constrained conditions. However, a large number of sensors were used. Wan et al. [33] designed a dynamic nonparametric belief propagation algorithm to obtain the UAV location information, but the amount of calculation was large, and only a two-dimensional space was simulated, which is not suitable for large-scale cluster UAV use. Fan et al. [34] proposed a multi-AUV colocation method based on factor graphs and product algorithms. Experimental results show that, its positioning accuracy is better than those of the extended Kalman filter (EKF) and unscented Kalman filter (UKF) algorithms; however, it is a centralized processing method. The aforementioned algorithms consider the difficulty of UAV positioning under conditions such as limited GNSS signals. However, problems such as the inability to eliminate the constraints of the ground control station and the requirement of a large number of sensors remain, making such methods unsuitable for use in disaster environments. The human-machine coordinated positioning does not consider the influence of the speed and heading errors of the drone on the positioning accuracy during flight. In response to the above problems, this paper proposes a multi-UAV colocation algorithm based on a factor graph (MUAV-FGC). The main contributions of the algorithm are as follows:
(1)
Analyzed the problem of multi-UAV colocation error in a disaster environment, proposed a multi-UAV factor graph colocation algorithm, and designed a factor graph model of the master-slave UAV coordinated location(2)
Analyzed the influence of the UAV’s speed and heading error on the positioning accuracy during the positioning process. In the factor graph model, the variable expectation and variance are used as the information transmitted between the factors and deduced the factor graph-based cooperative positioning algorithm error(3)
Finally, a simulation experiment was established and compared with the traditional UAV EKF positioning algorithm. The results show that the MUAV-FGC algorithm proposed in this study significantly improves the positioning accuracy [35]


## 2. Analysis of Multi-UAV Coordinated Positioning

### 2.1. Multi-UAV Colocation Model

The cooperative positioning model of multiple UAVs is shown in Figure1. The Cartesian coordinate system is established with the UAV transmitting base station B as the origin. In the figure, MUAV is the main UAV, and UAV is the UAV to be positioned. In the positioning process, it is assumed that the position of the main UAV is accurate, that is, the main UAV is an airborne 5G base station, and the position information of the main UAV is obtained by 5G data communication between the UAV and the main UAV.Figure 1
Multi-UAV coordinated positioning model.The three-dimensional motion model of UAV can be expressed as(1)x=vcosβcosα,y=vcosβsinα,z=vsinβ,where x,y,z, v, and α,β represent the position coordinates of the slave drone, speed of the slave drone, and heading angles, respectively.If the speed of the drone at timek, the measured horizontal heading angle, and the vertical heading angle are vk, α, and β, respectively, then the position of the drone at time k is xk,yk,zk, and the position of the drone at time k−1 is xk−1,yk−1,zk−1, and Δt is sampling periods; thus, the following three-dimensional discrete motion model for the UAV can be obtained:
(2)xk=xk−1+vΔtcosβcosα,yk=yk−1+vΔtcosβsinα,zk=zk−1+vΔtsinβ.The slave and master drones communicate with each other to obtain the relative position information. The calculation equation for the relative distance information is(3)d=xk−xkm2+yk−ykm2+zk−zkm2,where xkm,ykm,zkm represents the position of the main drone at time k.


### 2.2. Positioning Error Analysis

According to the three-dimensional motion model of the slave drone, the speed and heading position of the slave drone measured at timea are as follows:
(4)xa−=xa−1+va−Δtcosαa−cosβa−,ya−=ya−1+va−Δtcosαa−sinβa−,za−=za−1+va−Δtsinαa−.va− and αa−,βa− represent the measured value of the drone’s speed and heading angles at time a; xa−, ya−, and za− represent the a priori estimate of the position in the x, y, and z directions at time a, respectively; xa−1ya−1 and za−1 represent the true value of the position in the x, y, and z directions at time a−1, respectively. The speed and heading angle can be further expressed as
(5)va−=va+v~a,αa−=αa+α~a,βa−=βa+β~a,where va represents the true speed of the slave drone at time a and v~a represents the speed error of the slave drone at time a. αa,βa represents the true heading angles from the drone at time a, andα~a,β~a represents the heading errors from the drone at time a. Substituting equation (5) into equation (4), we can obtain
(6)xa−=xa−1+Δtva+v~acosβa+β~acosαa+α~a,ya−=ya−1+Δtva+v~acosβa+β~asinαa+α~a,za−=za−1+Δtva+v~asinβa+β~a.On expanding equation (6), we obtain
(7)xa−=xa−1+Δtva+v~acosβacosβ~acosαacosα~a−cosβacosβ~asinαasinα~a−sinβasinβ~acosαacosα~a+sinβasinβ~asinαasinα~a,ya−=ya−1+Δtva+v~acosβacosβ~asinαacosα~a+cosβacosβ~acosαasinα~a−sinβasinβ~asinαacosα~a−sinβasinβ~acosαasinα~a,za−=za−1+Δtva+v~asinβacosβ~a+cosβasinβ~a.Assuming that the speed and heading errors are infinitesimal, then(8)sinα~a=α~a,cosα~a=1,v~aα~a=0,sinβ~a=β~,cosβ~a=1,v~aβ~a=0.Substituting equation (8) into equation (7), we can obtain
(9)xa−=xa−1+Δtvacosβacosαa−α~acosβasinαa−β~asinβacosαa+α~aβ~asinβasinαa+v~acosβacosαa,ya−=ya−1+Δtvacosβasinαa+α~acosβacosαa−β~asinβasinαa−α~aβ~asinβacosαa+v~acosβasinαa,za−=za−1+Δtvasinβa+β~cosβa+v~asinβa.From equation (9), it can be concluded that the coordinate position error is directly proportional to the speed error; it is also positively correlated with heading angle error. It can be seen that both the speed and heading error will have an impact on the positioning, resulting in unmanned machine positioning error increases. Therefore, based on the above analysis, this article introduces an analysis of the speed and heading angle errors in the positioning process.


## 2.1. Multi-UAV Colocation Model

The cooperative positioning model of multiple UAVs is shown in Figure1. The Cartesian coordinate system is established with the UAV transmitting base station B as the origin. In the figure, MUAV is the main UAV, and UAV is the UAV to be positioned. In the positioning process, it is assumed that the position of the main UAV is accurate, that is, the main UAV is an airborne 5G base station, and the position information of the main UAV is obtained by 5G data communication between the UAV and the main UAV.Figure 1
Multi-UAV coordinated positioning model.The three-dimensional motion model of UAV can be expressed as(1)x=vcosβcosα,y=vcosβsinα,z=vsinβ,where x,y,z, v, and α,β represent the position coordinates of the slave drone, speed of the slave drone, and heading angles, respectively.If the speed of the drone at timek, the measured horizontal heading angle, and the vertical heading angle are vk, α, and β, respectively, then the position of the drone at time k is xk,yk,zk, and the position of the drone at time k−1 is xk−1,yk−1,zk−1, and Δt is sampling periods; thus, the following three-dimensional discrete motion model for the UAV can be obtained:
(2)xk=xk−1+vΔtcosβcosα,yk=yk−1+vΔtcosβsinα,zk=zk−1+vΔtsinβ.The slave and master drones communicate with each other to obtain the relative position information. The calculation equation for the relative distance information is(3)d=xk−xkm2+yk−ykm2+zk−zkm2,where xkm,ykm,zkm represents the position of the main drone at time k.


## 2.2. Positioning Error Analysis

According to the three-dimensional motion model of the slave drone, the speed and heading position of the slave drone measured at timea are as follows:
(4)xa−=xa−1+va−Δtcosαa−cosβa−,ya−=ya−1+va−Δtcosαa−sinβa−,za−=za−1+va−Δtsinαa−.va− and αa−,βa− represent the measured value of the drone’s speed and heading angles at time a; xa−, ya−, and za− represent the a priori estimate of the position in the x, y, and z directions at time a, respectively; xa−1ya−1 and za−1 represent the true value of the position in the x, y, and z directions at time a−1, respectively. The speed and heading angle can be further expressed as
(5)va−=va+v~a,αa−=αa+α~a,βa−=βa+β~a,where va represents the true speed of the slave drone at time a and v~a represents the speed error of the slave drone at time a. αa,βa represents the true heading angles from the drone at time a, andα~a,β~a represents the heading errors from the drone at time a. Substituting equation (5) into equation (4), we can obtain
(6)xa−=xa−1+Δtva+v~acosβa+β~acosαa+α~a,ya−=ya−1+Δtva+v~acosβa+β~asinαa+α~a,za−=za−1+Δtva+v~asinβa+β~a.On expanding equation (6), we obtain
(7)xa−=xa−1+Δtva+v~acosβacosβ~acosαacosα~a−cosβacosβ~asinαasinα~a−sinβasinβ~acosαacosα~a+sinβasinβ~asinαasinα~a,ya−=ya−1+Δtva+v~acosβacosβ~asinαacosα~a+cosβacosβ~acosαasinα~a−sinβasinβ~asinαacosα~a−sinβasinβ~acosαasinα~a,za−=za−1+Δtva+v~asinβacosβ~a+cosβasinβ~a.Assuming that the speed and heading errors are infinitesimal, then(8)sinα~a=α~a,cosα~a=1,v~aα~a=0,sinβ~a=β~,cosβ~a=1,v~aβ~a=0.Substituting equation (8) into equation (7), we can obtain
(9)xa−=xa−1+Δtvacosβacosαa−α~acosβasinαa−β~asinβacosαa+α~aβ~asinβasinαa+v~acosβacosαa,ya−=ya−1+Δtvacosβasinαa+α~acosβacosαa−β~asinβasinαa−α~aβ~asinβacosαa+v~acosβasinαa,za−=za−1+Δtvasinβa+β~cosβa+v~asinβa.From equation (9), it can be concluded that the coordinate position error is directly proportional to the speed error; it is also positively correlated with heading angle error. It can be seen that both the speed and heading error will have an impact on the positioning, resulting in unmanned machine positioning error increases. Therefore, based on the above analysis, this article introduces an analysis of the speed and heading angle errors in the positioning process.


## 3. Multi-UAV Factor Graph Colocation Algorithm

### 3.1. Colocation Analysis of Factor Graph

Assuming that the UAV cannot receive the location information of the main UAV at timea, after a period of time, the location information of the main UAV is received at time k. At this time, the posterior position estimation from time a to time k is difficult to calculate, which easily leads to complicated speed and heading errors. Therefore, assuming that the amount of time from time a to time k is short, then
(10)v~=v~a=⋯=v~k,α~=α~a=⋯=α~k,β~=β~a=⋯=β~k.Accordingly, the errorδxa,δya,δza of the coordinate position at time a can be written as
(11)δxa=xa−−xa=Δtva−α~acosβasinαa−β~asinβacosαa+α~aβ~asinβasinαa+v~acosβacosαa,δya=ya−−ya=Δtvaα~acosβacosαa−β~asinβasinαa−α~aβ~asinβacosαa+v~acosβasinαa,δza=za−−za=Δtvaβ~acosβa+Δtv~asinβa,where xa−,ya−,za− represent a priori estimates of xa,ya,za, respectively. Because no position information is received from time a to time k, an a priori estimation is used to express the position as follows:
(12)xk−=xk+δxa+1+⋯+δxk,yk−=yk+δya+1+⋯+δyk,zk−=zk+δza+1+⋯+δzk.Substituting equation (11) into equation (12), we obtain equation (13) as
(13)xk−=xk+v~Δtcosβa+1cosαa+1⋯+cosβkcosαk−α~Δtva+1cosβa+1sinαa+1+⋯+Δtcosβksinαk−β~Δtsinβa+1cosαa+1+⋯Δtsinβkcosαk+α~β~Δtsinβa+1sinαa+1+⋯Δtsinβksinαk,yk−=yk+v~Δtcosβa+1sinαa+1⋯+cosβksinαk+α~Δtva+1cosβa+1cosαa+1+⋯+Δtcosβkcosαk−β~Δtsinβa+1sinαa+1+⋯Δtsinβksinαk−α~β~Δtsinβa+1cosαa+1+⋯Δtsinβkcosαk,zk−=zk+v~Δtsinβa+1+⋯+Δtsinβk+β~va+1Δtcosβa+1+⋯+vkΔtcosβk.For representation, the coordinate position errorδxk,δyk,δzk at time k can be written as
(14)δxk=v~δxkv~+α~δxkα~+β~δxkβ~,δyk=v~δykv~+α~δykα~+β~δykβ~,δzk=v~δzkv~+β~δzkβ~.It can also be rewritten as(15)δxkδykδzk=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~v~α~β~,v~α~β~=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~−1δxkδykδzk.When the master-slave drone is colocated, the position update needs to be calculated after subtracting the corresponding estimation error from the measured value of the speed and heading:(16)xk−=xk−1+vk−−v~k′Δtcosβk−−β~k−,cosαk−−α~k−,yk−=yk−1+vk−−v~k′Δtcosβk−−β~k−,sinαk−−α~k−,zk−=zk−1+vk−−v~k′Δtsinβk−−β~k−.According to equation (14) and the UAV’s three-dimensional discrete motion and relative distance model, make the information transfer between its variables can be obtained, as demonstrated in Figure 2.The rectangular and elliptical boxes in the figure represent function and factor nodes, respectively.Figure 2
Multi-UAV factor graph colocation algorithm.


### 3.2. Procedure of the Factor Graph Colocation Algorithm

As shown in Figure2, the multi-UAV factor graph colocation algorithm is used to calculate the mean and variance of the transfer between the variable and function node. The specific steps are as follows:


#### 3.2.1. Initialization

Initial state before colocation:(17)x^k−=x0,σx^k−2=σx02,y^k−=y0,σy^k−2=σy02,z^k−=z0,σz^k−2=σz02.


#### 3.2.2. Update of A Priori Estimatex^k−,y^k−,z^k−

At noninitial time, dead reckoning can be performed according to equation (2). μv and σv2 are the expected speed and variance, and μα,σα2 and μβ,σβ2 are the expected heading and variance, respectively. Because the speed is not related to the heading, covv,α=0covv,β=0, we can obtain the following formula:
(18)μx^k−=μx^k−1−+Δtμvcosμβ−σβ2cosβ2cosμα−σα2cosα2,μy^k−=μy^k−1−+Δtμvcosμβ−σβ2cosβ2sinμα−σα2sinα2,μz^k−=μz^k−1−+Δtμvsinμβ−σβ2sinβ2,σx^k−2=σx^k−12+cosμβΔtσv2−μvsinμβΔtσβ22sinμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·cosμα−σα2cosμα22+sinμασα2μv2cosμβ−σβ2cosμβ22,σy^k−2=σy^k−12+cosμβΔtσv2−μvsinμβΔtσβ22cosμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·sinμα−σα2sinμα22+cosμασα2μv2cosμβ−σβ2cosμβ22,σz^k−2=σz^k−12+sinμβΔtσv2−μvcosμβΔtσβ22.


#### 3.2.3. First Update ofx^k−,y^k−,z^k−

At this time,x′,z′,y′ after the coordinate transformation (Figure 2) have not been calculated, and the variable nodes xk,yk,zk and x^k−,y^k−,z^k− are equal:
(19)xk=x^k−,yk=y^k−,zk=z^k−.


#### 3.2.4. First Update ofx^k−,y^k−,z^k−

To avoid the collision of the coordinate axes of the master and slave drones and to prevent the resultant large calculation errors, the coordinates of the master and slave drones are converted once in the function node to obtainxk′,yk′,zk′ and xk′m,yk′m,zk′m.
(20)xk′=xkcosθycosθz−sinθxsinθysinθz−ykcosθxsinθz+zksinθycosθz+sinθxcosθysinθz,yk′=xkcosθysinθz+sinθxsinθycosθz+ykcosθxcosθz+zksinθysinθz−sinθxcosθycosθz,zk′=xk−cosθxsinθy+yksinθx+zkcosθxcosθy,xk′m=xkmcosθycosθz−sinθxsinθysinθz−ykmcosθxsinθz+zkmsinθycosθz+sinθxcosθysinθz,yk′m=xkmcosθysinθz+sinθxsinθycosθz+ykmcosθxcosθz+zkmsinθysinθz−sinθxcosθycosθz,zk′m=xkm−cosθxsinθy+ykmsinθx+zkmcosθxcosθy,where θx,θy, and θz are the angle of rotation along the x-, y-, and z-axes, respectively.


#### 3.2.5.Δxk,Δyk,Δzk Update

The role of function nodes B, C, and D is to convert the relative and absolute position information. Therefore, the function probability density of these function nodes transferred to nodeΔxk,Δyk,Δzk is
(21)NΔx,xk′m−μxk′,σxk2,NΔy,yk′m−μyk′,σyk2,NΔz,zk′m−μzk′,σzk2.Owing tod2=Δxk2+Δyk2+Δzk2, the probability density function passed from the function node E to the variable node can be expressed as
(22)NΔxk,±dk2−μΔxk2,μΔxk2σxk2+dk2σdk2dk2−μΔxk2,NΔyk,±dk2−μΔyk2,μΔyk2σyk2+dk2σdk2dk2−μΔyk2,NΔzk,±dk2−μΔzk2,μΔzk2σxk2+dk2σdk2dk2−μΔzk2.


#### 3.2.6. Second Update ofxk′,yk′,zk′

The probability density of the function nodes B, C, and D transfer function toxk′,yk′,zk′ is
(23)Nxk′,xk′m−μΔxk,σΔxk2,Nyk′,yk′m−μΔyk,σΔyk2,Nzk′,zk′m−μΔzk,σΔzk2.


#### 3.2.7. Second Update ofxk,yk,zk

After calculation and finishing, the updated function probability density ofxk,yk,zk is as follows:
(24)xk~Nxk,μxk,σxk2,yk~Nyk,μyk,σyk2,zk~Nzk,μzk,σzk2,1σxk2=1σx^k−2+1σxk′′2,1σyk2=1σy^k−2+1σyk′′2,1σzk2=1σz^k−2+1σzk′′2,μxk=σxk2μx^k−σx^k−2+μxk′′σxk′′2,μyk=σyk2μy^k−σy^k−2+μyk′′σyk′′2,μzk=σxk2μz^k−σz^k−2+μzk′′σzk′′2,where μxk′′,μyk′′,μzk′′ and σxk′′2,σyk′′2,σzk′′2 are the information transferred to xk,yk,zk after xk′,yk′,zk′ coordinate conversion. The coordinate conversion process is as follows:
(25)xkykzk=cosθycosθz−sinθxsinθysinθzcosθxsinθzsinθycosθz+sinθxcosθysinθzcosθysinθz+sinθxsinθycosθzcosθxcosθzsinθysinθz−sinθxcosθycosθz−cosθxsinθysinθxcosθxcosθy−1xk′yk′zk′.


### 3.3. Error Analysis of Factor Graph Colocation Algorithm

The error formula can be rewritten as(26)δxkδykδzk=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~v~α~β~,v~α~β~=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~−1δxkδykδzk.In the case of colocation, the position update needs to be calculated after subtracting the corresponding estimation error from the measured value of the speed and heading as follows:(27)xk−=xk−1+vk−−v~k′Δtcosβk−−β~k−cosαk−−α~k−,yk−=yk−1+vk−−v~k′Δtcosβk−−β~k−sinαk−−α~k−,zk−=zk−1+vk−−v~k′Δtsinβk−−β~k−,where v~k′=v~k−1′+v~k−1,v~k−1=0α~k′=α~k−1′+α~k−1,α~k−1=0β~k′=β~k−1′+β~k−1,β~k−1=0; therefore, the expectation and variance values can be expressed as follows:
(28)μv~k′=μv~k−1′+μv~k−1,σv~k′2=σv~k−1′2,μα~k′=μα~k−1′+μα~k−1,σα~k′2=σα~k−1′2,μβ~k′=μβ~k−1′+μβ~k−1,σβ~k′2=σβ~k−1′2.After receiving the measurement information of the main drone from the drone at timea and performing error estimation, δxav~=0, δxav~ is used as the starting point of δxkv~ for the next update. At this time, the expectation and variance of δxav~ are both 0. Subsequently, δxkv~ after time a can be recursively expressed as δxkv~=δxk−1v~+Δtcosβkcosαk; however, because the actual value of β,α cannot be determined, it is represented by β^,α^, that is,
(29)α^k=αk−−α~k′,μα^k=μαk−−μα~k′,σα^k2=σαk−2+σα~k′2,β^k=βk−−β~k′,μα^k=μβk−−μβ~k′,σβ^k2=σβk−2+σβ~k′2.Further, the expectation and variance ofδxkv~ can be expressed as
(30)μδxkv~=μδxk−1v~+Δtμcosβ^kμcosα^k,σδxkv~2=σδxk−1v~2+Δt2σcosβ^k2σcosαk2+σcosβ^k2μ2+σcosα^k2μcosβ^k2.Similarly, the expressions ofδykv~,δzkv~, δxkα~,δykα~,δzkα~, δxkβ~,δykβ~,δzkβ~ and the corresponding expectations and variances can be obtained as follows:
(31)δxkv~=δxk−1v~+Δtcosβ^kcosα^k,δykv~=δyk−1v~+Δtcosβ^ksinα^k,δzkv~=δzk−1v~+Δtsinβ^k,μδxkv~=μδxk−1v~+Δtμcosβ^kμcosα^k,σδxkv~2=σδxk−1v~2+Δt2σcosβ^k2σcosα^k2+σcosβ^k2μcosα^k2+σcosα^k2μcosβ^k2,μδykv~=μδyk−1v~+Δtμcosβ^kμsinα^k,σδykv~2=σδyk−1v~2+Δt2σcosβ^k2σsinα^k2+σcosβ^k2μsinα^k2+σsinα^k2μcosβ^k2,μδzkv~=μδzk−1v~+Δtμsinβ^k,σδzkv~2=σδzk−1v~2+Δt2σsinβ^k2,δxkα~=δxk−1α~−v^kΔtcosβ^sinα^,δykα~=δyk−1α~+v^kΔtcosβ^cosα^,δzkα~=0,μδxkα~=μδxk−1α~−Δtμcosβ^kμsinα^k,σδxkα~2=σδxk−1α~2+Δt2σcosβ^k2σsinsα^k2+σcosβ^k2μsinα^k2+σsinα^k2μcosβ^k2,μδykα~=μδyk−1α~+Δtμcosβ^kμcosα^k,σδykα~2=σδyk−1α~2+Δt2σcosβ^k2σcosα^k2+σcosβ^k2μcosα^k2+σcosα^k2μcosβ^k2,μδzkα~=0,σδzkα~2=0,δxkβ~=0,δykβ~=0,δzkβ~=δzk−1+v^kΔtcosβ^k,μδxkβ~=0,σδxkβ~2=0,μδykβ~=0,σδykβ~2=0,μδzkβ~=μδzk−1β~+Δtμv^kcosβ^k,σδzkα~2=σδzk−1β~2+Δt2σv^kcosβ^k2.Equation (32) presents the updated formula of v~k−,α~k−,β~k−.
(32)v~k−=v~k−1,α~k−=α~k−1,β~k−=β~k−1.If the slave drone does not receive the position information of the master drone at timek, then v~k−=v~kα~k−=α~kβ~k−=β~k, and the expected variance can be expressed as follows:
(33)μδxk=μyk−−μxk,σδxk2=σxk−2+σxk2,μδyk=μyk−−μyk,σδyk2=σyk−2+σyk2,μδzk=μzk−−μzk,σδzk2=σzk−2+σzk2.As shown in Figure2, the information of the variable node δxkv~ is obtained by the variable nodes δxk−1v~, α~k, and β~k through the function node, which can be expressed as Nδxkv~,μδxkv~,σδxkv~2. In addition, the information of the variable node δykv~ is obtained by the variable nodes δyk−1v~, α~k, and β~k through the function node, which can be expressed as Nδykv~,μδykv~,σδykv~2. Further, for the variable node δzkv~, its information is obtained by the variable nodes δzk−1v~, α~k, and β~k through the δyk−1α~α~kNδzkv~,μδzkv~,σδzkv~2 function node, which can be expressed as Nδzkv~,μδzkv~,σδzkv~2.The information of the variable nodeδxkα~ is obtained by δxk−1α~, α~k, and v~k through the function node, which can be expressed as Nδxkα~,μδxkα~,σδxkα~2; the information of the variable node δykα~ is obtained by δyk−1α~, α~k, and v~k through the function node, which can be expressed as Nδykα~,μδykα~,σδykα~2; and the information of the variable node δzkα~ is obtained by δzk−1α~, α~k, and v~k through the function node, which can be expressed as Nδzkα~,μδzkα~,σδzkα~2.Furthermore, the information of the variable nodeδxkβ~ is obtained by δxk−1β~, β~k, and v~k through the function node, which can be expressed as Nδxkβ~,μδxkβ~,σδxkβ~2; the information of the variable node δykβ~ is obtained by δyk−1β~, β~k, and v~k through the function node, which can be expressed as Nδykβ~,μδykβ~,σδykβ~2; and the information of the variable node δzkβ~ is obtained by δzk−1β~, β~k, and v~k through the function node, which can be expressed as Nδzkβ~,μδzkβ~,σδzkβ~2.The information passed from the function nodesv~k−1, α~k−1, and β~k−1 to variable nodes v~k, α~k, and β~k can be expressed as Nv~k,μv~k,σv~k2, Nα~k,μα~k,σα~k2, and Nβ~k,μβ~k,σβ~k2, respectively.The information transferred from the function nodes F, G, and H to the variable nodesδxk, δyk, and δzk can be represented as Nδxk,μδxk,σδxk2, Nδyk,μδyk,σδyk2, and Nδzk,μδzk,σδzk2, respectively. The information transferred from function node I to the variable node v~k can be represented as Nv~k+,μv~+,σv~+2, the information transferred to α~k can be represented as Nα~k+,μα~+,σα~+2, and the information transferred to β~k can be represented as Nβ~k+,μβ~+,σβ~+2.The a priori estimate ofv~k and the information passed from function node I to the variable node v~k can be combined to obtain the posterior estimate of v~k at time k:
(34)μv~k=σv~k2μv~k−σv~k−2+μv~k+σv~k+2,σv~k2=σv~k−2⋅σv~k+2σv~k−2+σv~k+2.Similarly, the posterior estimates ofα~k andβ~k are
(35)μα~k=σα~k2μα~k−σα~k−2+μα~k+σα~k+2,σα~k2=σα~k−2⋅σα~k+2σα~k−2+σα~k+2,μβ~k=σβ~k2μβ~k−σβ~k−2+μβ~k+σβ~k+2,σβ~k2=σβ~k−2⋅σβ~k+2σβ~k−2+σβ~k+2.


## 3.1. Colocation Analysis of Factor Graph

Assuming that the UAV cannot receive the location information of the main UAV at timea, after a period of time, the location information of the main UAV is received at time k. At this time, the posterior position estimation from time a to time k is difficult to calculate, which easily leads to complicated speed and heading errors. Therefore, assuming that the amount of time from time a to time k is short, then
(10)v~=v~a=⋯=v~k,α~=α~a=⋯=α~k,β~=β~a=⋯=β~k.Accordingly, the errorδxa,δya,δza of the coordinate position at time a can be written as
(11)δxa=xa−−xa=Δtva−α~acosβasinαa−β~asinβacosαa+α~aβ~asinβasinαa+v~acosβacosαa,δya=ya−−ya=Δtvaα~acosβacosαa−β~asinβasinαa−α~aβ~asinβacosαa+v~acosβasinαa,δza=za−−za=Δtvaβ~acosβa+Δtv~asinβa,where xa−,ya−,za− represent a priori estimates of xa,ya,za, respectively. Because no position information is received from time a to time k, an a priori estimation is used to express the position as follows:
(12)xk−=xk+δxa+1+⋯+δxk,yk−=yk+δya+1+⋯+δyk,zk−=zk+δza+1+⋯+δzk.Substituting equation (11) into equation (12), we obtain equation (13) as
(13)xk−=xk+v~Δtcosβa+1cosαa+1⋯+cosβkcosαk−α~Δtva+1cosβa+1sinαa+1+⋯+Δtcosβksinαk−β~Δtsinβa+1cosαa+1+⋯Δtsinβkcosαk+α~β~Δtsinβa+1sinαa+1+⋯Δtsinβksinαk,yk−=yk+v~Δtcosβa+1sinαa+1⋯+cosβksinαk+α~Δtva+1cosβa+1cosαa+1+⋯+Δtcosβkcosαk−β~Δtsinβa+1sinαa+1+⋯Δtsinβksinαk−α~β~Δtsinβa+1cosαa+1+⋯Δtsinβkcosαk,zk−=zk+v~Δtsinβa+1+⋯+Δtsinβk+β~va+1Δtcosβa+1+⋯+vkΔtcosβk.For representation, the coordinate position errorδxk,δyk,δzk at time k can be written as
(14)δxk=v~δxkv~+α~δxkα~+β~δxkβ~,δyk=v~δykv~+α~δykα~+β~δykβ~,δzk=v~δzkv~+β~δzkβ~.It can also be rewritten as(15)δxkδykδzk=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~v~α~β~,v~α~β~=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~−1δxkδykδzk.When the master-slave drone is colocated, the position update needs to be calculated after subtracting the corresponding estimation error from the measured value of the speed and heading:(16)xk−=xk−1+vk−−v~k′Δtcosβk−−β~k−,cosαk−−α~k−,yk−=yk−1+vk−−v~k′Δtcosβk−−β~k−,sinαk−−α~k−,zk−=zk−1+vk−−v~k′Δtsinβk−−β~k−.According to equation (14) and the UAV’s three-dimensional discrete motion and relative distance model, make the information transfer between its variables can be obtained, as demonstrated in Figure 2.The rectangular and elliptical boxes in the figure represent function and factor nodes, respectively.Figure 2
Multi-UAV factor graph colocation algorithm.


## 3.2. Procedure of the Factor Graph Colocation Algorithm

As shown in Figure2, the multi-UAV factor graph colocation algorithm is used to calculate the mean and variance of the transfer between the variable and function node. The specific steps are as follows:


### 3.2.1. Initialization

Initial state before colocation:(17)x^k−=x0,σx^k−2=σx02,y^k−=y0,σy^k−2=σy02,z^k−=z0,σz^k−2=σz02.


### 3.2.2. Update of A Priori Estimatex^k−,y^k−,z^k−

At noninitial time, dead reckoning can be performed according to equation (2). μv and σv2 are the expected speed and variance, and μα,σα2 and μβ,σβ2 are the expected heading and variance, respectively. Because the speed is not related to the heading, covv,α=0covv,β=0, we can obtain the following formula:
(18)μx^k−=μx^k−1−+Δtμvcosμβ−σβ2cosβ2cosμα−σα2cosα2,μy^k−=μy^k−1−+Δtμvcosμβ−σβ2cosβ2sinμα−σα2sinα2,μz^k−=μz^k−1−+Δtμvsinμβ−σβ2sinβ2,σx^k−2=σx^k−12+cosμβΔtσv2−μvsinμβΔtσβ22sinμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·cosμα−σα2cosμα22+sinμασα2μv2cosμβ−σβ2cosμβ22,σy^k−2=σy^k−12+cosμβΔtσv2−μvsinμβΔtσβ22cosμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·sinμα−σα2sinμα22+cosμασα2μv2cosμβ−σβ2cosμβ22,σz^k−2=σz^k−12+sinμβΔtσv2−μvcosμβΔtσβ22.


### 3.2.3. First Update ofx^k−,y^k−,z^k−

At this time,x′,z′,y′ after the coordinate transformation (Figure 2) have not been calculated, and the variable nodes xk,yk,zk and x^k−,y^k−,z^k− are equal:
(19)xk=x^k−,yk=y^k−,zk=z^k−.


### 3.2.4. First Update ofx^k−,y^k−,z^k−

To avoid the collision of the coordinate axes of the master and slave drones and to prevent the resultant large calculation errors, the coordinates of the master and slave drones are converted once in the function node to obtainxk′,yk′,zk′ and xk′m,yk′m,zk′m.
(20)xk′=xkcosθycosθz−sinθxsinθysinθz−ykcosθxsinθz+zksinθycosθz+sinθxcosθysinθz,yk′=xkcosθysinθz+sinθxsinθycosθz+ykcosθxcosθz+zksinθysinθz−sinθxcosθycosθz,zk′=xk−cosθxsinθy+yksinθx+zkcosθxcosθy,xk′m=xkmcosθycosθz−sinθxsinθysinθz−ykmcosθxsinθz+zkmsinθycosθz+sinθxcosθysinθz,yk′m=xkmcosθysinθz+sinθxsinθycosθz+ykmcosθxcosθz+zkmsinθysinθz−sinθxcosθycosθz,zk′m=xkm−cosθxsinθy+ykmsinθx+zkmcosθxcosθy,where θx,θy, and θz are the angle of rotation along the x-, y-, and z-axes, respectively.


### 3.2.5.Δxk,Δyk,Δzk Update

The role of function nodes B, C, and D is to convert the relative and absolute position information. Therefore, the function probability density of these function nodes transferred to nodeΔxk,Δyk,Δzk is
(21)NΔx,xk′m−μxk′,σxk2,NΔy,yk′m−μyk′,σyk2,NΔz,zk′m−μzk′,σzk2.Owing tod2=Δxk2+Δyk2+Δzk2, the probability density function passed from the function node E to the variable node can be expressed as
(22)NΔxk,±dk2−μΔxk2,μΔxk2σxk2+dk2σdk2dk2−μΔxk2,NΔyk,±dk2−μΔyk2,μΔyk2σyk2+dk2σdk2dk2−μΔyk2,NΔzk,±dk2−μΔzk2,μΔzk2σxk2+dk2σdk2dk2−μΔzk2.


### 3.2.6. Second Update ofxk′,yk′,zk′

The probability density of the function nodes B, C, and D transfer function toxk′,yk′,zk′ is
(23)Nxk′,xk′m−μΔxk,σΔxk2,Nyk′,yk′m−μΔyk,σΔyk2,Nzk′,zk′m−μΔzk,σΔzk2.


### 3.2.7. Second Update ofxk,yk,zk

After calculation and finishing, the updated function probability density ofxk,yk,zk is as follows:
(24)xk~Nxk,μxk,σxk2,yk~Nyk,μyk,σyk2,zk~Nzk,μzk,σzk2,1σxk2=1σx^k−2+1σxk′′2,1σyk2=1σy^k−2+1σyk′′2,1σzk2=1σz^k−2+1σzk′′2,μxk=σxk2μx^k−σx^k−2+μxk′′σxk′′2,μyk=σyk2μy^k−σy^k−2+μyk′′σyk′′2,μzk=σxk2μz^k−σz^k−2+μzk′′σzk′′2,where μxk′′,μyk′′,μzk′′ and σxk′′2,σyk′′2,σzk′′2 are the information transferred to xk,yk,zk after xk′,yk′,zk′ coordinate conversion. The coordinate conversion process is as follows:
(25)xkykzk=cosθycosθz−sinθxsinθysinθzcosθxsinθzsinθycosθz+sinθxcosθysinθzcosθysinθz+sinθxsinθycosθzcosθxcosθzsinθysinθz−sinθxcosθycosθz−cosθxsinθysinθxcosθxcosθy−1xk′yk′zk′.


## 3.2.1. Initialization

Initial state before colocation:(17)x^k−=x0,σx^k−2=σx02,y^k−=y0,σy^k−2=σy02,z^k−=z0,σz^k−2=σz02.


## 3.2.2. Update of A Priori Estimatex^k−,y^k−,z^k−

At noninitial time, dead reckoning can be performed according to equation (2). μv and σv2 are the expected speed and variance, and μα,σα2 and μβ,σβ2 are the expected heading and variance, respectively. Because the speed is not related to the heading, covv,α=0covv,β=0, we can obtain the following formula:
(18)μx^k−=μx^k−1−+Δtμvcosμβ−σβ2cosβ2cosμα−σα2cosα2,μy^k−=μy^k−1−+Δtμvcosμβ−σβ2cosβ2sinμα−σα2sinα2,μz^k−=μz^k−1−+Δtμvsinμβ−σβ2sinβ2,σx^k−2=σx^k−12+cosμβΔtσv2−μvsinμβΔtσβ22sinμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·cosμα−σα2cosμα22+sinμασα2μv2cosμβ−σβ2cosμβ22,σy^k−2=σy^k−12+cosμβΔtσv2−μvsinμβΔtσβ22cosμασα2+cosμβΔtσv2−μvsinμβΔtσβ22·sinμα−σα2sinμα22+cosμασα2μv2cosμβ−σβ2cosμβ22,σz^k−2=σz^k−12+sinμβΔtσv2−μvcosμβΔtσβ22.


## 3.2.3. First Update ofx^k−,y^k−,z^k−

At this time,x′,z′,y′ after the coordinate transformation (Figure 2) have not been calculated, and the variable nodes xk,yk,zk and x^k−,y^k−,z^k− are equal:
(19)xk=x^k−,yk=y^k−,zk=z^k−.


## 3.2.4. First Update ofx^k−,y^k−,z^k−

To avoid the collision of the coordinate axes of the master and slave drones and to prevent the resultant large calculation errors, the coordinates of the master and slave drones are converted once in the function node to obtainxk′,yk′,zk′ and xk′m,yk′m,zk′m.
(20)xk′=xkcosθycosθz−sinθxsinθysinθz−ykcosθxsinθz+zksinθycosθz+sinθxcosθysinθz,yk′=xkcosθysinθz+sinθxsinθycosθz+ykcosθxcosθz+zksinθysinθz−sinθxcosθycosθz,zk′=xk−cosθxsinθy+yksinθx+zkcosθxcosθy,xk′m=xkmcosθycosθz−sinθxsinθysinθz−ykmcosθxsinθz+zkmsinθycosθz+sinθxcosθysinθz,yk′m=xkmcosθysinθz+sinθxsinθycosθz+ykmcosθxcosθz+zkmsinθysinθz−sinθxcosθycosθz,zk′m=xkm−cosθxsinθy+ykmsinθx+zkmcosθxcosθy,where θx,θy, and θz are the angle of rotation along the x-, y-, and z-axes, respectively.


## 3.2.5.Δxk,Δyk,Δzk Update

The role of function nodes B, C, and D is to convert the relative and absolute position information. Therefore, the function probability density of these function nodes transferred to nodeΔxk,Δyk,Δzk is
(21)NΔx,xk′m−μxk′,σxk2,NΔy,yk′m−μyk′,σyk2,NΔz,zk′m−μzk′,σzk2.Owing tod2=Δxk2+Δyk2+Δzk2, the probability density function passed from the function node E to the variable node can be expressed as
(22)NΔxk,±dk2−μΔxk2,μΔxk2σxk2+dk2σdk2dk2−μΔxk2,NΔyk,±dk2−μΔyk2,μΔyk2σyk2+dk2σdk2dk2−μΔyk2,NΔzk,±dk2−μΔzk2,μΔzk2σxk2+dk2σdk2dk2−μΔzk2.


## 3.2.6. Second Update ofxk′,yk′,zk′

The probability density of the function nodes B, C, and D transfer function toxk′,yk′,zk′ is
(23)Nxk′,xk′m−μΔxk,σΔxk2,Nyk′,yk′m−μΔyk,σΔyk2,Nzk′,zk′m−μΔzk,σΔzk2.


## 3.2.7. Second Update ofxk,yk,zk

After calculation and finishing, the updated function probability density ofxk,yk,zk is as follows:
(24)xk~Nxk,μxk,σxk2,yk~Nyk,μyk,σyk2,zk~Nzk,μzk,σzk2,1σxk2=1σx^k−2+1σxk′′2,1σyk2=1σy^k−2+1σyk′′2,1σzk2=1σz^k−2+1σzk′′2,μxk=σxk2μx^k−σx^k−2+μxk′′σxk′′2,μyk=σyk2μy^k−σy^k−2+μyk′′σyk′′2,μzk=σxk2μz^k−σz^k−2+μzk′′σzk′′2,where μxk′′,μyk′′,μzk′′ and σxk′′2,σyk′′2,σzk′′2 are the information transferred to xk,yk,zk after xk′,yk′,zk′ coordinate conversion. The coordinate conversion process is as follows:
(25)xkykzk=cosθycosθz−sinθxsinθysinθzcosθxsinθzsinθycosθz+sinθxcosθysinθzcosθysinθz+sinθxsinθycosθzcosθxcosθzsinθysinθz−sinθxcosθycosθz−cosθxsinθysinθxcosθxcosθy−1xk′yk′zk′.


## 3.3. Error Analysis of Factor Graph Colocation Algorithm

The error formula can be rewritten as(26)δxkδykδzk=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~v~α~β~,v~α~β~=δxkv~δxkα~δxkβ~δykv~δykα~δykβ~δzkv~0δzkβ~−1δxkδykδzk.In the case of colocation, the position update needs to be calculated after subtracting the corresponding estimation error from the measured value of the speed and heading as follows:(27)xk−=xk−1+vk−−v~k′Δtcosβk−−β~k−cosαk−−α~k−,yk−=yk−1+vk−−v~k′Δtcosβk−−β~k−sinαk−−α~k−,zk−=zk−1+vk−−v~k′Δtsinβk−−β~k−,where v~k′=v~k−1′+v~k−1,v~k−1=0α~k′=α~k−1′+α~k−1,α~k−1=0β~k′=β~k−1′+β~k−1,β~k−1=0; therefore, the expectation and variance values can be expressed as follows:
(28)μv~k′=μv~k−1′+μv~k−1,σv~k′2=σv~k−1′2,μα~k′=μα~k−1′+μα~k−1,σα~k′2=σα~k−1′2,μβ~k′=μβ~k−1′+μβ~k−1,σβ~k′2=σβ~k−1′2.After receiving the measurement information of the main drone from the drone at timea and performing error estimation, δxav~=0, δxav~ is used as the starting point of δxkv~ for the next update. At this time, the expectation and variance of δxav~ are both 0. Subsequently, δxkv~ after time a can be recursively expressed as δxkv~=δxk−1v~+Δtcosβkcosαk; however, because the actual value of β,α cannot be determined, it is represented by β^,α^, that is,
(29)α^k=αk−−α~k′,μα^k=μαk−−μα~k′,σα^k2=σαk−2+σα~k′2,β^k=βk−−β~k′,μα^k=μβk−−μβ~k′,σβ^k2=σβk−2+σβ~k′2.Further, the expectation and variance ofδxkv~ can be expressed as
(30)μδxkv~=μδxk−1v~+Δtμcosβ^kμcosα^k,σδxkv~2=σδxk−1v~2+Δt2σcosβ^k2σcosαk2+σcosβ^k2μ2+σcosα^k2μcosβ^k2.Similarly, the expressions ofδykv~,δzkv~, δxkα~,δykα~,δzkα~, δxkβ~,δykβ~,δzkβ~ and the corresponding expectations and variances can be obtained as follows:
(31)δxkv~=δxk−1v~+Δtcosβ^kcosα^k,δykv~=δyk−1v~+Δtcosβ^ksinα^k,δzkv~=δzk−1v~+Δtsinβ^k,μδxkv~=μδxk−1v~+Δtμcosβ^kμcosα^k,σδxkv~2=σδxk−1v~2+Δt2σcosβ^k2σcosα^k2+σcosβ^k2μcosα^k2+σcosα^k2μcosβ^k2,μδykv~=μδyk−1v~+Δtμcosβ^kμsinα^k,σδykv~2=σδyk−1v~2+Δt2σcosβ^k2σsinα^k2+σcosβ^k2μsinα^k2+σsinα^k2μcosβ^k2,μδzkv~=μδzk−1v~+Δtμsinβ^k,σδzkv~2=σδzk−1v~2+Δt2σsinβ^k2,δxkα~=δxk−1α~−v^kΔtcosβ^sinα^,δykα~=δyk−1α~+v^kΔtcosβ^cosα^,δzkα~=0,μδxkα~=μδxk−1α~−Δtμcosβ^kμsinα^k,σδxkα~2=σδxk−1α~2+Δt2σcosβ^k2σsinsα^k2+σcosβ^k2μsinα^k2+σsinα^k2μcosβ^k2,μδykα~=μδyk−1α~+Δtμcosβ^kμcosα^k,σδykα~2=σδyk−1α~2+Δt2σcosβ^k2σcosα^k2+σcosβ^k2μcosα^k2+σcosα^k2μcosβ^k2,μδzkα~=0,σδzkα~2=0,δxkβ~=0,δykβ~=0,δzkβ~=δzk−1+v^kΔtcosβ^k,μδxkβ~=0,σδxkβ~2=0,μδykβ~=0,σδykβ~2=0,μδzkβ~=μδzk−1β~+Δtμv^kcosβ^k,σδzkα~2=σδzk−1β~2+Δt2σv^kcosβ^k2.Equation (32) presents the updated formula of v~k−,α~k−,β~k−.
(32)v~k−=v~k−1,α~k−=α~k−1,β~k−=β~k−1.If the slave drone does not receive the position information of the master drone at timek, then v~k−=v~kα~k−=α~kβ~k−=β~k, and the expected variance can be expressed as follows:
(33)μδxk=μyk−−μxk,σδxk2=σxk−2+σxk2,μδyk=μyk−−μyk,σδyk2=σyk−2+σyk2,μδzk=μzk−−μzk,σδzk2=σzk−2+σzk2.As shown in Figure2, the information of the variable node δxkv~ is obtained by the variable nodes δxk−1v~, α~k, and β~k through the function node, which can be expressed as Nδxkv~,μδxkv~,σδxkv~2. In addition, the information of the variable node δykv~ is obtained by the variable nodes δyk−1v~, α~k, and β~k through the function node, which can be expressed as Nδykv~,μδykv~,σδykv~2. Further, for the variable node δzkv~, its information is obtained by the variable nodes δzk−1v~, α~k, and β~k through the δyk−1α~α~kNδzkv~,μδzkv~,σδzkv~2 function node, which can be expressed as Nδzkv~,μδzkv~,σδzkv~2.The information of the variable nodeδxkα~ is obtained by δxk−1α~, α~k, and v~k through the function node, which can be expressed as Nδxkα~,μδxkα~,σδxkα~2; the information of the variable node δykα~ is obtained by δyk−1α~, α~k, and v~k through the function node, which can be expressed as Nδykα~,μδykα~,σδykα~2; and the information of the variable node δzkα~ is obtained by δzk−1α~, α~k, and v~k through the function node, which can be expressed as Nδzkα~,μδzkα~,σδzkα~2.Furthermore, the information of the variable nodeδxkβ~ is obtained by δxk−1β~, β~k, and v~k through the function node, which can be expressed as Nδxkβ~,μδxkβ~,σδxkβ~2; the information of the variable node δykβ~ is obtained by δyk−1β~, β~k, and v~k through the function node, which can be expressed as Nδykβ~,μδykβ~,σδykβ~2; and the information of the variable node δzkβ~ is obtained by δzk−1β~, β~k, and v~k through the function node, which can be expressed as Nδzkβ~,μδzkβ~,σδzkβ~2.The information passed from the function nodesv~k−1, α~k−1, and β~k−1 to variable nodes v~k, α~k, and β~k can be expressed as Nv~k,μv~k,σv~k2, Nα~k,μα~k,σα~k2, and Nβ~k,μβ~k,σβ~k2, respectively.The information transferred from the function nodes F, G, and H to the variable nodesδxk, δyk, and δzk can be represented as Nδxk,μδxk,σδxk2, Nδyk,μδyk,σδyk2, and Nδzk,μδzk,σδzk2, respectively. The information transferred from function node I to the variable node v~k can be represented as Nv~k+,μv~+,σv~+2, the information transferred to α~k can be represented as Nα~k+,μα~+,σα~+2, and the information transferred to β~k can be represented as Nβ~k+,μβ~+,σβ~+2.The a priori estimate ofv~k and the information passed from function node I to the variable node v~k can be combined to obtain the posterior estimate of v~k at time k:
(34)μv~k=σv~k2μv~k−σv~k−2+μv~k+σv~k+2,σv~k2=σv~k−2⋅σv~k+2σv~k−2+σv~k+2.Similarly, the posterior estimates ofα~k andβ~k are
(35)μα~k=σα~k2μα~k−σα~k−2+μα~k+σα~k+2,σα~k2=σα~k−2⋅σα~k+2σα~k−2+σα~k+2,μβ~k=σβ~k2μβ~k−σβ~k−2+μβ~k+σβ~k+2,σβ~k2=σβ~k−2⋅σβ~k+2σβ~k−2+σβ~k+2.


## 4. Simulation Results and Analysis

The movement of the drone was simulated in MATLAB to verify the feasibility of this algorithm. The simulation conditions were as follows: five UAVs were moving in an area of1000m×600m×300m. The position of the main UAV is accurately determined. The slave UAVx and main UAV communicate with each other, and the slave UAV obtains the location and observation information from the main UAV. Figure 3 illustrates the initial position of the slave and main UAV.Figure 3
Initial position of the drone.In the experiment, the MUAV-FGC algorithm estimates the speed error of the slave UAV, as shown in Figure4. The abscissa in the figure represents time (s), and the ordinate is the speed error estimate (m/s). The figure depicts that, as the experiment progresses, the speed error continues to accumulate. The estimated average speed error from the UAV was 0.0644 m/s during the experiment time of 600 s. Therefore, the speed error must be compensated during the positioning process to improve positioning accuracy.Figure 4
Speed error estimation results.In the experiment, the MUAV-FGC algorithm estimates the dead angle error of the slave UAV, as shown in Figure5. In the figure, the abscissa represents the time of the experiment (s), the ordinate represents the dead-end angle error (°), the curve with “∗” is the horizontal dead-angle error curve, and the curve with “⚪” is the vertical dead angle curve. As shown, the vertical dead-angle error fluctuates between -0.2°and 0.2°, and the horizontal dead-angle error stabilizes at approximately -0.58°after 400 s. In addition, the error of the dead angle will affect the positioning; therefore, it is necessary to compensate for the heading angle error in the positioning process to improve the positioning accuracy.Figure 5
Dead angle error estimation results.Figure6 presents the positioning error curves of the MUAV-FGC and the extended Kalman algorithms. In the figure, the abscissa represents the time (s), the ordinate represents the positioning error (m), the curve with “∗” is the positioning error curve of the MUAV-FGC algorithm used in this study, and the curve with “⚪” is the positioning error curve of the EKF algorithm. As shown in Figure 6, the MUAV-FGC algorithm curve is below the EKF algorithm curve, indicating that the average positioning error of the MUAV-FGC algorithm is smaller than that of the EKF algorithm. Thus, the positioning accuracy of the MUAV-FGC algorithm is higher than that of the EKF algorithm. This is because the EKF algorithm does not estimate and compensate for the speed and heading errors, resulting in a high positioning error, whereas the MUAV-FGC algorithm significantly reduces the positioning error.Figure 6
Positioning error curve.Figure7 demonstrates the mean and root mean square values of the positioning error of the EKF and FG algorithms in the experiment. The root mean square and mean values of the EKF algorithm are 16.47 and 5.09, whereas those of the MUAV-FGC algorithm are 13.58 and 4.13, respectively. The positioning error of the MUAV-FGC algorithm was found to be smaller than that of the EKF algorithm. Compared with the EKF algorithm, the mean positioning and root mean square errors of the MUAV-FGC algorithm was reduced by approximately 18.86%, and 17.54%, respectively.Figure 7
RMS error comparison.


## 5. Conclusion

In this study, we addressed the problem of multi-UAV colocation in a disaster environment, proposed a multi-UAV factor graph colocation algorithm, and derived formulas for speed and heading angle. In the factor graph model, the expectation and variance of variables are used as the information transferred between factors, and the process of error estimation algorithm based on the factor graph is derived. Based on the simulation experiment results, the average positioning error and root mean square error of the MUAV-FGC algorithm are reduced by approximately 18.86% and 17.54%, respectively, along with providing a better positioning performance, compared to the EKF algorithm.



---

*Source: 8272116-2022-08-21.xml*