cooperative localization of low-cost uav using relative...

American Institute of Aeronautics and Astronautics

1

Cooperative Localization of Low-cost UAV Using Relative Range Measurements in Multi-UAV Flight

Yaohong Qu1 Northwestern Polytechnical University, Xi’an, P. R. China, 710072

Youmin Zhang2* Diagnosis, Flight Control and Simulation Lab, Concordia University, Montreal, Canada, H3G 2W1

This paper investigates the fault-tolerant localization problem when the GPS receiver of one low-cost UAV in a fleet works improperly due to failure. A cooperative localization algorithm based on inter-UAV range measurements is proposed. Similar to the principle of GPS, the UAV’s location in 2D horizontal plane can be determined using the relative ranges to the other three UAVs at known location in inertial coordinate system. Considering the fact that accuracy of UAV’s location is worse than a positioning satellite, a Kalman filter is employed respectively on the three UAVs to estimate their locations with a constant velocity (CV) model during each computing cycle. Based on the estimations and the geometric relationship of the relative ranges, the location of UAV with GPS communication malfunction is calculated. Furthermore, in order to know well about the accuracy and statistic characteristics of calculation result, the horizontal dilution of positioning (HDOP) at length is analyzed through constructing the error equations. Taking the calculation results as observation data, another Kalman filter is applied to the UAV with malfunction, which can calculate the variance of observation noise adaptively on real-time. At last, by programming in Matlab/Simulink, a simulation example with four UAVs in a 2D scenario is shown to evaluate the effectiveness of the proposed algorithm.

Nomenclature k, j = discrete time step of Kalman filter q = parameter of system model’s variance matrix Q t0 = flight start time t = flight end time ρ = inter-UAV relative range, m x, y, z = known coordinates of satellites or UAVs, m X, Y, Z = unknown coordinates of users or UAV with malfunction, m Cb = GPS receiver clock bias x0, y0 = initial position of UAV, m ψ = heading angle (equal to path angle for neglecting side-slide angle), rad Va = airspeed of UAV (constant in this paper, equal to ground speed for neglecting wind), m/s ΔT = time of computing cycle, s ΔZ = lateral deviation, m xnom = nominal solution vector KP ,KI, Kd = control parameters of PID wve, wvn = uncorrelated Guassian white noise of system σ = stand variance

1 Senior Lecturer, School of Automation, Youyi West Road #127, Shaanxi, P.R.China, 710072. Current position: Visiting Scholar at Dept. of Mechanical and Industrial Engineering, Concordia University. 2 Associate Professor, Dept. of Mechanical and Industrial Engineering, 1515 St. Catherine W., Montreal, Quebec H3G 2W1, Canada, AIAA Senior Member. Corresponding author ([email protected]).

AIAA Guidance, Navigation, and Control Conference2 - 5 August 2010, Toronto, Ontario Canada

AIAA 2010-8187

Copyright © 2010 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved.


2

I. Introduction

owadays, Unmanned Aerial Vehicles (UAVs) are increasingly used in military and scientific research. Cooperative working with multiple UAVs is one of the most promising technologies for future space missions.

In recent years, there have been significant researches in the area of coordinated control of multiple UAVs. Examples include spacecraft formation flying1-3, UAV formation flying4-6, coordinated rendezvous of UAVs7, coordinated path planning8, task coordination for UAVs8, 9, and so on. All these researches are almost based on the navigation system working in proper order. Moreover, Low-cost and small size is also a trend of the future development of intelligent UAVs. Considering the constraints on the cost and payload size, some relevant technologies are researched recently, such as the low-cost aided dead reckoning (DR) navigation system10, low-cost tightly coupled GPS/INS integration based on nonlinear Kalman filtering11, a small fixed-wing UAV with autonomous hierarchical control architecture12, small UAV wing systems with lightweight linear broadband antennas13, etc.

As known to all, the long-term localization and navigation of UAV is not independent of the non-autonomous navigation equipments, such as global positioning system (GPS), Global Orbiting Navigation Satellite System (GLNASS), Loran-C, and so on. Because of the proper principle and high accuracy, the localization and navigation technology based on range measurements has been accepted by many satellite-based navigation systems including GPS, GLONASS, Galileo system, Compass, etc. Many practical applications, especially like GPS on vehicles, show its effectiveness. In fact, we often select one of them to integrate with inertial navigation system (INS). Usually, the most common navigation form is integrating INS with GPS since the short-term accuracy of INS and long-term accuracy of GPS are complementary14-17. Because the GPS receiver is of low price and small size, there are a significant number of low-cost and high-intelligent UAVs equipped with GPS equipment only18-20, which means the UAVs can only obtain their location from the GPS. While GPS technology has already been mature, the signal is still easily affected by external interference, noise or receiving equipment failure/malfunction, especially in a severe environment. For UAVs relying solely on GPS for navigation, such an event may be catastrophic.

Fault-tolerant localization for UAVs using inter-UAV range measurements was studied by Mao21. Because one of the distance measurements is only applied in this method, the accuracy is not good enough. Consequently, this paper investigates the fault-tolerant localization problem of UAV with more information in multi-UAV flight condition. Like the principle of GPS, which the user’s location can be determined by ranges between the positioning satellites and the user, the location of a UAV in multi-UAV cooperative flight can be calculated through measuring the relative ranges from it to others at known location. Of course, the location accuracy of UAV is not better than a positioning satellite, but Kalman filtering can help to improve the accuracy of UAV because it is not difficult for us to establish a UAV’s motion model22. Nevertheless, the accuracy is not enough for the localization and navigation of the UAV. So, we taking the calculation results as observation data, an adaptive Kalman filter algorithm for the UAV with malfunction is also proposed in this paper.

This paper is broken into multiple sections, each building upon the previous and the rest is organized as follows. Section II introduces our algorithm. Firstly, we describe the general principle of localizing UAVs with range measurements based on GPS, and the scheme of our method is introduced. Then, the accuracy of localization is discussed through deducing the error equation. Finally, an adaptive Kalman filter algorithm is presented. Section III introduces the simulation procedures using the proposed method and presents the simulation results. Based on the results, effectiveness of the proposed algorithm is analyzed and evaluated. Finally, conclusions are given in Section IV.

II. Cooperative Localization

A. Principle and Error Equations of GPS In inertial coordinate system, the observation equations in three dimensions (3D) space for each satellite with

known coordinates (xi ,yi ,zi ) and unknown user coordinates (X, Y , Z) are given by

biiii CZzYyXx +−+−+−= 222 )()()(ρ (1) where Cb is receiver clock bias. Here, the unknown variables are X, Y, Z, Cb respectively. Thus, to obtain a 3D position, at least four satellites are required by GPS, as shown in Fig. 1.

N


3

From the Eq. (1), we know that these are nonlinear equations that can be linearized using Taylor series23. Let the vector of ranges be ρ= h(x), where a nonlinear function h(x) of the four dimensional vector x represents user position and receiver clock bias, and expand the left-hand side of this equation in a Taylor series about some nominal solution xnom for the unknown vector

TbCZYX ],,,[=x (2)

for which

⎪⎭

⎪⎬⎫

−=−=

+∂

∂+===

)()(

)()()( |nomnom

nomnom

xhxhρxxx

H.O.Txxxhxhxhρ

xx

δδδ (3)

where H.O.T stands for “higher-order terms”. These equations become

⎪⎭

⎪⎬⎫

−=−=−=

=∂

∂==

nomnomnom ZZzYYyXXxnom

δδδδδδ

,,

)( )1(| xHxxxhρ

xx (4)

where H(1) is the first-order term in the Taylor series expansion

ρvx

xρ

ρρρ

+∂∂≈

−=

δ

δ

nomnomnom ZYX

nomnomnom ZYXZYX

,,

),,(),,( (5)

vρ represents noise in receiver measurements. This vector equation can be written in scalar form where i = satellite number as

⎪⎪⎪⎪

⎭

⎪⎪⎪⎪

⎬

⎫

−+−+−−−=

∂∂

−+−+−−−=

∂∂

−+−+−−−=

∂∂

222

222

222

)()()()(

)()()()(

)()()()(

nominominomi

nomii

nominominomi

nomii

nominominomi

nomii

ZzYyXxZz

Z

ZzYyXxYy

Y

ZzYyXxXx

X

ρ

ρ

ρ

(6)

for i = 1, 2, 3, 4 (i.e., four satellites) (7)

We can combine Eq. (5) and (6) into the matrix equation

Fig. 1 A GPS user can be located by no less than four satellites


4

⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢

⎣

⎡

+⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

4

3

2

1

444

333

222

111

4

3

2

1

1

1

1

1

ρ

ρ

ρ

ρ

δδδδ

ρρρ

ρρρ

ρρρ

ρρρ

δρδρδρδρ

vvvv

CZYX

ZYX

ZYX

ZYX

ZYX

b

(8)

which we can write in symbolic form as kvxHρ += δδ )1( (9)

To calculate H(1), one needs satellite positions and the nominal value of the user’s position. To calculate the geometric dilution of precision (GDOP) (approximately), we obtain

xHρ δδ )1(= (10) δρ and H(1) are known from the pseudorange, satellite position, and nominal value of the user’s position. The

correction δx is the unknown vector. If we premultiply both sides of Eq. (10) by H(1)T , the result will be

xHHρH δδ )( (1)(1)(1) TT = (11) Then we premultiply Eq. (11) by (H(1)T H(1) )-1:

ρHHHx δδ TT (1)1(1)(1) )( −= (12) If δx and δρ are assumed random with zero mean, the error covariance

1(1)(1)(1)(1)1(1)(1)

(1)1(1)(1)(1)1(1)(1)

)()(])[()(

))((

−−

−−

><=><=

><

HHHρρHHHρHHHρHHH

xx

TTTT

TTTTT

T

EE

E

δδδδ

δδ (13)

The pseudorange measurement covariance is assumed uncorrelated satellite-to-satellite with variance σ2: 2σδδ >=< TE ρρ I4 (14)

a 4x4 matrix. Substituting Eq. (14) into Eq. (13) gives

1(1)(1)2 )())(( −>=< HHxx TTE σδδ (15) From the Eq. (15), we can obtain all the DOPs (including GDOP, HDOP, VDOP, TDOP) which represent

sensitivities to pseudorange errors. In fact, we are principally interested in the diagonal elements of 1(1)(1) )( −HH T with σ2 = 1m2 ( see, e.g., Chapter 2 of Ref. 23).

B. Cooperative Localization Algorithm Assume that multiple UAVs equipped respectively with a GPS receiver are flying at a constant height, and a

UAV’s GPS receiver does not work because of external interference during the flight. Further assume that the UAV with malfunction can know the relative distances to others by range measurements.

According to the analysis above, we know that the location coordinate of UAV in 3D space is determined by four satellites at least. So, in 2D coordinate system, a UAV can be located by three other UAVs at known location, as shown in Fig. 2.

Considering the location accuracy of UAV from GPS is not better than the satellite, we design a Kalman filter on every UAV to improve its accuracy. The location of the UAV with malfunction is calculated using the relative range measurements. Then, taking the calculation result as observation, another Kalman filter is applied to the UAV. We assume the calculation result is Guass random distribution with zero mean and its variance can be estimated by HDOP in every step, thus the Kalman filter is adaptive. The schematic structure of the localization is shown in Fig. 3.


5

C. Location and Calculation In 2D plane, the relative distance between two UAVs is expressed as

22 )()( YyXx iii −+−=ρ , (16) where (X, Y ) are the unknown coordinates, (xi , y) known coordinates, and i =1, 2, 3 (the index number of UAVs at known location).

We square both sides of Eq. (16): YyXxyxYX iiiii 22)()( 22222 −−+++=ρ . (17)

Let )( 22 YXA += (18)

Substitute the relevant term in Eq. (17) with Eq. (18) and express each equation, we obtain

Fig. 2 A cooperative localization example with four UAVs (Three UAVs can be located by GPS; the other can be located in 2D horizontal plane using the three relevant range measurements)

Fig. 3 The schematic structure of proposed cooperative localization method

AdaptiveKalman

filter

DOP Calculation

Location Calculation

CommunicationBlock

GPS

antenna

Kalman filter1

CommunicationBlock

UAV1

GPS

antenna

Kalman Filter2

CommunicationBlock

UAV2

GPS

antenna

Kalman Filter3

CommunicationBlock

UAV3

Ranging

Ranging

Ranging

Ranging

UAV4


6

⎪⎭

⎪⎬

⎫

−−=+−−−=+−−−=+−

YyXxAyxYyXxAyxYyXxAyx

3323

23

23

2222

22

22

112

12

12

1

22)(22)(22)(

ρρρ

(19)

Eq. (19) can be expressed in vector form as HxM = , (20)

where

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+−+−+−

=)()()(

23

23

23

22

22

22

21

21

21

yxyxyx

ρρρ

M , ⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−−−−−

=122122122

33

22

11

yxyxyx

H , ⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

AYX

x . (21)

Then we premultiply both sides of Eq. (21) by H-1: MHx 1−= (22)

If the rank of H, the number of linearly independent columns of the matrix H, is less than 3, then H will not be

invertible. In that case, its determinant is given as det H = |H| = 0. (23)

This paper only investigates the case where any three UAVs are not collinear, which means H is invertible. In our future plan, we will continue the work for the general case.

D. Error Equation Similar to Eq. (6) and (8), error equation can be written by

⎪⎪⎭

⎪⎪⎬

⎫

−+−−−=

∂∂

−+−−−=

∂∂

22

22

)()()(

)()()(

nominomi

nomii

nominomi

nomii

YyXxYy

Y

YyXxXx

Xρ

ρ

(24)

for i = 1, 2, 3 (i.e., three UAVs at known location) (22)

or

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢

⎣

⎡

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

3

2

1

13

12

11

3

2

1

1

1

1

ρ

ρ

ρ

δδδ

ρρ

ρρ

ρρ

δρδρδρ

vvv

CYX

YX

YX

YX

b

(23)

Eq. (23) can also be written as kvxHρ += δδ )1( , (24)

where Xnom, Ynom, Znom may be chosen as the estimation of model in Kalman filter. Considering that the variances of three UAVs’ locations are different after Kalman filter, the variance of δx is

given by

TTE )]([00

0000

][))(( 1)1(

23

22

21

1)1( −−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡>=< HHxx

σσ

σδδ , (25)

where σ12, σ2

2, σ32 are respectively from the estimations of three Kalman filters.

Note that δx=[ΔX ΔY ΔCb]T, so we can also give another form of Eq. (25) as

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

ΔΔΔΔΔΔΔΔΔΔΔΔΔΔΔ

>=<

333231

232221

1312112

2

)()()()()()()()()(

))((AAAAAAAAA

CEYCEXCECYEYEXYECXEYXEXE

E

bbb

b

bTxx δδ (26)


7

E. Design of Kalman Filter Motion of a UAV can be taken as a constant velocity (CV) model in every running cycle because the computing

frequency is generally so quick that the UAV’s heading angle and speed can be nearly constant in a calculating cycle. Thus, we can give a discrete motion model for UAV expressed as

)()()(),1()1( kkkkkk WΓXΦX ++=+ , (27) where

[ ]Tyx vyvx=X , ⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

Δ

Δ

=+

1000100

0010001

),1(T

T

kkΦ , 4x4I=Γ , [ ]Tvnve kwkwk )(0)(0)( =W (28)

and x = east location of UAV in east-north-up coordinate system y = north location of UAV in east-north-up coordinate system vx = east speed of UAV in east-north-up coordinate system vy = east speed of UAV in east-north-up coordinate system ΔT = time of computing cycle wve, wvn = uncorrelated Guassian white noise of system.

In this paper, we assume that all the UAVs have a same model including ΔT and wve, wvn. In fact, the assumption is reasonable because the robustness of Kalman filter is very well for observation correction.

The observations of three fault-free UAVs come from their own GPS receivers respectively. To simplify the problem, we assume the location is provided directly in rectangular coordinate system (E-N-U) (transformation from (Lon, Lat, Alt) to (E,N,U) as seen in Ref. 23). Thus, the observation equation can be easily described by

⎥⎦

⎤⎢⎣

⎡++

++⎥⎦

⎤⎢⎣

⎡=

++++=+

)1()1(

)1(01000001

)1()1()1()1(

2

1

kvkv

k

kkkk

X

VXHZ , (29)

where V is the observation noise vector. In order to apply Kalman filters, we assume that W(k) and V(k) satisfy the condition as follows:

[ ][ ]

0)()()(),(

)()()()(),(,0)(

)()()()(),(,0)(

>=<>=<

>=<>=<=

>=<>=<=

T

kjT

kjT

jkEjkCov

kjkEjkCovkE

kjkEjkCovkE

VWVW

RVVVVV

QWWWWW

δδ

, (30)

where δij is the Dirac Delta function. Now, we give the discrete Kalman Filter procedures as follows:

⎪⎪⎪

⎭

⎪⎪⎪

⎬

⎫

++++++−+++−=+++−++++=+

+++++++=++++=+

+=+

−

TT

TT

T

kkkkk/kkkkk/kkkkk/kkkkk/kkkk/kkk

k,kkk,kk/kkk,kk/kk

)1()1()1()]1()1([)1()]1()1([)1()]1(ˆ)1()1()[1()1(ˆ)1(ˆ

])1()1()1()1([)1()1()1()()1()()1()1(

)(ˆ)1()1(ˆ

1

KRKHKIPHKIPXHZKXX

RHPHHPKQΦPΦP

XΦX

. (31)

As introduced earlier, the observation of UAV with malfunction comes from the result of location calculation. Now, we are principally interested in the diagonal elements of Eq. (26) and we know that A11 and A22 will respectively reflect the accuracy of X coordinate and Y coordinate. We define 2211 AAHDOP += which reflects the accuracy of location in 2D plane. For the UAV with malfunction, we can calculate the observation variance as follows:

⎥⎦

⎤⎢⎣

⎡=

22

11

00

)(A

AkR . (32)

Consequently, the adaptive Kalman filter can be applied to the UAV.


8

III. Simulation Results and Analysis

A. Navigation and Guidance of UAV In order to realize the above proposed idea, we apply UAV’s lateral model and control law to the four UAVs .

The control law of heading holder during flight is shown as Fig. 4.

The guidance and tracking control law adopts a PID controller as

∫Δ+Δ+Ψ=Ψt

IPg ZdtKZK00 (32)

where

⎪⎪⎪

⎩

⎪⎪⎪

⎨

⎧

Δ+Δ

>Δ+Δ

−<Δ+Δ−

=Δ+Δ

∫∫∫

∫elseZdtKZK

ZdtKZK

ZdtKZK

ZdtKZKt

IP

t

IP

t

IP

t

IP

0

0

0

0 22

22ππ

ππ

(33)

The locations of all the UAVs in the simulation are provided by its dead reckoning (DR), that is,

⎪⎭

⎪⎬

⎫

+=

+=

∫∫

dtVyy

dtVxxt

t a

t

t a

)cos(

)sin(

0

0

0

0

ψ

ψ (34)

where (x0, y0) = initial position of UAV

t0 = start time t = flight end time ψ = heading angle (equal to path angle for neglecting side-slide angle) Va = airspeed of UAV (constant in this paper, equal to ground speed for neglecting wind).

During lateral path tracking control, the lateral deviation ΔZ is the main control signals. Let ΔZ of the left-deviation positive and right-deviation negative. Assume that the two waypoints are A whose coordinates are (x1, y1)，B with coordinates (x2, y2) and the location of UAV is P with coordinates (x0, y0), respectively. Hence, the lateral deviation |ΔZ| can be calculated by

Fig. 4 Heading holder control structure of a UAV

Kp

Lateral Linearized

Model of a UAV

Actuator of Aileron

Actuator of Rudder

Kψ

Kør

Kr

Kø

Ψg

φ

p

r

Ψ


9

212

212

1221012012

)()(

)()(

xxyy

xyxyyxxxyyZ

−+−

−+−−−=Δ (35)

The symbols of ΔZ can be determined by vector determination method. In 3D space (e.g. using East-North-Up coordinate system), we construct three vectors respectively as follows:

}0,,{ 1010 yyxx −−=AP ， }0,,{ 1212 yyxx −−=AB ， }1,0,0{=kr

(36)

Let ))(())(()( 10121210 yyxxyyxxf −−−−−=•⊗= kABAP

r (37)

Then, we can draw the following conclusions: i) If f > 0, point P is located in planning routes on the left, ΔZ > 0; ii) If f = 0, point P is located in planning routes, ΔZ = 0; iii) If f < 0, point P is located in planning routes on the right, ΔZ < 0.

B. Simulation In 2D horizontal plane, we plan some waypoints for each UAV beforehand as shown Table 1. Assume the GPS

receiver of UAV4 does not work due to failure. Furthermore, in order to imitate GPS observation and range measurements, we deliberately add the white noise with zero mean and variance of (15m)2 to the nominal trajectory and the white noise with zero mean and (20m)2 variance to the nominal relative range measurements; take 0.02ms as the computing cycle time; take 50m/s as the flight speed of all UAVs. We spent about 6 minutes simulation altogether. Fig. 5 shows all information about the trajectories of four UAVs. Considering large amounts of data included in the picture to see clearly, we specially give a detailed view of UAV4’s trajectories in Fig. 6. The accuracy of UAV4’s location through the proposed method is illustrated in Fig. 7.

Furthermore, with the purpose of testing the effectiveness of the proposed algorithm, another general Kalman filter is applied simultaneously, in which the variance of observations is chosen as a constant by Monte Carlo method. The variances of position errors with the two methods are listed in Table 2, respectively. The position error means the distance between the estimation of UAV4’s location and the nominal location. In addition, we also show the results from the two methods by changing the parameter q of system model’s variance matrix Q, where

⎥⎦

⎤⎢⎣

⎡=

qq0

0Q . (38)

. Table 1 Planned waypoints for UAVs

No. of UAV Location of waypoints (in order)

UAV1 1.(0,0), 2.(0, 4000), 3.(4000,4000), 4.(4000,0) UAV2 1.(8000, 0), 2.(4000, 8000), 3.(12000,4000) UAV3 1.(7000,8000), 2.(3000,9000), 3.(8000,14000) UAV4 1.(2000,-4000), 2.(6000,-4000), 3.(7000,-7000),

4.(5000,-9000), 5.(1000,-6000)


10

Fig. 5 Trajectories of all four UAVs (including planned routes, nominal trajectories, observations, and results from KFs )

Fig. 6 Partial enlarged view of the trajectories of UAV4


11

Table 2 Variances of estimated UAV4’s location distance error through two methods

q (Q= ⎥⎦

⎤⎢⎣

⎡q

q0

0) general KF proposed method

0 1.92E+03 2.31E+03 1 10.10991929 18.78001498 2 11.99681431 14.38721255 3 13.05477957 13.53442523 4 13.55431818 12.53049611

5 14.54714398 12.3869762 10 17.27333854 13.3388194

20 20.80124612 15.28298557 50 25.82446245 18.64166945 100 30.34732223 21.63266587

C. Result Analysis 1) The simulation work is well fit for applications, because the dynamic model and the kinematical model of a

real UAV are all adopted in this paper. To construct the simulation data, the lateral heading control law including the nonlinear elements, which was already examined frequently in real flight applications, is also adopted.

Fig. 7 Accuracy of UAV4’s location (The picture on the top: distance errors from location of UAV4 to the nominal location

The picture below: HDOP curve during calculating UAV4’s location using relative range measurements )


12

2) From Fig. 6 and 7, we can see that the estimated location of UAV4 is very near to its nominal location. Especially, from the top picture in Fig. 7, we can also see that UAV4’s location distance errors only by geometry computing go up to about 350m in this simulation condition, while they are almost below 50m by the proposed method.

3) Comparing the two sub-figures in Fig, 7 and note that the time scales of their X axis are the same, we know that the location error curve goes up with increase of HDOP. So we can conclude that the locations of three UAVs with GPS affect the location accuracy of UAV with malfunction.

4) According to the variances of location distance error listed in Table 2, we can draw a conclusion that the proposed algorithm is significantly better than the general KF method when the parameter q in the matrix Q is larger. It can be explained from two sides. On one hand, the proposed method can adaptively estimate the variance of observation, while the variance of observation with the general KF method has to be assigned in advance. In other words, the former is time-variable adaptively but the latter is taken as a constant. On the other hand, the larger q means the lager uncertainty of system model and a smaller role of observation in Kalman filter. In fact, it is a very important conclusion because the system model usually cannot be estimated very well in applications.

IV. Conclusion The traditional localization and navigation means for small and low-cost UAVs is based on GPS. However, the

GPS is very vulnerable to fail in some special circumstances such as battle fields and bad weather conditions so that the UAVs have to abandon their task or even crash directly. Therefore, it is very important to develop fault-tolerant localization methods in a long voyage.

Based on the principle of GPS, we present a fault-tolerant localization algorithm for UAV with malfunction in GPS receiver by using inter-UAV relative range measurements. The contribution of the work is the development of a framework which the UAV with GPS malfunction can be located by other members in a cooperative working UAV team. Besides, the adaptive calculation of observation variance in term of the HDOP is also a part of our contribution. Finally, the simulation results show that the proposed method is effective.

Although we assume that all the UAVs is flying at a constant height, the proposed method is also easily applied into the 3D space. One of future works is to combine this work with the selection algorithm of HDOP in scenarios with more UAVs.

Acknowledgements First author’s work is supported by National Nature Science Foundation of China (No.60974146). Second

author’s work is supported by the Natural Sciences and Engineering Research Council of Canada (NSERC) through a Strategic Project Grant (STPGP 350889-07) and a Discovery Project Grant. The authors would like to thank all the colleagues of the Diagnosis, Flight Control and Simulation Lab of Concordia University for their help.

References

1 R. W. Beard, J. Lawton, and F. Y. Hadaegh, “A Feedback Architecture for Formation Control,” IEEE Transactions on Control Systems Technology, Vol. 9, 2001, pp. 777-790.

2 W. Kang and H.-H. Yeh, “Coordinated Attitude Control of Multi-Satellite Systems,” International Journal of Robust and Nonlinear Control, Vol. 12, 2002, pp. 185-205.

3 P. K. C. Wang and F. Y. Hadaegh, “Coordination and Control of Multiple Microspacecraft Moving in Formation,” The Journal of the Astronautical Sciences, Vol. 44, No. 3, 1996, pp. 315-355.

4 A. W. Proud, M. Pachter, and J. J. D'Azzo, “Close Formation Flight Control,” Proceedings of the AIAA Guidance, Navigation, and Control Conference and Exhibit, Portland, 1999, pp. 1231-1246.

5 J. M. Fowler and R. D'Andrea, “Distributed Control of Close Formation Flight,” Proc. of IEEE Conference on Decision and Control, Las Vegas, NV, 2002, pp. 2972-2977.

6 D. M. Stipanovic, G. Inalhan, R. Teo, and C. J.Tomlin, “Decentralized Overlapping Control of a Formation of Unmanned Aerial Vehicles,” CDC, Las Vegas, NV, 2002, pp. 2829-2835.

7 R. W. Beard, T. W. McLain, M. Goodrich, and E. P. Anderson, “Coordinated Target Assignment and Intercept for Unmanned Air Vehicles,” IEEE Transactions on Robotics and Automation, Vol. 18, 2002, pp. 911-922.

8 J. S. Bellingham, M. Tillerson, M. Alighanbari, and J. P. How, “Cooperative Path Planning for Multiple UAVs in Dynamic and Uncertain Environments”, Proc. of IEEE Conference on Decision and Control, Las Vegas, NV, 2002, pp. 2816-2822.

9 A. Richards, J. Bellingham, M. Tillerson, and J. How, “Coordination and Control of UAVs”, Proceedings of the AIAA Guidance, Navigation, and Control Conference, Monterey, CA, 2002, pp. 2002-4588.

10 D. Egziabher, C. Boyce, J. Powell, and P. Enge, “An Inexpensive DME-Aided Dead Reckoning Navigator”, ION Journal


13

of Navigation, Vol. 50, No. 4, 2004, pp. 247-263. 11 Y. Li, and J. Wang, “Low-cost Tightly Coupled GPS/INS Integration Based on Nonlinear Kalman Filtering Design”, In

Institute of Navigation National Technical Meeting, 2006. 12 R.Beard, D. Kingston, M. Quigley, etc, “Autonomous Vehicle Technologies for Small Fixed-Wing UAVs”, Journal of

Aerospace Computing, Information, and Communication, Vol. 2, 2005, pp. 92-108. 13 M. Hilliard, J. Mead, R. Rincon, and H. Hildebrand, “Lightweight Linear Broadband Antennas Enabling Small UAV Wing

Systems and Space Flight Nanosat Concept”, Geoscience and Remote Sensing Symposium, Vol. 5, 2004, pp. 3577-3580. 14 O.S.Salychev, and V.Voronov, “Attitude Determination with GPS-aided Inertial Navigation Systems”, Proceedings of the

56th Annual Meeting of IAIN World Congress, San Diego, CA, 2000, pp.705-711. 15 N.G.Mathur, and F.Van Graas, “Feasibility of using a Low-Cost Inertial Measurement Unit with Centimeter-Level GPS”,

Proceedings of the 56th Annual Meeting of IAIN World Congress, San Diego, CA,2000, pp.712-720. 16 Sang Heon Oh, Dong-Hwan Hwang, Sang Jeong Lee, and Se Hwan Kim., “Attitude Determination GPS/INS Integrated

Navigation System with FDI Algorithm for a UAV ,” Journal of Mechanical Science and Technology, Vol.19, No.8, 2005. 17 Gebre-egziabher D, Hayward R C, and Powell J D. “A Low Cost GPS/Inertial Attitude Heading Reference System

(AHRS) for General Aviation Applications”, IEEE Position Location and Navigation Symposium, 1998, pp.518-525. 18 http://www.chinaga.com/all/3812.html. 19 Tung-jung Hsu, “UAV Automatic Navigation and Autopilot System Implementation by using DSP, FPGA and GPS

Modules”, Master Thesis, Aeronautics & Astronautics Dept., National Cheng Kung University, 2003. 20 Lv Yanmei, Li Xiaomin, Peng Xianhua, and Sun Jiangsheng, “Design of the Navigation System of UAV in High Dynamic

Circumstance Based on GPS”, Proceedings of 6th International Symposium on Test and Measurement, Vol. 7, 2005. 21 Mao, G., S. Drake, and B. Anderson, “Design of an Extended Kalman Filter for UAV Localization”, IEEE Information,

Decision and Control, 2007, pp. 224–229. 22 X. Rong Li, and Vesselin P. Jilkov, “Survey of Maneuvering Target Tracking”, IEEE Transactions on Aerospace and

Electronic Systems, Vol. 39, No. 4, 2003, pp. 1333-1364. 23 Mohinder S. Grewal, Lawrence R. Weill, and Angus P. Andrews, “Global Positioning System, Inertial Navigation, and

Integration”, Second Edition, John Wiley & Sons, Inc., Hoboken, New Jersey, 2007.

cooperative localization of low-cost uav using relative...

Documents