Cooperative Localisation of a GPS-Denied UAV using Direction of Arrival Measurements
James S. Russell, Mengbin Ye, Brian D.O. Anderson, Hatem Hmam, Peter Sarunic
11 Cooperative Localisation of a GPS-Denied UAVusing Direction-of-Arrival Measurements
James S. Russell, Mengbin Ye, Brian D.O. Anderson, Hatem Hmam, Peter Sarunic
Abstract —A GPS-denied UAV (Agent B) is localised throughINS alignment with the aid of a nearby GPS-equipped UAV(Agent A), which broadcasts its position at several time instants.Agent B measures the signals’ direction of arrival with respect toAgent B’s inertial navigation frame. Semidefinite programmingand the Orthogonal Procrustes algorithm are employed, andaccuracy is improved through maximum likelihood estimation.The method is validated using flight data and simulations. Athree-agent extension is explored.
Index Terms —Localisation, INS alignment, Direction-of-Arrival Measurement, GPS-Denied, Semidefinite Programming
I. I
NTRODUCTION
Unmanned aerial vehicles (UAVs) play a central role inmany defence reconnaissance and surveillance operations. For-mations of UAVs can provide greater reliability and coveragewhen compared to a single UAV. To provide meaningful datain such operations, all UAVs in a formation must have acommon reference frame (typically the global frame). Tra-ditionally, UAVs have access to the global frame via GPS.However, GPS signals may be lost in urban environmentsand enemy controlled airspace (jamming). Overcoming lossof GPS signal is a hot topic in research [3], and offers a rangeof different problems in literature [2, 18].Without access to global coordinates, a UAV must rely on itsinertial navigation system (INS). Stochastic error in on-boardsensor measurements causes the INS frame to accumulate drift.At any given time, drift can be characterised by a rotation andtranslation with respect to the global frame, and is assumed tobe independent between UAVs in a formation. As a result, INSframe drift cannot be modelled deterministically. Informationfrom both the global and INS frames must be collected andused to determine the drift between frames in order to achieveINS frame alignment. We describe this process as cooperativelocalisation when multiple vehicles interact to achieve the task.
The work of Russell, Ye, and Anderson was supported by the AustralianResearch Council (ARC) Discovery Project DP-160104500, by 111-ProjectNo. D17019, and by Data61-CSIRO. Ye was supported by an AustralianGovernment Research Training Program (RTP) Scholarship.J.S. Russell, M. Ye and B.D.O. Anderson are with the ResearchSchool of Engineering, Australian National University, Canberra, Australia { u5542624, mengbin.ye, brian.anderson } @anu.edu.au .B.D.O. Anderson is also with Hangzhou Dianzi University, Hangzhou,China, and with Data61-CSIRO (formerly NICTA Ltd.) in Canberra,Australia. H. Hmam and P. Sarunic are with Australian Defence Science andTechnology Group (DST Group), Edinburgh, Australia { hatem.hmam,peter.sarunic } @dst.defence.gov.au . Various measurement types such as distance between agentsand direction of arrival of a signal (we henceforth call DOA )can be used for this process. In the context of UAVs, additionalsensors add weight and consume power. As a result, onegenerally aims to minimise the number of measurement typesrequired for localisation. This paper studies a cooperativeapproach to localisation using DOA measurements.When two or more GPS-enabled UAVs can simultaneouslymeasure directions with respect to the global frame towards theGPS-denied UAV, the location of the GPS-denied UAV is givenby the point minimising distances to the half-line loci derivedfrom the directional measurements [6, 25, 26]. Operationalrequirements may limit the number of nearby GPS-enabledUAVs to one single agent. We therefore seek a solution whichdoes not require simultaneous measurements to a single point.When the GPS-denied agent is able to simultaneouslymeasure directions with respect to its local INS frame towardsmultiple landmarks (two in two-dimensional space, or threein three-dimensional space) with known global coordinates,triangulation-based measurements can be used to achieve lo-calisation. This problem is studied in three-dimensional spacein [4], and in two-dimensional space in [8, 9]. If only one land-mark bearing can be measured at any given time, a bearing-only SLAM algorithm may be used to progressively constructa map of the environment on the condition each landmarkis seen at least twice. Alignment of a GPS-denied agent’sINS frame could then be achieved by determining the rotationand translation between the map’s coordinate frame and theglobal coordinate frame. In practice, landmark locations maybe unknown, or there may be no guarantee they are stationaryor permanent, and hence we require a localisation algorithmwhich is independent of landmarks in the environment. Iter-ative filtering methods such as the Extended Kalman Filterare often required when drift is significant between updates,however in our problem context the drift is sufficiently slowto be modelled as stationary over short periods. Given that theUAVs have limited computational capacity on board, we aremotivated to formulate a localisation algorithm which does notinvolve an iterative filtering technique.Without reliance on landmarks, the only directional mea-surements available are between the GPS-denied and the GPS-enabled UAVs. Given their potentially large separation, theseUAVs are modelled as point agents, and therefore one singledirectional measurement is available at any given time. A A bearing generally describes a scalar measurement between two pointsin a plane, whereas a direction-of-arrival is a vector measurement betweentwo points in three-dimensional ambient space, which is the space that thispaper considers. a r X i v : . [ c s . R O ] N ov stationary target is localised by an agent using bearing-onlymeasurements in two-dimensional space [5, 12], and in three-dimensional space [23]. A similar problem is considered in[20], in which a mobile source is localised using measurementsreceived at a stationary receiver using an iterative filteringtechnique. In these works, either the entity being localisedor a receiver must be stationary, and the entity requiringlocalisation must broadcast a signal. For operational reasons,the agent requiring localisation may be unable to broadcastsignals, or agents involved may not be allowed to remainstationary. As a result, the approaches in [5], [12], [20] and[23] are not suitable. Commonly used computer vision tech-niques such as structure from motion [15] require directionalmeasurements towards multiple stationary points or towardsa stationary point from multiple known positions. This is notpossible in our problem context. The constraints flowing fromthe measurement and motion requirements we are imposingtherefore represent a significant technical challenge.One algorithm satisfying all the constraints above wasproposed in [28], in which two agents perform sinusoidalmotion in two-dimensional ambient space and directionalmeasurements are used to obtain the distance between AgentsA and B, but localisation of B in the global frame is notachieved.Motivated by interest from Australia’s Defence Science andTechnology Group, this paper seeks to address the problemof localising a GPS-denied UAV, which we will call AgentB, with the assistance of a GPS-enabled UAV, which wewill call Agent A. Both agents move arbitrarily in three-dimensional space. Agent B navigates using an INS frame.Agent A broadcasts its position in the global coordinate frameat discrete instants in time. For each broadcast of Agent A,Agent B is able to take a DOA measurement towards AgentA.The problem setup and the solution we propose are bothnovel. In particular, while the literature discussed above con-siders certain aspects from the following list, none considerall of the following aspects simultaneously: • The network consists of only two mobile agents (andis therefore different to the sensor network localisationproblems in the literature). • There is no a priori knowledge or sensing of a stationaryreference point in the global frame. • The UAVs execute unconstrained arbitrary motion inthree-dimensional space. • Cooperation occurs between a GPS-enabled and a GPS-denied UAV (transmission of signals is unidirectional;from Agent A to Agent B).It is the combination of all the above aspects which makethe problem significantly more difficult and thus existingmethods are unsuitable. In [30], this problem is studied in two-dimensional space using bearing measurements. One singlepiece of data is acquired at each time step. In this paper, eachDOA measurement gives two scalar quantities, and adding the Agent A’s role in the cooperation is to broadcast its position over a seriesof time instants. third dimension significantly complicates the problem, thusrequiring new techniques to be introduced.In our proposed solution, we localise Agent B by identifyingthe relationship between the global frame (navigated by AgentA) and the inertial navigation frame of Agent B. The rela-tionship is identified by solving a system of linear equationsfor a set of unknown variables. The nature of the problemmeans quadratic constraints exist on some of the variables. Toimprove robustness against noisy measurements, we exploitthe quadratic constraints and use semidefinite programming(SDP) and the Orthogonal Procrustes algorithm to obtain aninitial solution for maximum likelihood (ML) estimation. Ourapproach combining SDP, the Orthogonal Procrustes algorithmand ML estimation is a key novel contribution over existingworks.We evaluate the performance of the algorithm by (i) using areal set of trajectories and (ii) using Monte Carlo simulations.Sets of unsuitable trajectories are identified, in which ourproposed method cannot feasibly obtain a unique solution.Finally, we explore an extension of the algorithm to a three-agent network in which two agents are GPS-denied.The rest of the paper is structured as follows. In Section IIthe problem is formalised. In Section III a localisation methodusing a linear equation formulation is proposed. Section IVextends this method to semidefinite programming to producea more robust localisation algorithm. In Section V, a maximumlikelihood estimation method is presented to refine resultsfurther. Section VI presents simulation results to evaluate theperformance of the combined localisation algorithm. SectionVII extends the localisation algorithm to a three-agent net-work. The paper is concluded in Section VIII. II. P
ROBLEM D EFINITION
Two agents, which we call Agent A and Agent B, travelalong arbitrary trajectories in three-dimensional space. AgentA has GPS and therefore navigates with respect to the globalframe. Because Agent B cannot access GPS, it has no abilityto self-localise in the global frame, but can self-localise andnavigate in a local inertial frame by integrating gyroscope andaccelerometer measurements.This two-agent localisation problem involves 4 frames asin Figure 1. The importance of each frame, and its use inobtaining the localisation, will be made clear in the sequel.Frames are labelled as follows: • Let A denote the global frame (available only to AgentA), • let B denote the local INS frame of Agent B, • let B denote the body-centred INS frame of Agent B(axes of frames B and B are parallel by definition), • let B denote the body-fixed frame of Agent B. Early sections in this paper (covering up to and including employmentof Orthogonal Procrustes algorithm) appeared in less detail in the conferencepaper [21]. Additions have been made to these sections - the literature reviewis now more extensive; the role of different coordinate frames is muchmore explicitly set out; the algorithm’s performance is now validated onreal flight trajectories, and the inclusion of apparently redundant quadraticconstraints in the SDP problem formulation is justified further. Analysisof unsuitable trajectories, ML refinement and the three-agent extension arefurther extensions beyond [21].
The expression of directional measurements with respectto the INS frame in vector form motivates the definition ofthe body-centred frame B . Later, we find that differences inbody fixed frame azimuth and elevation measurement noisemotivate the use of B when discussing maximum likelihoodestimation.Note that agents A and B are denoted by a single letter,whereas frames A and B i for i = 2,3,4 are denoted by aletter-number pair. Positions of each agent in their respectivenavigation frames ( A and B ) are obtained through a discrete-time measurement process. Let p I J ( k ) denote the position ofAgent J in coordinates of frame I at the k th time instant. Let u J , v J , w J denote Agent J’s coordinates in the global frame( A ), and x J , y J , z J denote Agent J’s coordinates in AgentB’s local INS frame ( B ). It follows that: p A A ( k ) = [ u A ( k ) , v A ( k ) , w A ( k )] (cid:62) (1) p B B ( k ) = [ x B ( k ) , y B ( k ) , z B ( k )] (cid:62) (2)The rotation and translation of Agent B’s local INS frame( B ) with respect to the global frame ( A ) evolves via drift.Although this drift is significant over long periods, frame B can be modelled as stationary with respect to frame A overshort intervals . During these short intervals, the followingmeasurement process occurs multiple times. At each timeinstant k , the following four activities occur simultaneously : • Agent B records its own position in the INS frame p B B ( k ) . • Agent A records and broadcasts its position in the globalframe p A A ( k ) . • Agent B receives the broadcast of p A A ( k ) from Agent A,and measures this signal’s DOA using instruments fixedto the UAV’s fuselage. This directional measurement istherefore naturally referenced to the body-fixed frame B . • Agent B’s attitude, i.e. orientation with respect to the INSframes B and B is known. An expression for the DOA If loss of GPS is sustained for extensive periods we recommend using thealgorithm in this paper as an initialisation for a recursive filtering algorithm. The relative speed of the UAVs with respect to the speed of light isnegligible.Fig. 1. Illustration of coordinate frames in a two-dimensional space measurement referenced to the axes INS frames B cantherefore be easily calculated.A DOA measurement, referenced to a frame with axesdenoted x, y, z , is expressed as follows: • Azimuth ( θ ): angle formed between the positive x axisand the projection of the free vector from Agent Btowards Agent A onto the xy plane. • Elevation ( φ ): angle formed between the free vector fromAgent B towards Agent A and xy plane. The angle ispositive if the z component of the unit vector towardsAgent A is positive.An illustration of azimuth and elevation components of aDOA measurement is provided in Figure 2.The problem addressed in this paper, namely the localisationof Agent B, is achieved if we can determine the relationshipbetween the global frame A and the local INS frame B .This information can be used to determine global coordinatesof Agent B at each time instant k : p A B ( k ) = [ u B ( k ) , v B ( k ) , w B ( k )] (cid:62) (3)Passing between the global frame ( A ) and the local INSframe of Agent B ( B ) is achieved by a rotation of frame axes(defined by a rotation matrix, call it R B A ) and translation t B A of frame. For instance, the coordinate vector of the positionof Agent A referenced to the INS frame of Agent B is: p B A ( k ) = R B A p A A ( k ) + t B A (4)We therefore have p A B ( k ) = R B (cid:62) A ( p B B ( k ) − t B A ) (5)where R B (cid:62) A = R A B and − R B (cid:62) A t B A = t A B . The locali-sation problem can be reduced to solving for R B A ∈ SO (3) with entries r ij and t B A ∈ R with entries t i .The matrix R B A is a rotation matrix if and only if R B A R B (cid:62) A = I and det( R B A ) = 1 . As will be seen in thesequel, these constraints are equivalent to a set of quadraticconstraints on the entries of R B A . In total there are 12 entriesof R B A and t B A to be found as we work directly with r ij . Fig. 2. Illustration of azimuth and elevation components of a DOA measure-ment
III. L
INEAR S YSTEM M ETHOD
This section presents a linear system (LS) method to solvingthe localisation problem. Given enough measurements, thelinear system approach can achieve exact localisation whenusing noiseless DOA measurements, so long as Agents A andB avoid a set of unsuitable trajectories (which are detailedin Section VI-C) in which rank-deficiency is encountered.Building on this, Section IV introduces non-linear constraintsto the linear problem defined in this section to improveaccuracy in the presence of noise.
A. Forming a system of linear equations
The following analysis holds for all k instants in time,hence we drop the argument k . The DOA measurement can berepresented by a unit vector pointing from Agent B to AgentA. This vector is defined by azimuth and elevation angles θ and φ referenced to the local INS frame B , and its coordinatesin the frame B are given by: ˆ q ( θ, φ ) = [ ˆ q , ˆ q , ˆ q ] = [cos θ cos φ, sin θ cos φ, sin φ ] (cid:62) (6)Define ¯ q . = (cid:107) p B A − p B B (cid:107) as the Euclidean distance betweenAgent A and Agent B (which is not available to either agent).Scaling to obtain the unit vector ˆ q gives ˆ q ( θ, φ ) = 1¯ q (cid:2) x A − x B , y A − y B , z A − z B (cid:3) (cid:62) (7)Applying equation (4) yields: ˆ q ˆ q ˆ q = 1¯ q r u A + r v A + r w A + t − x B r u A + r v A + r w A + t − y B r u A + r v A + r w A + t − z B (8)The left hand vector is calculated directly from DOA mea-surements. Cross-multiplying entries 1 and 3 of both vectorseliminates the unknown ¯ q , and rearranging yields: ( u A ˆ q ) r + ( v A ˆ q ) r + ( w A ˆ q ) r − ( u A ˆ q ) r − ( v A ˆ q ) r − ( w A ˆ q ) r + ( ˆ q ) t − ( ˆ q ) t = ( ˆ q ) x B − ( ˆ q ) z B (9)Similarly, from the second and third entries in (8) ( u A ˆ q ) r + ( v A ˆ q ) r + ( w A ˆ q ) r − ( u A ˆ q ) r − ( v A ˆ q ) r − ( w A ˆ q ) r + ( ˆ q ) t − ( ˆ q ) t = ( ˆ q ) y B − ( ˆ q ) z B (10) Notice that both equations (9) and (10) are linear in theunknown r ij and t i terms. Given a series of K DOA mea-surements (each giving φ ( k ) , θ ( k ) ), (9) and (10) can then beused to construct the following system of linear equations: A Ψ = b , A ∈ R K × (11)where A , b are completely known, containing θ ( k ) , φ ( k ) , p A A and p B B . The 12-vector of unknowns Ψ is defined as: Ψ = [ r r r ... r r r t t t ] (cid:62) (12)Entry-wise definitions of A and b are listed in Appendix A.These entries of Ψ can be used to reconstruct the trajectory ofAgent B in the global frame using (5), and therefore solving(11) for Ψ constitutes as a solution to the localisation problem. If K ≥ , the matrix A will be square or tall. In thenoiseless case, if A is of full column rank, equation (11) willbe solvable. B. Example of LS method in noiseless case using real flighttrajectories
We demonstrate the linear system method using trajectoriesperformed by aircraft operated by the Australian DefenceScience and Technology Group. Positions of both Agent Aand B within the global frame and Agent B within the INSframe were measured by on-board instruments, whereas wegenerated a set of calculated
DOA values using the aboverecorded real measurements.These trajectories are plotted in Figure 3. We will makeadditional use of this trajectory pair in the noisy measurementcase presented in Section IV, and in the maximum likelihoodestimation refinement of the noisy case localisation result inSection V. Extensive Monte Carlo simulations demonstratinglocalisation for a large number of realistic flight trajectoriesare left to the noisy measurement case.The rotation matrix and translation vector describing therelationship between the global frame and Agent B’s inertialnavigation frame are: R B A = . − .
032 3 . × − .
032 1 .
000 0 . − . × − − .
002 1 . (13) T B A = (cid:2) .
87 6 .
18 1 . (cid:3) (cid:62) (14)Azimuth and elevation angle measurements are tabulated inTable I. Using (11), R B A and T B A were obtained exactly forthe given flight trajectories; the solution is shown by the greenline in Figure 3.IV. S EMIDEFINITE PROGRAMMING METHOD
This section presents a semidefinite programming (SDP)method for localisation, extending from the linear system (LS)approach presented in Section III. This method reduces theminimum required number of DOA measurements to obtain aunique solution, and is more robust than LS in terms of DOAmeasurement noise and unsuitable trajectories are reduced.Rank-relaxed SDP is used to incorporate the quadratic con-straints on certain entries of Ψ arising from the properties ofrotation matrices. The inclusion of rotation matrix constraintsin SDP problems has been used previously to jointly estimatethe attitude and spin-rate of a satellite [22], and in camerapose estimation using SFM techniques when directional mea-surements are made to multiple points simultaneously [1]. Wenow apply this technique in a novel context to achieve INSalignment of Agent B, sufficient for its localisation. Finally,the Orthogonal Procrustes algorithm (O) is used to compensatefor the rank relaxation of the SDP. By realistic, we mean that the distance separation between successivemeasurements is consistent with UAV flight speeds and ensures the UAV doesnot exceed an upper bound on the turn/climb rate. Further detail is providedin Section VI-B.
TABLE IP
OSITIONS OF A GENTS A AND B, NOISELESS
DOA
MEASUREMENTS , AND
SDP+O+ML
SOLUTION FOR REAL TRAJECTORY PAIR WITH NOISY
DOA
MEASUREMENTS k p A A [m] p B B [m] p A B [m] DOA [ θ φ ] [rads] p A B using SDP+O+ML1 [349 . − . . (cid:62) [1039 . . . (cid:62) [202 . . . (cid:62) [-1.4403 0.0447] [135 . .
16 276 . (cid:62) [781 . − . . (cid:62) [1486 . . . (cid:62) [647 . . . (cid:62) [-1.4409 0.0474] [583 . . . (cid:62) [1007 . − . . (cid:62) [1946 . . . (cid:62) [1105 . . . (cid:62) [-1.6430 0.0697] [1044 . . . (cid:62) [869 . − . . (cid:62) [2140 . . . (cid:62) [1308 . . . (cid:62) [-2.0459 0.0723] [1230 . . . (cid:62) [431 . . . (cid:62) [2201 . . . (cid:62) [1383 . . . (cid:62) [-2.2708 0.0464] [1279 . . . (cid:62) [33 . − . . (cid:62) [2032 . . . (cid:62) [1224 . . . (cid:62) [-2.1512 0.0317] [1101 . . . (cid:62) A. Quadratic constraints on entries of Ψ Rank-relaxed semidefinite programming (in the presenceof inexact or noise contaminated data) benefits from theinclusion of quadratic constraint equations. We now identify21 quadratic and linearly independent constraint equations onentries of R B A , which all appear in Ψ in (12). Recall theorthogonality property of rotation matrices R B A R B A (cid:62) = I .By computing each entry of R B A R B A (cid:62) , setting these equalto entries of I , and referencing the i th entry of Ψ as ψ i , wedefine constraints: C = ψ + ψ + ψ − (15a) C = ψ + ψ + ψ − (15b) C = ψ + ψ + ψ − (15c) C = ψ ψ + ψ ψ + ψ ψ = 0 (15d) C = ψ ψ + ψ ψ + ψ ψ = 0 (15e) C = ψ ψ + ψ ψ + ψ ψ = 0 (15f)To simplify notation we call C j : k the set of constraints C i for i = j, .., k . Similarly, by computing each entry of R B A (cid:62) R B A and setting these equal to I , we define constraints C .We omit presentation of C due to space limitations andsimilarity with C . The sets C and C are clearlyequivalent.Further constraints are required to ensure det( R B A ) = 1 .Cramer’s formula states that R B A − = adj ( R B A ) / det( R B A ) ,where adj ( R B A ) denotes the adjugate matrix of R B A . Or-thogonality of R B A implies R B A (cid:62) = adj ( R B A ) or that R B A = adj ( R B A ) (cid:62) . By computing entries of the first columnof Z = R B A − adj ( R B A ) (cid:62) and setting these equal to , wedefine constraints C : C = ψ − ( ψ ψ − ψ ψ ) = 0 (16a) C = ψ − ( ψ ψ − ψ ψ ) = 0 (16b) C = ψ − ( ψ ψ − ψ ψ ) = 0 (16c)Similarly, by computing the entries of the second and thirdcolumns of Z and setting these equal to , we define con-straints C and C respectively. Due to space limita-tions, we omit presenting them. The complete set C . = C Ψ constrains R B A to be a rotation matrix. The set of constraintsis not an independent set, e.g. C is equivalent to C . Thebenefits of the inclusion of dependent constraints is discussedfurther in Section IV-C. As we will show below, due to these additional relations,localisation requires azimuth and elevation measurements at aminimum of 4 instants only ( K = 4 ), as opposed to 6 instantsrequired in Section III. B. Formulation of the Semidefinite Program
The goal of the semidefinite program is to obtain: argmin Ψ || A Ψ − b || (17)subject to C Ψ . Equivalently, we seek argmin Ψ || A Ψ − b || subject to C Ψ . We define the inner product of two matrices U and V as (cid:104) U , V (cid:105) = trace ( U V (cid:62) ) . One obtains || A Ψ − b || = (cid:104) P , X (cid:105) (18)where P = (cid:2) A b (cid:3) (cid:62) (cid:2)
A b (cid:3) and X = [ Ψ (cid:62) − (cid:62) [ Ψ (cid:62) − and X is a rank 1 positive-semidefinite matrix . The con-straints C Ψ can also be expressed in inner product form. For i = 1 , ..., , C i = 0 is equivalent to (cid:104) Q i , X (cid:105) = 0 forsome easily determined Q i . Solving for Ψ in (17) is thereforeequivalent to solving for: argmin Ψ (cid:104) P , X (cid:105) (19) X ≥ (20)rank ( X ) = 1 (21) X , = 1 (22) (cid:104) Q i , X (cid:105) = 0 i = 1 , ..., (23) C. Rank Relaxation of Semidefinite Program
This semidefinite program is a reformulation of a quadrati-cally constrained quadratic program (QCQP). Computationallyspeaking, QCQP problems are generally NP-hard. A closeapproximation to the true solution can be obtained in polyno-mial time if the rank 1 constraint on X , i.e. (22), is relaxed.A full technical explanation of semidefinite relaxation, anddiscussion on its applicability can be found in [17]. Thisrelaxation significantly increases the dimension of the SDPsolver’s co-domain. A notable consequence is that dependentconstraints which are linearly independent over R within C Ψ ,such as sets C and C , cease to be redundant whenexpressed in inner-product form and applied to entries of X . Hypothesis testing using extensive simulations confirmed All matrices M which can be expressed in the form of M = v (cid:62) v where v is a row vector are positive-semidefinite matrices. with confidence above 95% that inclusion of quadraticallydependent constraints causes an improvement in localisationaccuracy.The solution to the relaxed semidefinite program X istypically close to being a rank 1 matrix when DOA mea-surements are noisy. The closest rank 1 approximation to X ,which we call ˆ X , is obtained by evaluating the singular valuedecomposition of X , then setting all singular values except thelargest equal to zero. From ˆ X , one can then use the definitionof X to obtain the approximation of Ψ , which we will call ˆΨ . Entries ˆ ψ i for i = 10 , , can be used immediately toconstruct an estimate for t B A , which we will call t . Entries ˆ ψ i for i = 1 , ..., will be used to construct an intermediateapproximation of R B A , which we call (cid:98) R , and which we willrefine further. D. Orthogonal Procrustes Algorithm
Due to the relaxation of the rank constraint (23) on X , itis no longer guaranteed that entries of ˆΨ strictly satisfy theset of constraints C Ψ . Specifically, the matrix (cid:98) R may not bea rotation matrix. The Orthogonal Procrustes algorithm is acommonly used tool to determine the closest orthogonal matrix(denoted R ) to a given matrix, (cid:98) R . This is given by R =argmin Ω || Ω − (cid:98) R || F , subject to ΩΩ (cid:62) = I , where || . || F is theFrobenius norm.When noise is high, the above method occasionally returns R such that det( R ) = − . In this case, we employ a specialbut standard case of the Orthogonal Procrustes algorithm [10]to ensure we obtain rotation matrices and avoid reflections byflipping the last column in one of the unitary matrix factorsof the singular value decomposition.The matrix R and vector t are the final estimates of R B A and t B A using semidefinite programming and the OrthogonalProcrustes algorithm. The estimate of Agent B’s position inthe global frame is p A B = R (cid:62) ( p B B − t ) .For convenience, we use SDP+O to refer to the processof solving a rank-relaxed semidefinite program, and thenapplying the Orthogonal Procrustes algorithm to the result. E. Example of SDP+O method with noisy DOA measurements
In this subsection, we apply the SDP+O method to performlocalisation in a noisy DOA measurement case using thereal trajectory example from Section III. A popular practicefor performing DOA measurements from Agent B towardsAgent A is to use fixed RF-antennas and/or optical sensorson board Agent B’s airframe. The horizontal RF antennatypically has a larger aperture (generally around 4 times) thanthe vertical RF antenna. This is due to the typical ratio of afixed-wing UAV’s wingspan to its fuselage height. As a result,errors in azimuth and elevation measurements, referenced tothe body-fixed frame B , are modelled by independent zero-mean Gaussian distributed variables with different standarddeviations, denoted σ Θ and σ Φ . The measure used for closeness to rank 1 is the ratio of the two largestsingular values in the singular value decomposition of X . z a l t i t ude [ m e t r e s ] x coordinate [metres] y coordinate [metres]
00 -500-500 P AA (global coordinates of A)P BA (global coordinates of B)LS+O (noiseless)SDP+O ( =0.5 ° , =2 ° )SDP+O+ML ( =0.5 ° , =2 ° ) Fig. 3. Recovery of global coordinates of Agent B for recorded trajectories.Errors are σ θ = 0 . ◦ and σ φ = 2 ◦ with respect to body fixed frame for theDOA measurements We now add noise (for the purposes of this simulationexample based on the real data) to the calculations of body-fixed frame azimuth and elevation components of DOA.Strictly speaking, each noise is expected to follow a von Misesdistribution, which generalises a Gaussian distribution to acircle [11]. For small noise, as often encountered, the vonMises distribution can be approximated by a Gaussian distri-bution. In this example we assume body-fixed frame azimuthand elevation measurement errors have standard deviations of σ Θ = 0 . ◦ and σ Φ = 2 ◦ .Samples of Gaussian error with these standard deviationswere added to body-fixed frame ( B ) elevation and azimuthmeasurements calculated as described in Section III. Thesewere converted to DOA measurements referenced to the INSframe B . The SDP+O algorithm was used to obtain R and t using the agents’ position coordinates in their respective nav-igation frames and the noisy DOA values. The reconstructedtrajectory p A B is plotted in Figure 3 with the dotted black line.Position data of the reconstructed trajectory p A B are tabulatedin I. Remark 1.
The accuracy of the SDP+O solution in thenoiseless case was observed to deteriorate when the conditionnumber of the true solution for X is high. This is due toa form of inherent regularisation in the SDP solver Yalmip[16]. When the approximate magnitude of the norm || t B A || is known, one approach is to straightforwardly introduce ascaling coefficient before entries t i for i = 1 , , in equations(9) and (10) equal to the approximate norm of || t B A || .Furthermore, if an approximation of t B A is known a priorias (cid:101) t B A , the following controlled shifting algorithm may beapplied to reduce the effect of regularisation error. We defineshifted positions p sB B ( k ) = p B B ( k ) − (cid:101) t B A . By substituting p sB B for p B B in the SDP, the vector t obtained through SDPis an estimate of t B A − (cid:101) t B A . V. M
AXIMUM L IKELIHOOD E STIMATION
This section presents a maximum likelihood estimation(ML) method to refine estimates R and t obtained using the SDP+O algorithm. The MLE refinement uses the series ofDOA measurements expressed with respect to the body-fixedframe B , and the known values for σ Θ and σ Φ describingthe expected distribution of DOA errors. A non-linear log-likelihood function for DOA measurement error is derived.The minimum of the log-likelihood function cannot be foundanalytically, so we employ an iterative gradient descent ap-proach instead. A. Likelihood Function Derivation
In this section, DOA values are always expressed withrespect to the body-fixed frame of Agent B ( B ) to exploit theindependence of azimuth and elevation measurement errors.This is a change from Sections III and IV, in which DOAmeasurements were generally expressed with respect to thelocal INS frame B . The transformation between coordinateframes B and B is known to Agent B.Suppose body-fixed frame measurements of azimuth andelevation Θ( k ) and Φ( k ) are contaminated by zero meanGaussian noise as follows: • (cid:101) Θ( k ) = Θ( k ) + ξ Θ , ξ Θ ∼ N (0 , σ Θ2 ) • (cid:101) Φ( k ) = Φ( k ) + ξ Φ , ξ Φ ∼ N (0 , σ Φ2 ) To calculate noiseless azimuth and elevation measurements,an expression must be derived for the position of Agent A inAgent B’s body-fixed frame B . Observe that p B A ( k ) = R B B ( k )( R B A p A A ( k ) + t B A ) + t B B ( k ) (24)To help distinguish coordinate reconstructions based on es-timates of R and t from true coordinates, reconstructedpositions will be explicitly expressed as functions of R and t : p B A ( k, R , t ) = R B B ( k )( Rp A A ( k ) + t ) + t B B ( k ) (25)By definition of azimuth and elevation in Section II: θ B ( k, R , t ) = arcsin (cid:16) p B A ( k, R , t ) z || p B A ( k, R , t ) || (cid:17) (26) φ B ( k, R , t ) = atan2 (cid:16) p B A ( k, R , t ) y , p B A ( k, R , t ) x (cid:17) (27)where p B A = [ p B A x , p B A y , p B A z ] (cid:62) . The likelihood functionfor the set of DOA measurements is defined as follows: L ( p A A , p B B | R , t )= 1 σ Θ √ π K (cid:89) k =1 exp (cid:104) − ( (cid:101) θ B ( k ) − θ B ( k, R , t )) σ (cid:105) × σ Φ √ π K (cid:89) k =1 exp (cid:104) − ( (cid:101) φ B ( k ) − φ B ( k, R , t )) σ (cid:105) (28)It can be shown that maximising L ( p A A , p B B | R , t ) is equiv-alent to minimising K (cid:88) k =1 (cid:104) ( (cid:101) θ B ( k ) − θ B ( k, R , t )) σ + ( (cid:101) φ B ( k ) − φ B ( k, R , t )) σ (cid:105) (29) B. Gradient descent and adaptive step size
Possible parametrisations for the rotation matrix R includeEuler angles, quaternion representation and Rodrigues rotationformula. In this paper we parametrise R by a 3-vector of Eulerangles, and t is a 3-vector. This defines a mapping from R → R , t , and the gradient of (29) can be expressed as a vector in R . The log-likelihood function is non-linear with respect tothis R parametrisation of R and t . As a result, this functionmay be non-convex, meaning the equation D log L = 0 mayhave multiple solutions, with only one of these being the globalminimum. A gradient descent algorithm is therefore initialisedusing the result of the SDP+O method, and is used to convergetowards a local minimum, which it is hoped will be the globalminimum or close to it.Instead of selecting a constant step size, which may lead toovershooting local minima or excessive computation time, anadaptive step size approach is adopted. The backtracking linesearch algorithm discussed in [24] selects an optimally largestep size satisfying a constraint placed on the average gradientover the step. C. Example of ML refinement of SDP+O solution
In this subsection, we demonstrate the benefits of maximumlikelihood estimation. ML was performed using the real flighttrajectory data presented in Sections III and IV. The resultingreconstructed trajectory p B A is presented in Figure 3 as thesolid black line, and its coordinates are tabulated in TableI. Additionally, in this section we present the decrease andconvergence in the value of the negative log-likelihood func-tion (29), frame rotation error and reconstructed position error over successive iterations of the gradient descent algorithm inFigures 4, 5 and 6.The error in INS frame rotation is reduced by over 60%,and the reconstructed position error of Agent B is reducedby over 70% by iterating the gradient descent algorithm.This represents a significant gain with respect to the SDP+Oestimate, which served as the initialisation point of the gradientdescent. Monte Carlo simulations covering a large set oftrajectories are presented in Section VI.VI. S IMULATION R ESULTS
In this section, metrics are defined for performance evalu-ation of the localisation algorithm. Monte Carlo simulationsof realistic flight trajectories are performed to evaluate theeffect of errors in body-fixed frame azimuth and elevationmeasurements. We also investigate the expected incrementalimprovement in localisation accuracy as the number of DOAmeasurements K increases. Finally, trajectories of Agents Aand B which are unsuitable for localisation of Agent B arediscussed.In the preliminary conference paper [21], we comparedthe performance of the LS+O and SDP+O methods TheLS+O method collapsed when small amounts of noise wereintroduced to DOA measurements, whereas rotation error Metrics are defined in the sequel, see Section VI-A below. These trajectories satisfy a set of assumptions detailed in Section VI-B.
Gradient Descent Iteration -2 -1 F un c t i on v a l ue Fig. 4. Convergence of negative log-likelihood function using ML for realtrajectory pair
Gradient Descent Iteration R o t a t i on e rr o r ( deg r ee s ) Fig. 5. Improvement in rotation error in degrees using ML for real trajectorypair increased linearly with respect to DOA measurement noisewhen using the SDP+O method. The SDP+O method is thesuperior method, and there is no reason to employ LS+O.
A. Metrics for error in R and t This paper uses the geodesic metric for rotation [14]. Allsequences of rotations in three dimensions can be expressedas one rotation about a single axis [19]. The geodesic metricon SO (3) defined by d ( R , R ) = arccos (cid:18) tr ( R (cid:62) R ) − (cid:19) (30)is the magnitude of angle of rotation about this axis [27].Where R BA is known, the error of rotation R is defined as d ( R , R BA ) . Position error is defined as the average Euclidiandistance between true global coordinates of Agent B, andestimated global coordinates over the K measurements taken,divided (to secure normalisation) by the average distancebetween aircraft. p A B ( k ) = R (cid:62) ( p B B − t ) (31) error ( p A B ) = (cid:80) k || p A B ( k ) − p A B ( k ) || Kd (32)where d represents the average distance between aircraft. Gradient Descent Iteration P o s i t i on e rr o r ( no r m a li s ed ) Fig. 6. Improvement in reconstructed position error using ML for realtrajectory pair
Number of DOA measurements (K) M ed i an po s i t i on e rr o r σ θ = 0.1 ° , M σ θ = 0.1 ° , S σ θ = 1 ° , M σ θ = 1 ° , S σ θ = 2 ° , M σ θ = 2 ° , S Fig. 7. Median error ( p A B ) vs. number of DOA measurements used to solveSDP+O (S) and SDP+O+ML (M) from K = 2 to K = 20 . B. Monte Carlo Simulations on Random Trajectories usingSDP+O and ML
In this subsection, we summarise the results of MonteCarlo simulations to evaluate the expected performance of theSDP+O method and the SDP+O+ML method.Pairs of realistic trajectories for Agents A and B aregenerated in accordance with the following assumptions: • Initial horizontal separation of 800m • Initial vertical separation of 50m (initial altitudes of 300and 350 metres) • Average speed of 50 metres per second (97 knots)
Number of DOA measurements (K) M ed i an ang l e e rr o r i n deg r ee s σ θ = 0.1 ° , M σ θ = 0.1 ° , S σ θ = 1 ° , M σ θ = 1 ° , S σ θ = 2 ° , M σ θ = 2 ° , S Fig. 8. Median d ( R , R BA ) vs. number of DOA measurements used to solveSDP+O (S) and SDP+O+ML (M) from K = 2 to K = 20 • Measurements taken every 5 seconds • Initial compass heading of each UAV in the xy planefollows a uniform distribution from 0 to 360 degrees.This is expressed as h ∼ U (0 , degrees • Every 5 seconds, the recorded direction of each UAV inthe xy plane changes by δ degrees where δ ∼ N ( c, ) .The value c ∼ U ( − , is constant for a giventrajectory and corresponds to an average curve for atrajectory • At each time measured, the rate of climb of each UAVin degrees is
RoC ∼ N (0 , ) To represent the drift in the INS of Agent B, rotations R B A were generated by independently sampling three Euler angles α, β, γ where α, β, γ ∼ U ( − π, π ) , and translations t B A =[ t , t , t ] (cid:62) were generated by sampling entries t , t , t ∼ U ( − , .As discussed in Section IV-E, we assume the standarddeviation in elevation measurement error in the body-fixedframe B is four times the standard deviation in azimuthmeasurement error in B , i.e. σ Φ = 4 σ Θ . We vary the DOAerror by σ Θ = [0 . ◦ , ◦ , ◦ ] . Errors in the order of σ Θ = 0 . ◦ are representative of an optical sensor, whereas the largererrors are representative of antenna-based (RF) measurements.For each value of σ Θ studied, and for each number of DOAmeasurements K from 2 to 20, we simulated 100 uniqueand realistic UAV trajectory pairs (Agent A and Agent B).For each trajectory pair, localisation was performed using theSDP+O method, and metrics d ( R , R BA ) and error ( P A B ) ,were calculated. The ML method was then used to enhance theresult of the SDP+O method, and the error metrics were re-calculated. After all simulations were completed, the median values of d ( R , R BA ) and error ( P A B ) for both the SDP+Oand SDP+O+ML methods were calculated across each set 100simulations. The results of the Monte Carlo simulations areplotted in Figures 7 and 8.Median d ( R , R BA ) and error ( P A B ) errors decrease signif-icantly when 4 or more DOA measurements ( K ) are used.Both metrics show an asymptotic limit to performance acrossall three noise levels as the number of DOA measurements ( K )increases. Median rotation error d ( R , R BA ) and error ( p A B ) appear to exhibit similar asymptotic performance gain over thenumber of DOA measurements K up to 20. Remark 2.
The maximal parametrisation for R , t consistsof 12 entries, and the largest set of independent quadraticconstraints consists of 6 relations. Polynomial equation sets of n independent relations in n unknowns will generically havemultiple solutions if some relations are quadratic. The additionof a scalar measurement generically yields a unique solution.Therefore 4 DOA measurements are required to obtain theminimum of (12 −
6) + 1 = 7 scalar measurements.C. Unsuitable trajectories for localisation
In this subsection we are motivated to identify trajectoriesof Agents A and B which may lead to multiple solutions for For asymmetric distributions such as errors (which are nonnegative bydefinition and contain extreme outliers), the median is a superior measure ofcentral tendency than the mean [7, 13]. a l t i t ude [ m ] y coordinate [m]
100 0 300 x coordinate [m]
Fig. 9. Illustration of trajectory pairs leading to equal DOA measurements(disconnected blue lines) over K measurement instants. SDP+O+ML algo-rithm unable to discern distance from which Agent B (solid blue) observesAgent A (red). a l t i t ude [ m ] x coordinate [m] -200-1000 y coordinate [m] Fig. 10. Illustration of straight line motion of Agent A (red, trajectory given by ( x ( t ) , y ( t ) , z ( t )) = (100 t, , ). Agent B (blue) observes Agent A throughan unaligned INS frame. In this figure, the solid blue trajectory is the actualpath of Agent B. However, each dotted blue line is also an admissible solution. R and t in the noiseless case, and consequently unreliablesolutions in the noisy case. One can prove that if Agent A’smotion is restricted to a plane, a set of three columns of matrix A in Eqn. (11) become linearly dependent, and therefore aunique solution cannot be obtained using the LS method.When quadratic constraints are included in the SDP+Omethod, rank deficiency of A no longer automatically impliesthe existence of multiple solutions. For example, the SDP+Omethod obtains a unique solution if Agent A’s motion is planarand Agent B’s motion is arbitrary. Multiple solutions never-theless can exist for certain non-generic unsuitable trajectoriesfor the SDP+O method.We begin by considering the case where DOA measure-ments expressed with respect to the Local INS frame B areequal at each time instant. This is illustrated by an example inFigure 9. A similar problem is expected in the far field case,where the distance between Agents A and B is sufficientlylarge that DOA measurements become approximately equaldespite each Agent’s trajectory remaining arbitrary. In thesecases, multiple solutions exist for t B A .Multiple solutions may also arise if Agent A’s trajectoryappears similar from multiple perspectives. The localisationprocess may be incapable of determining the direction fromwhich DOA measurements were taken with respect to theglobal frame. For example, if Agent A follows a straight line,a set of recorded DOA measurements may be achieved by viewing Agent A from any direction in a circle perpendicularto Agent A’s motion and centred at Agent A’s trajectory. Thisis illustrated in Figure 10.VII. T HREE -A GENT E XTENSION AND B EYOND
This section explores in a preliminary way how theSDP+O+ML algorithm may be extended to localise two GPS-denied agents efficiently. Trivially, each GPS-denied aircraftcould measure DOA of the GPS-equipped agent’s broadcast ofits position, and use the SDP+O+ML algorithm independentlyof each other to estimate drift in their local frames. Weare motivated to determine whether a trilateral algorithmmay be more resilient to DOA measurement error and/orunsuitable trajectories, and may perhaps require fewer DOAmeasurements from each aircraft than simply repeating thetwo-agent localisation algorithm with each GPS-denied agent.We introduce a GPS-denied Agent C, whose local INS framehas rotation and translation parameters R C A and t C A withrespect to the global frame. We conclude this section bydiscussing the challenges involved in generalising our findingsto arbitrary n -agent networks. A. Measurement process in three-agent network
To describe measurements within a network of more thantwo agents, one minor notation change is required: • DOA measurements made by Agent I towards Agent Jwill henceforth be expressed in the INS coordinate frameof Agent I as ( θ JI , φ JI )At each time instant k in the discrete-time process: • Agents A and B interact as per the two-agent case. • Agent C receives the broadcast of Agent A’s globalcoordinates, and measures this signal’s DOA with respectto frame C , which we denote ( θ AC , φ AC ). • Agent C broadcasts its position with respect to its INSframe p C C , as well as the measurement ( θ AC , φ AC ) toAgent B, who also takes a DOA measurement towardsAgent C. This measurement is denoted ( θ CB , φ CB ).All DOA and position measurements are therefore relayedto Agent B, who performs the localisation algorithm presentedin this section. B. Forming system of linear equations in three-agent network
In Section III, the linear system A Ψ = b was formed usingrelations stemming from the collinearity of the vector ( p B A − p B B ), and the vector in the direction of DOA measurement( θ AB , φ AB ). We refer to this system of equations as S AB ,where the subscript references the agents involved. A similarsystem S AC can be constructed independently using AgentC’s DOA measurements towards Agent A and p C C .In the three-agent network, Agent B also measures theDOA towards Agent C’s broadcast, with respect to AgentB’s local INS frame B . To exploit the collinearity of thevectorial representation of the DOA measurement ( θ CB , φ CB ) In this section we relax the condition preventing GPS-denied agents frombroadcasting signals and ( p B C − p B B ), an expression for the position coordinatevector p B C is required. As achieved in equations (7) and (8)in Section III, this position may be expressed in terms ofentries of R B C and t B C , and the linear system S BC maybe defined similarly to S AB in Section III. Systems S AB , S AC and S BC can be assembled, forming a large system oflinear equations S ABC with 36 scalar unknowns (9 rotationmatrix entries and 3 translation vector entries for each distinctagent pair).At each time instant k for k = 1 , ..., K , two linear equationsare obtained from each DOA measurement of ( θ AB , φ AB ),( θ AC , φ AC ) and ( θ C B , φ C B ). As a result, 6 linear equationsare obtained at each time instant. Performing the measurementprocess 6 times ( K = 6) produces 36 linear equations. Gener-ically, in the noiseless case, a unique solution therefore existsfor K = 6 time instants. When using only the LS method, thethree-agent localisation problem requires the same minimumnumber of time instants as solving two independent two-agentlocalisation problems concurrently, yet requires more DOAmeasurements than the sum of the number of measurementsrequired in two separate two-agent localisation problems.However, quadratic relationships between R B A , t B A , R C A , t C A , R C B and t C B significantly reduce the required numberof time instants ( K ) at which measurements occur. C. Quadratic constraints in three-agent network and example
In the two-agent case, 21 linearly independent quadraticconstraints were identified in order to determine the rotationand translation between two frames. In the three-agent case,when the underlying undirected graph is a clique, we identify × linearly independent quadratic constraints usingrotation matrix properties of R B A , R C A , and R C B .In the three-agent case, additional quadratic constraintscan be identified due to relationships between rotated andtranslated reference frames. • Applying the rotation R C A is equivalent to applyingrotations R B A and R C B successively. This relationshipis expressed in equation (33). Setting the entries of theleft hand side to zero yields 9 quadratic constraints. • The difference between the global frame representationof vectors t C A and t B A is equal to the global framerepresentation of t C B . This relationship is expressed inequation (34). Setting entries of the left hand side to zeroyields 3 quadratic constraints. R C A − R C B R B A = (33) ( R A C t C A C − R A B t B A ) − R A C t C B = (34)As mentioned in Section IV-C, dependent constraints whichare not linearly dependent are included to improve the accu-racy of the SDP solver. The relationship between rotationsdescribed in equation (33) may be expressed in 3 distinctcoordinate frames ( A , B and C ), and hence × linearly independent quadratic constraints may be derivedusing the relationship in equation (33). Similarly, the rela-tionship between translations described in equation (34) maybe expressed in 3 distinct coordinate frames ( A , B and C ), z a l t i t ude [ m e t r e s ]
800 500 y coordinate [metres] x coordinate [metres] P AA P BA P CA Result P BA Result P CA Fig. 11. Illustration of example of successful localisation within three-agentnetwork in noiseless case for K = 3 and hence × linearly independent quadratic constraintsmay be derived using the relationship in equation (34). In total,we have derived (21 ×
3) + (9 ×
3) + (3 ×
3) = 99 linearlyindependent quadratic constraints for a system of 36 unknownvariables. These constraints may be expressed in inner-productform as performed in Section IV.Rank-relaxed semidefinite programming can be used toobtain solutions for each INS frame’s rotation and translationwith respect to the global frame, and the Orthogonal Procrustesalgorithm can be applied to each individual resulting rotationmatrix. This defines the three-agent SDP+O method.To illustrate successful localisation in the three-agent case,realistic trajectories were defined for Agents A, B and Cfor K = 3 time instants. These are presented in Figure11. Only Agents B and C were assigned random INS framerotations and translations as prescribed in Section VI, andthe three-agent SDP+O method was used to obtain estimatesof R B A , t B A , R C A and t C A . Each directional measurementconsists of two scalar measurements, and hence a total of × × K = 18 scalar measurements were obtained. Locali-sation was successful, which demonstrates that only 3 timeinstants ( K = 3 ) are required for the three-agent SDP+Oalgorithm to obtain the exact solution in the noiseless case.Earlier, it was established that a minimum of 6 time instantswere required to achieve a unique solution in the three-agentcase using LS+O, and a minimum of 4 time instants wererequired to achieve a unique solution in the two-agent caseusing SDP+O. We have therefore demonstrated that a trilateralalgorithm can achieve localisation of two GPS-denied agentsin less measurement time instants than applying the bilateralalgorithm twice independently. We note that this extension tothree-agents is not applicable if the measurement graph is atree because measurements are required between each pair ofagents within the three-agent network. D. Challenges in extension to n -agent networks While the results in Section VII-C demonstrate that locali-sation of two agents may be achieved in fewer measurementtime instants K when a three-agent extension is used, formal-ising an extension to arbitrary n -agent networks presents asignificant theoretical challenge. There exist comprehensive works such as [31] and [29]on bearing rigidity of an arbitrary network in R where allagents share the same reference frame, and some concepts aregeneralised to R n in [32]. However, bearing rigidity theory fornetworks in R n when agents do not share a reference frameis comparatively underdeveloped when compared to distance-based rigidity. For example, a general theory does not exist forthe minimum number of measurements required for rigidity.We also note the risk of an explosion in computationalcomplexity when extending our algorithm to n -agent networks.The relative pose of INS frames of any two agents linked byan edge in the underlying undirected measurement graph mustbe determined in order to use Eqn. (8). We cannot substituteentries of Ψ relating to the relative pose of two INS frames(such as entries of R C B and t C B in the three-agent case) withassociated quadratic expressions using relationships such asEqn. (38) or Eqn. (39), or else the objective in Eqn. (15)will cease to be quadratic and the SDP method will not beapplicable. As a result, extending the algorithm to a largenumber of agents risks an exponential increase in the numberof variables to be determined. It is not clear exactly howmany measurements between the n agents would be requiredto obtain a result in the noiseless case.VIII. C ONCLUSION
This paper studied a cooperative localisation problem be-tween a GPS-denied and a GPS-enabled UAV. A localisa-tion algorithm was developed in two stages. We showedthat a linear system of equations built from six or moremeasurements yielded the localisation solution for generictrajectories. The second stage considered the inclusion ofquadratic constraints due to rotation matrix constraints. Rankrelaxed semidefinite programming was used, and the solutionadjusted using the Orthogonal Procrustes algorithm. This gavethe algorithm greater resilience to noisy measurements andunsuitable trajectories. Maximum likelihood estimation wasthen used to improve the algorithm’s results. Simulations werepresented to illustrate the algorithm’s performance. Finally,an approach was outlined to extend the two-agent solutionto a three agent network in which only one agent has globallocalisation capacity. Future work may include implementationon aircraft to perform localisation in real time and validateour Monte Carlo analysis on measurement noise. We alsohope to extend our trilateral algorithm to larger networks byestablishing further theory on bearing rigidity when agents donot share a common reference frame.R
EFERENCES [1] M. Arie-Nachimson, S. Z. Kovalsky, I. Kemelmacher-Shlizerman, A. Singer, and R. Basri. Global motionestimation from point matches. In
Proceedings of the2012 Second International Conference on 3D Imaging,Modeling, Processing, Visualization & Transmission .[2] H. Bai and R. W. Beard. Relative heading estimation fortarget handoff in gps-denied environments. In
AmericanControl Conference (ACC), 2016 , pages 336–341. IEEE,2016. [3] G. Balamurugan, J. Valarmathi, and V. Naidu. Surveyon uav navigation in gps denied environments. In SignalProcessing, Communication, Power and Embedded Sys-tem (SCOPES), 2016 International Conference on , pages198–204. IEEE, 2016.[4] P. Batista, C. Silvestre, and P. Oliveira. Navigationsystems based on multiple bearing measurements.
IEEETransactions on Aerospace and Electronic Systems , 51(4):2887–2899, Oct 2015.[5] H. Bayram, J. V. Hook, and V. Isler. Gathering bearingdata for target localization.
IEEE Robotics and Automa-tion Letters , 1(1):369–374, Jan 2016.[6] A. N. Bishop, B. Fidan, B. D. O. Anderson, K. Dogancay,and P. N. Pathirana. Optimality analysis of sensor-targetgeometries in passive localization: Part 1 - bearing-onlylocalization. In
Intelligent Sensors, Sensor Networksand Information, 2007. ISSNIP 2007. 3rd InternationalConference on .[7] S. Boslaugh.
Statistics in a nutshell: A desktop quickreference . O’Reilly Media, Inc., 2012.[8] K. Dogancay. Self-localization from landmark bearingsusing pseudolinear estimation techniques.
IEEE Transac-tions on Aerospace and Electronic Systems , 50(3):2361–2368, July 2014.[9] Y. Duan, R. Ding, and H. Liu. A probabilistic method ofbearing-only localization by using omnidirectional visionsignal processing. In
Intelligent Information Hiding andMultimedia Signal Processing (IIH-MSP), 2012 EighthInternational Conference on , pages 285–288, July 2012.[10] D. Eggert, A. Lorusso, and R. Fisher. Estimating 3-drigid body transformations: a comparison of four majoralgorithms.
Machine Vision and Applications , 9(5):272–290, Mar 1997.[11] C. Forbes, M. Evans, N. Hastings, and B. Peacock.
Statistical Distributions . John Wiley & Sons, 2011.[12] M. Gavish and A. J. Weiss. Performance analysis ofbearing-only target location algorithms.
IEEE Transac-tions on Aerospace and Electronic Systems , 28(3):817–828, Jul 1992.[13] H. L. Harter. The method of least squares and somealternatives: Part ii.
International Statistical Review/ Revue Internationale de Statistique , 42(3):235–282,1974.[14] D. Q. Huynh. Metrics for 3d rotations: Comparison andanalysis.
Journal of Mathematical Imaging and Vision ,35(2):155–164, 2009.[15] J. J. Koenderink and A. J. van Doorn. Affine structurefrom motion.
J. Opt. Soc. Am. A , 8(2):377–385, Feb1991.[16] J. L¨ofberg. Yalmip : A toolbox for modeling andoptimization in matlab. In
In Proceedings of the CACSDConference , Taipei, Taiwan, 2004.[17] Z. Luo, W. Ma, A. M. So, Y. Ye, and S. Zhang. Semidefi-nite relaxation of quadratic optimization problems.
IEEESignal Processing Magazine , 27(3):20–34, May 2010.[18] J. Morales, P. Roysdon, and Z. Kassas. Signals ofopportunity aided inertial navigation. In
Proceedings ofION GNSS Conference , pages 1492–1501, 2016. [19] B. Palais, R. Palais, and S. Rodi. A disorienting lookat euler’s theorem on the axis of a rotation.
AmericanMathematical Monthly , 116(10):892–909, 2009.[20] J. Reis, P. Batista, P. Oliveira, and C. Silvestre. Sourcelocalization based on acoustic single direction measure-ments.
IEEE Transactions on Aerospace and ElectronicSystems , pages 1–1, 2018.[21] J. S. Russell, M. Ye, B. D. Anderson, H. Hmam, andP. Sarunic. Cooperative localisation of a gps-denieduav in 3-dimensional space using direction of arrivalmeasurements.
IFAC-PapersOnLine , 50(1):8019 – 8024,2017. 20th IFAC World Congress.[22] J. Saunderson, P. A. Parrilo, and A. S. Willsky. Semidef-inite relaxations for optimization problems over rotationmatrices. In , pages 160–166, Dec 2014.[23] S. Sohn, B. Lee, J. Kim, and C. Kee. Vision-based real-time target localization for single-antenna gps-guideduav.
IEEE Transactions on Aerospace and ElectronicSystems , 44(4):1391–1401, Oct 2008.[24] P. S. Stanimirovi´c and M. B. Miladinovi´c. Acceleratedgradient descent methods with line search.
NumericalAlgorithms , 54(4):503–520, 2010.[25] L. G. Taff. Target localization from bearings-only obser-vations.
IEEE Transactions on Aerospace and ElectronicSystems , 33(1):2–10, Jan 1997.[26] O. Tekdas and V. Isler. Sensor placement fortriangulation-based localization.
IEEE Transactions onAutomation Science and Engineering , 7(3):681–685, July2010.[27] R. Tron, J. Thomas, G. Loianno, K. Daniilidis, andV. Kumar. A distributed optimization framework for lo-calization and formation control: Applications to vision-based measurements.
IEEE Control Systems , 36(4):22–44, Aug 2016.[28] M. Ye, B. D. O. Anderson, and C. Yu. Bearing-onlymeasurement self-localization, velocity consensus andformation control.
IEEE Transactions on Aerospace andElectronic Systems , 53(2):575–586, April 2017.[29] D. Zelazo, A. Franchi, and P. R. Giordano. Rigiditytheory in se(2) for unscaled relative position estimationusing only bearing measurements. , pages 2703–2708, 2014.[30] L. Zhang, M. Ye, B. D. O. Anderson, P. Sarunic, andH. Hmam. Cooperative localisation of uavs in a gps-denied environment using bearing measurements. In , pages 4320–4326, Dec 2016.[31] S. Zhao and D. Zelazo. Bearing rigidity and almostglobal bearing-only formation stabilization.
IEEE Trans-actions on Automatic Control , 61(5):1255–1268, May2016.[32] S. Zhao and D. Zelazo. Localizability and distributedprotocols for bearing-based network localization in arbi-trary dimensions.
Automatica , 69:334 – 341, 2016. A PPENDIX
A. Forms of A and b The matrix A is defined in Section III-A as follows: A (2 k − ,
1) = u A ( k ) sin( φ ( k )) A (2 k − ,
2) = v A ( k ) sin( φ ( k )) A (2 k − ,
3) = w A ( k ) sin( φ ( k )) A (2 k − ,
4) = 0 A (2 k − ,
5) = 0 A (2 k − ,
6) = 0 A (2 k − ,
7) = − u A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − ,
8) = − v A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − ,
9) = − w A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − ,
10) = sin( φ ( k )) A (2 k − ,
11) = 0 A (2 k − ,
12) = − cos( θ ( k )) cos( φ ( k )) A (2 k,
1) = 0 A (2 k,
2) = 0 A (2 k,
3) = 0 A (2 k,
4) = u A ( k ) sin( φ ( k )) A (2 k,
5) = v A ( k ) sin( φ ( k )) A (2 k,
6) = w A ( k ) sin( φ ( k )) A (2 k,
7) = − u A ( k ) sin( θ ( k )) cos( φ ( k )) A (2 k,
8) = − v A ( k ) sin( θ ( k )) cos( φ ( k )) A (2 k,
9) = − w A ( k ) sin( θ ( k )) cos( φ ( k )) A (2 k,
10) = 0 A (2 k,
11) = sin( φ ( k )) A (2 k,
12) = − sin( θ ( k )) cos( φ ( k )) The vector b is defined as follows: b (2 k −
1) = − cos( θ ( k )) cos( φ ( k )) z B ( k )+ sin( φ ( k )) x B ( k ) b (2 k ) = − sin( θ ( k )) cos( φ ( k )) z B ( k )+ sin( φ ( k )) y B ( kk