Cooperative Localisation of a GPS-Denied UAV using Direction of Arrival Measurements

1 Cooperati v e Localisation of a GPS-Denied U A V using Direction-of-Arri v al Measurements James S. Russell, Mengbin Y e, Brian D.O. Anderson, Hatem Hmam, Peter Sarunic Abstract —A GPS-denied U A V (Agent B) is localised through INS alignment with the aid of a nearby GPS-equipped U A V (Agent A), which broadcasts its position at se veral time instants. Agent B measures the signals’ direction of arrival with respect to Agent B’ s inertial na vigation frame. Semidefinite programming and the Orthogonal Procrustes algorithm are employed, and accuracy is impro ved through maximum likelihood estimation. The method is validated using flight data and simulations. A three-agent extension is explor ed. Index T erms —Localisation, INS alignment, Direction-of- Arrival Measurement, GPS-Denied, Semidefinite Programming I . I N T R O D U C T I O N Unmanned aerial vehicles (UA Vs) play a central role in many defence reconnaissance and surv eillance operations. For - mations of UA Vs can provide greater reliability and cov erage when compared to a single UA V . T o provide meaningful data in such operations, all U A Vs in a formation must have a common reference frame (typically the global frame). T ra- ditionally , U A Vs have access to the global frame via GPS. Howe ver , GPS signals may be lost in urban en vironments and enemy controlled airspace (jamming). Overcoming loss of GPS signal is a hot topic in research [3], and offers a range of dif ferent problems in literature [2, 18]. W ithout access to global coordinates, a U A V must rely on its inertial navigation system (INS). Stochastic error in on-board sensor measurements causes the INS frame to accumulate drift. At any given time, drift can be characterised by a rotation and translation with respect to the global frame, and is assumed to be independent between UA Vs in a formation. As a result, INS frame drift cannot be modelled deterministically . Information from both the global and INS frames must be collected and used to determine the drift between frames in order to achie ve INS frame alignment. W e describe this process as cooperativ e localisation when multiple v ehicles interact to achie ve the task. The work of Russell, Y e, and Anderson was supported by the Australian Research Council (ARC) Discovery Project DP-160104500, by 111-Project No. D17019, and by Data61-CSIRO. Y e was supported by an Australian Government Research Training Program (R TP) Scholarship. J.S. Russell, M. Y e and B.D.O. Anderson are with the Research School of Engineering, Australian National University , Canberra, Australia { u5542624, mengbin.ye, brian.anderson } @anu.edu.au . B.D.O. Anderson is also with Hangzhou Dianzi Univ ersity , Hangzhou, China, and with Data61-CSIR O (formerly NICT A Ltd.) in Canberra, Australia. H. Hmam and P . Sarunic are with Australian Defence Science and T echnology Group (DST Group), Edinburgh, Australia { hatem.hmam, peter.sarunic } @dst.defence.gov.au . V arious measurement types such as distance between agents and direction of arriv al of a signal (we henceforth call DOA 1 ) can be used for this process. In the conte xt of U A Vs, additional sensors add weight and consume power . As a result, one generally aims to minimise the number of measurement types required for localisation. This paper studies a cooperative approach to localisation using DO A measurements. When two or more GPS-enabled U A Vs can simultaneously measure directions with r espect to the global fr ame tow ards the GPS-denied U A V , the location of the GPS-denied U A V is gi ven by the point minimising distances to the half-line loci deriv ed from the directional measurements [6, 25, 26]. Operational requirements may limit the number of nearby GPS-enabled U A Vs to one single agent. W e therefore seek a solution which does not require simultaneous measurements to a single point. When the GPS-denied agent is able to simultaneously measure directions with r espect to its local INS frame to wards multiple landmarks (two in two-dimensional space, or three in three-dimensional space) with known global coordinates, triangulation-based measurements can be used to achieve lo- calisation. This problem is studied in three-dimensional space in [4], and in two-dimensional space in [8, 9]. If only one land- mark bearing can be measured at any giv en time, a bearing- only SLAM algorithm may be used to progressiv ely construct a map of the en vironment on the condition each landmark is seen at least twice. Alignment of a GPS-denied agent’ s INS frame could then be achiev ed by determining the rotation and translation between the map’ s coordinate frame and the global coordinate frame. In practice, landmark locations may be unknown, or there may be no guarantee they are stationary or permanent, and hence we require a localisation algorithm which is independent of landmarks in the environment. Iter- ativ e filtering methods such as the Extended Kalman Filter are often required when drift is significant between updates, howe ver in our problem context the drift is sufficiently slow to be modelled as stationary ov er short periods. Giv en that the U A Vs have limited computational capacity on board, we are motiv ated to formulate a localisation algorithm which does not in volve an iterativ e filtering technique. W ithout reliance on landmarks, the only directional mea- surements av ailable are between the GPS-denied and the GPS- enabled U A Vs. Gi ven their potentially large separation, these U A Vs are modelled as point agents, and therefore one single directional measurement is av ailable at any gi ven time. A 1 A bearing generally describes a scalar measurement between two points in a plane, whereas a direction-of-arrival is a vector measurement between two points in three-dimensional ambient space, which is the space that this paper considers. 2 stationary target is localised by an agent using bearing-only measurements 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 measurements receiv ed at a stationary receiver using an iterati ve filtering technique. In these works, either the entity being localised or a receiv er must be stationary , and the entity requiring localisation must broadcast a signal. For operational reasons, the agent requiring localisation may be unable to broadcast signals, or agents in volved may not be allowed to remain stationary . 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 directional measurements tow ards multiple stationary points or towards a stationary point from multiple kno wn positions. This is not possible in our problem context. The constraints flo wing from the measurement and motion requirements we are imposing therefore represent a significant technical challenge. One algorithm satisfying all the constraints above was proposed in [28], in which two agents perform sinusoidal motion in two-dimensional ambient space and directional measurements are used to obtain the distance between Agents A and B, but localisation of B in the global frame is not achiev ed. Motiv ated by interest from Australia’ s Defence Science and T echnology Group, this paper seeks to address the problem of localising a GPS-denied U A V , which we will call Agent B, with the assistance of a GPS-enabled U A V , which we will 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 frame at discrete instants in time. For each broadcast of Agent A, Agent B is able to take a DOA measurement tow ards Agent A. The problem setup and the solution we propose are both nov el. In particular , while the literature discussed abov e con- siders certain aspects from the following list, none consider all of the following aspects simultaneously: • The network consists of only two mobile agents (and is therefore different to the sensor network localisation problems in the literature). • There is no a priori knowledge or sensing of a stationary reference point in the global frame. • The UA Vs ex ecute unconstrained arbitrary motion in three-dimensional space. • Cooperation 2 occurs between a GPS-enabled and a GPS- denied U A V (transmission of signals is unidirectional; from Agent A to Agent B). It is the combination of all the above aspects which make the problem significantly more dif ficult and thus existing methods are unsuitable. In [30], this problem is studied in two- dimensional space using bearing measurements. One single piece of data is acquired at each time step. In this paper, each DO A measurement giv es two scalar quantities, and adding the 2 Agent A ’ s role in the cooperation is to broadcast its position ov er a series of time instants. third dimension significantly complicates the problem, thus requiring ne w techniques to be introduced. In our proposed solution, we localise Agent B by identifying the relationship between the global frame (navigated by Agent A) and the inertial navigation frame of Agent B. The rela- tionship is identified by solving a system of linear equations for a set of unknown variables. The nature of the problem means quadratic constraints exist on some of the variables. T o improv e robustness against noisy measurements, we exploit the quadratic constraints and use semidefinite programming (SDP) and the Orthogonal Procrustes algorithm to obtain an initial solution for maximum likelihood (ML) estimation. Our approach combining SDP , the Orthogonal Procrustes algorithm and ML estimation is a key novel contribution over existing works. W e ev aluate the performance of the algorithm by (i) using a real set of trajectories and (ii) using Monte Carlo simulations. Sets of unsuitable trajectories are identified, in which our proposed 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 follo ws. In Section II the problem is formalised. In Section III a localisation method using a linear equation formulation is proposed. Section IV extends this method to semidefinite programming to produce a more rob ust localisation algorithm. In Section V, a maximum likelihood estimation method is presented to refine results further . Section VI presents simulation results to ev aluate the performance of the combined localisation algorithm. Section VII extends the localisation algorithm to a three-agent net- work. The paper is concluded in Section VIII. 3 I I . P R O B L E M D E FI N I T I O N T wo agents, which we call Agent A and Agent B, travel along arbitrary trajectories in three-dimensional space. Agent A has GPS and therefore navigates with respect to the global frame. Because Agent B cannot access GPS, it has no ability to self-localise in the global frame, but can self-localise and navigate in a local inertial frame by integrating gyroscope and accelerometer measurements. This two-agent localisation problem inv olves 4 frames as in Figure 1. The importance of each frame, and its use in obtaining the localisation, will be made clear in the sequel. Frames are labelled as follows: • Let A 1 denote the global frame (available only to Agent A), • let B 2 denote the local INS frame of Agent B, • let B 3 denote the body-centred INS frame of Agent B (axes of frames B 2 and B 3 are parallel by definition), • let B 4 denote the body-fixed frame of Agent B. 3 Early sections in this paper (cov ering up to and including employment of Orthogonal Procrustes algorithm) appeared in less detail in the conference paper [21]. Additions have been made to these sections - the literature re view is now more extensive; the role of different coordinate frames is much more explicitly set out; the algorithm’ s performance is now validated on real flight trajectories, and the inclusion of apparently redundant quadratic constraints in the SDP problem formulation is justified further. Analysis of unsuitable trajectories, ML refinement and the three-agent extension are further extensions beyond [21]. 3 The expression of directional measurements with respect to the INS frame in vector form motiv ates the definition of the body-centred frame B 3 . Later , we find that differences in body fixed frame azimuth and elev ation measurement noise motiv ate the use of B 4 when discussing maximum likelihood estimation. Note that agents A and B are denoted by a single letter , whereas frames A 1 and B i for i = 2,3,4 are denoted by a letter-number pair . Positions of each agent in their respectiv e navigation frames ( A 1 and B 2 ) are obtained through a discrete- time measurement process. Let p I 0 J ( k ) denote the position of Agent J in coordinates of frame I 0 at the k th time instant. Let u J , v J , w J denote Agent J’ s coordinates in the global frame ( A 1 ), and x J , y J , z J denote Agent J’ s coordinates in Agent B’ s local INS frame ( B 2 ). It follows that: p A 1 A ( k ) = [ u A ( k ) , v A ( k ) , w A ( k )] > (1) p B 2 B ( k ) = [ x B ( k ) , y B ( k ) , z B ( k )] > (2) The rotation and translation of Agent B’ s local INS frame ( B 2 ) with respect to the global frame ( A 1 ) e volv es via drift. Although this drift is significant over long periods, frame B 2 can be modelled as stationary with respect to frame A 1 ov er short intervals 4 . During these short intervals, the following measurement process occurs multiple times. At each time instant k , the following four activities occur simultaneously 5 : • Agent B records its own position in the INS frame p B 2 B ( k ) . • Agent A records and broadcasts its position in the global frame p A 1 A ( k ) . • Agent B receiv es the broadcast of p A 1 A ( k ) from Agent A, and measures this signal’ s DOA using instruments fixed to the U A V’ s fuselage. This directional measurement is therefore naturally referenced to the body-fixed frame B 4 . • Agent B’ s attitude, i.e. orientation with respect to the INS frames B 2 and B 3 is known. An expression for the DO A 4 If loss of GPS is sustained for extensi ve periods we recommend using the algorithm in this paper as an initialisation for a recursive filtering algorithm. 5 The relative speed of the UA Vs with respect to the speed of light is negligible. Fig. 1. Illustration of coordinate frames in a two-dimensional space measurement referenced to the axes INS frames B 3 can therefore be easily calculated. A DOA measurement, referenced to a frame with axes denoted x, y , z , is expressed as follows: • Azimuth ( θ ): angle formed between the positiv e x axis and the projection of the free vector from Agent B tow ards Agent A onto the xy plane. • Ele v ation ( φ ): angle formed between the free vector from Agent B tow ards Agent A and xy plane. The angle is positiv e if the z component of the unit vector towards Agent A is positiv e. An illustration of azimuth and elev ation components of a DO A measurement is provided in Figure 2. The problem addressed in this paper , namely the localisation of Agent B, is achiev ed if we can determine the relationship between the global frame A 1 and the local INS frame B 2 . This information can be used to determine global coordinates of Agent B at each time instant k : p A 1 B ( k ) = [ u B ( k ) , v B ( k ) , w B ( k )] > (3) Passing between the global frame ( A 1 ) and the local INS frame of Agent B ( B 2 ) is achiev ed by a rotation of frame axes (defined by a rotation matrix, call it R B 2 A 1 ) and translation t B 2 A 1 of frame. For instance, the coordinate vector of the position of Agent A referenced to the INS frame of Agent B is: p B 2 A ( k ) = R B 2 A 1 p A 1 A ( k ) + t B 2 A 1 (4) W e therefore hav e p A 1 B ( k ) = R B 2 > A 1 ( p B 2 B ( k ) − t B 2 A 1 ) (5) where R B 2 > A 1 = R A 1 B 2 and − R B 2 > A 1 t B 2 A 1 = t A 2 B 2 . The locali- sation problem can be reduced to solving for R B 2 A 1 ∈ S O (3) with entries r ij and t B 2 A 1 ∈ R 3 with entries t i . The matrix R B 2 A 1 is a rotation matrix if and only if R B 2 A 1 R B 2 > A 1 = I 3 and det( R B 2 A 1 ) = 1 . As will be seen in the sequel, these constraints are equivalent to a set of quadratic constraints on the entries of R B 2 A 1 . In total there are 12 entries of R B 2 A 1 and t B 2 A 1 to be found as we work directly with r ij . Fig. 2. Illustration of azimuth and elevation components of a DOA measure- ment 4 I I I . L I N E A R S Y S T E M M E T H O D This section presents a linear system (LS) method to solving the localisation problem. Given enough measurements, the linear system approach can achieve exact localisation when using noiseless DO A measurements, so long as Agents A and B av oid a set of unsuitable trajectories (which are detailed in Section VI-C) in which rank-deficiency is encountered. Building on this, Section IV introduces non-linear constraints to the linear problem defined in this section to improv e accuracy in the presence of noise. A. F orming a system of linear equations The follo wing analysis holds for all k instants in time, hence we drop the argument k . The DOA measurement can be represented by a unit vector pointing from Agent B to Agent A. This vector is defined by azimuth and elev ation angles θ and φ referenced to the local INS frame B 2 , and its coordinates in the frame B 2 are gi ven by: ˆ q ( θ, φ ) = [ ˆ q 1 , ˆ q 2 , ˆ q 3 ] = [cos θ cos φ, sin θ cos φ, sin φ ] > (6) Define ¯ q . = k p B 2 A − p B 2 B k as the Euclidean distance between Agent A and Agent B (which is not available to either agent). Scaling to obtain the unit vector ˆ q gives ˆ q ( θ, φ ) = 1 ¯ q  x A − x B , y A − y B , z A − z B  > (7) Applying equation (4) yields:   ˆ q 1 ˆ q 2 ˆ q 3   = 1 ¯ q   r 11 u A + r 12 v A + r 13 w A + t 1 − x B r 21 u A + r 22 v A + r 23 w A + t 2 − y B r 31 u A + r 32 v A + r 33 w A + t 3 − z B   (8) The left hand vector is calculated directly from DO A mea- surements. Cross-multiplying entries 1 and 3 of both vectors eliminates the unknown ¯ q , and rearranging yields: ( u A ˆ q 3 ) r 11 + ( v A ˆ q 3 ) r 12 + ( w A ˆ q 3 ) r 13 − ( u A ˆ q 1 ) r 31 − ( v A ˆ q 1 ) r 32 − ( w A ˆ q 1 ) r 33 + ( ˆ q 3 ) t 1 − ( ˆ q 1 ) t 3 = ( ˆ q 3 ) x B − ( ˆ q 1 ) z B (9) Similarly , from the second and third entries in (8) ( u A ˆ q 3 ) r 21 + ( v A ˆ q 3 ) r 22 + ( w A ˆ q 3 ) r 23 − ( u A ˆ q 2 ) r 31 − ( v A ˆ q 2 ) r 32 − ( w A ˆ q 2 ) r 33 + ( ˆ q 3 ) t 2 − ( ˆ q 2 ) t 3 = ( ˆ q 3 ) y B − ( ˆ q 2 ) z B (10) Notice that both equations (9) and (10) are linear in the unknown r ij and t i terms. Given a series of K DO A mea- surements (each giving φ ( k ) , θ ( k ) ), (9) and (10) can then be used to construct the following system of linear equations: A Ψ = b , A ∈ R 2 K × 12 (11) where A , b are completely kno wn, containing θ ( k ) , φ ( k ) , p A 1 A and p B 2 B . The 12-vector of unknowns Ψ is defined as: Ψ = [ r 11 r 12 r 13 ... r 31 r 32 r 33 t 1 t 2 t 3 ] > (12) Entry-wise definitions of A and b are listed in Appendix A. These entries of Ψ can be used to reconstruct the trajectory of Agent B in the global frame using (5), and therefore solving (11) for Ψ constitutes as a solution to the localisation problem. If K ≥ 6 , the matrix A will be square or tall. In the noiseless case, if A is of full column rank, equation (11) will be solv able. B. Example of LS method in noiseless case using r eal flight trajectories W e demonstrate the linear system method using trajectories performed by aircraft operated by the Australian Defence Science and T echnology Group. Positions of both Agent A and B within the global frame and Agent B within the INS frame were measured by on-board instruments, whereas we generated a set of calculated DO A values using the above recorded real measurements. These trajectories are plotted in Figure 3. W e will make additional use of this trajectory pair in the noisy measurement case presented in Section IV, and in the maximum likelihood estimation refinement of the noisy case localisation result in Section V. Extensiv e Monte Carlo simulations demonstrating localisation for a large number of realistic 6 flight trajectories are left to the noisy measurement case. The rotation matrix and translation vector describing the relationship between the global frame and Agent B’ s inertial navigation frame are: R B 2 A 1 =   1 . 000 − 0 . 032 3 . 78 × 10 − 5 0 . 032 1 . 000 0 . 002 − 9 . 48 × 10 − 5 − 0 . 002 1 . 000   (13) T B 2 A 1 =  854 . 87 6 . 18 1 . 93  > (14) Azimuth and elev ation angle measurements are tabulated in T able I. Using (11), R B 2 A 1 and T B 2 A 1 were obtained exactly for the given flight trajectories; the solution is shown by the green line in Figure 3. I V . S E M I D E FI N I T E P RO G R A M M I N G M E T H O D This section presents a semidefinite programming (SDP) method for localisation, extending from the linear system (LS) approach presented in Section III. This method reduces the minimum required number of DOA measurements to obtain a unique solution, and is more robust than LS in terms of DOA measurement 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 of rotation matrices. The inclusion of rotation matrix constraints in SDP problems has been used previously to jointly estimate the attitude and spin-rate of a satellite [22], and in camera pose estimation using SFM techniques when directional mea- surements are made to multiple points simultaneously [1]. W e now apply this technique in a novel context to achieve INS alignment of Agent B, sufficient for its localisation. Finally , the Orthogonal Procrustes algorithm (O) is used to compensate for the rank relaxation of the SDP . 6 By realistic, we mean that the distance separation between successive measurements is consistent with UA V flight speeds and ensures the U A V does not exceed an upper bound on the turn/climb rate. Further detail is provided in Section VI-B. 5 T ABLE I P O SI T I O NS O F A G E N TS A A N D B , N O I SE L E S S D O A M E A SU R E M EN T S , A N D S D P+ O + M L S O L U TI O N F O R R E A L T R A J EC T O RY PA I R W I TH N OI S Y D OA M E AS U R E ME N T S k p A 1 A [m] p B 2 B [m] p A 1 B [m] DO A [ θ φ ] [rads] p A 1 B using SDP+O+ML 1 [349 . 1 − 924 . 1 374 . 4] > [1039 . 2 574 . 2 311 . 3] > [202 . 5 561 . 3 310 . 4] > [-1.4403 0.0447] [135 . 9 468 . 16 276 . 2] > 2 [781 . 0 − 870 . 3 372 . 5] > [1486 . 1 519 . 4 310 . 9] > [647 . 3 492 . 1 309 . 9] > [-1.4409 0.0474] [583 . 6 426 . 3 297 . 9] > 3 [1007 . 0 − 522 . 7 373 . 3] > [1946 . 2 458 . 2 310 . 2] > [1105 . 2 416 . 2 309 . 1] > [-1.6430 0.0697] [1044 . 8 378 . 6 319 . 9] > 4 [869 . 8 − 91 . 3 373 . 2] > [2140 . 4 746 . 9 309 . 8] > [1308 . 6 698 . 5 309 . 2] > [-2.0459 0.0723] [1230 . 3 672 . 8 330 . 6] > 5 [431 . 4 56 . 6 373 . 1] > [2201 . 6 1166 . 4 308 . 8] > [1383 . 2 1115 . 8 309 . 0] > [-2.2708 0.0464] [1279 . 0 1093 . 9 334 . 7] > 6 [33 . 9 − 262 . 2 373 . 6] > [2032 . 8 1477 . 7 310 . 2] > [1224 . 5 1432 . 5 310 . 9] > [-2.1512 0.0317] [1101 . 3 1400 . 2 329 . 2] > A. Quadratic constraints on entries of Ψ Rank-relaxed semidefinite programming (in the presence of inexact or noise contaminated data) benefits from the inclusion of quadratic constraint equations. W e now identify 21 quadratic and linearly independent constraint equations on entries of R B 2 A 1 , which all appear in Ψ in (12). Recall the orthogonality property of rotation matrices R B 2 A 1 R B 2 A 1 > = I 3 . By computing each entry of R B 2 A 1 R B 2 A 1 > , setting these equal to entries of I 3 , and referencing the i th entry of Ψ as ψ i , we define constraints: C 1 = ψ 2 1 + ψ 2 2 + ψ 2 3 − 1 = 0 (15a) C 2 = ψ 2 4 + ψ 2 5 + ψ 2 6 − 1 = 0 (15b) C 3 = ψ 2 7 + ψ 2 8 + ψ 2 9 − 1 = 0 (15c) C 4 = ψ 1 ψ 4 + ψ 2 ψ 5 + ψ 3 ψ 6 = 0 (15d) C 5 = ψ 1 ψ 7 + ψ 2 ψ 8 + ψ 3 ψ 9 = 0 (15e) C 6 = ψ 4 ψ 7 + ψ 5 ψ 8 + ψ 6 ψ 9 = 0 (15f) T o 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 2 A 1 > R B 2 A 1 and setting these equal to I 3 , we define constraints C 7:12 . W e omit presentation of C 7:12 due to space limitations and similarity with C 1:6 . The sets C 1:6 and C 7:12 are clearly equiv alent. Further constraints are required to ensure det( R B 2 A 1 ) = 1 . Cramer’ s formula states that R B 2 A 1 − 1 = adj ( R B 2 A 1 ) / det( R B 2 A 1 ) , where adj ( R B 2 A 1 ) denotes the adjugate matrix of R B 2 A 1 . Or- thogonality of R B 2 A 1 implies R B 2 A 1 > = adj ( R B 2 A 1 ) or that R B 2 A 1 = adj ( R B 2 A 1 ) > . By computing entries of the first column of Z = R B 2 A 1 − adj ( R B 2 A 1 ) > and setting these equal to 0 , we define constraints C 13:15 : C 13 = ψ 1 − ( ψ 5 ψ 9 − ψ 6 ψ 8 ) = 0 (16a) C 14 = ψ 4 − ( ψ 3 ψ 8 − ψ 2 ψ 9 ) = 0 (16b) C 15 = ψ 7 − ( ψ 2 ψ 6 − ψ 3 ψ 5 ) = 0 (16c) Similarly , by computing the entries of the second and third columns of Z and setting these equal to 0 , we define con- straints C 16:18 and C 19:21 respectiv ely . Due to space limita- tions, we omit presenting them. The complete set C 1:21 . = C Ψ constrains R B 2 A 1 to be a rotation matrix. The set of constraints is not an independent set, e.g. C 1:6 is equiv alent to C 7:12 . The benefits of the inclusion of dependent constraints is discussed further in Section IV -C. As we will show below , due to these additional relations, localisation requires azimuth and elev ation measurements at a minimum of 4 instants only ( K = 4 ), as opposed to 6 instants required in Section III. B. F ormulation of the Semidefinite Pr ogram The goal of the semidefinite program is to obtain: argmin Ψ || A Ψ − b || (17) subject to C Ψ . Equiv alently , we seek argmin Ψ || A Ψ − b || 2 subject to C Ψ . W e define the inner product of two matrices U and V as h U , V i = trace ( U V > ) . One obtains || A Ψ − b || 2 = h P , X i (18) where P =  A b  >  A b  and X = [ Ψ > − 1] > [ Ψ > − 1] and X is a rank 1 positive-semidefinite matrix 7 . The con- straints C Ψ can also be expressed in inner product form. For i = 1 , ..., 21 , C i = 0 is equiv alent to h Q i , X i = 0 for some easily determined Q i . Solving for Ψ in (17) is therefore equiv alent to solving for: argmin Ψ h P , X i (19) X ≥ 0 (20) rank ( X ) = 1 (21) X 13 , 13 = 1 (22) h Q i , X i = 0 i = 1 , ..., 21 (23) C. Rank Relaxation of Semidefinite Pr ogram This semidefinite program is a reformulation of a quadrati- cally constrained quadratic program (QCQP). Computationally speaking, QCQP problems are generally NP-hard. A close approximation 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, and discussion on its applicability can be found in [17]. This relaxation significantly increases the dimension of the SDP solver’ s co-domain. A notable consequence is that dependent constraints which are linearly independent over R within C Ψ , such as sets C 1:6 and C 7:12 , cease to be redundant when expressed in inner-product form and applied to entries of X . Hypothesis testing using extensi ve simulations confirmed 7 All matrices M which can be expressed in the form of M = v > v where v is a row vector are positi ve-semidefinite matrices. 6 with confidence above 95% that inclusion of quadratically dependent constraints causes an improvement in localisation accuracy . The solution to the relaxed semidefinite program X is typically close to being a rank 1 matrix 8 when DOA mea- surements are noisy . The closest rank 1 approximation to X , which we call ˆ X , is obtained by ev aluating the singular value decomposition of X , then setting all singular v alues except the largest equal to zero. From ˆ X , one can then use the definition of X to obtain the approximation of Ψ , which we will call ˆ Ψ . Entries ˆ ψ i for i = 10 , 11 , 12 can be used immediately to construct an estimate for t B 2 A 1 , which we will call t . Entries ˆ ψ i for i = 1 , ..., 9 will be used to construct an intermediate approximation of R B 2 A 1 , which we call b R , and which we will refine further . D. Orthogonal Pr ocrustes Algorithm Due to the relaxation of the rank constraint (23) on X , it is no longer guaranteed that entries of ˆ Ψ strictly satisfy the set of constraints C Ψ . Specifically , the matrix b R may not be a rotation matrix. The Orthogonal Procrustes algorithm is a commonly used tool to determine the closest orthogonal matrix (denoted R ) to a given matrix, b R . This is giv en by R = argmin Ω || Ω − b R || F , subject to ΩΩ > = I , where || . || F is the Frobenius norm. When noise is high, the abov e method occasionally returns R such that det( R ) = − 1 . In this case, we employ a special but standard case of the Orthogonal Procrustes algorithm [10] to ensure we obtain rotation matrices and avoid reflections by flipping the last column in one of the unitary matrix factors of the singular value decomposition. The matrix R and vector t are the final estimates of R B 2 A 1 and t B 2 A 1 using semidefinite programming and the Orthogonal Procrustes algorithm. The estimate of Agent B’ s position in the global frame is p A 1 B = R > ( p B 2 B − t ) . For con venience, we use SDP+O to refer to the process of solving a rank-relaxed semidefinite program, and then applying the Orthogonal Procrustes algorithm to the result. E. Example of SDP+O method with noisy DO A measur ements In this subsection, we apply the SDP+O method to perform localisation in a noisy DOA measurement case using the real trajectory example from Section III. A popular practice for performing DO A measurements from Agent B tow ards Agent A is to use fixed RF-antennas and/or optical sensors on board Agent B’ s airframe. The horizontal RF antenna typically has a larger aperture (generally around 4 times) than the vertical RF antenna. This is due to the typical ratio of a fixed-wing UA V’ s wingspan to its fuselage height. As a result, errors in azimuth and elevation measurements, referenced to the body-fixed frame B 4 , are modelled by independent zero- mean Gaussian distributed variables with different standard deviations, denoted σ Θ and σ Φ . 8 The measure used for closeness to rank 1 is the ratio of the two largest singular values in the singular value decomposition of X . 2000 1500 300 1000 400 z altitude [metres] x coordinate [metres] 1000 500 500 y coordinate [metres] 0 0 -500 -500 P A A (global coordinates of A) P B A (global coordinates of B) LS+O (noiseless) SDP+O ( =0.5 ° , =2 ° ) SDP+O+ML ( =0.5 ° , =2 ° ) Fig. 3. Recov ery of global coordinates of Agent B for recorded trajectories. Errors are σ θ = 0 . 5 ◦ and σ φ = 2 ◦ with respect to body fixed frame for the DO A measurements W e now add noise (for the purposes of this simulation example based on the real data) to the calculations of body- fixed frame azimuth and elev ation components of DOA. Strictly speaking, each noise is expected to follow a von Mises distribution, which generalises a Gaussian distribution to a circle [11]. For small noise, as often encountered, the von Mises distribution can be approximated by a Gaussian distri- bution. In this example we assume body-fixed frame azimuth and elev ation measurement errors hav e standard deviations of σ Θ = 0 . 5 ◦ and σ Φ = 2 ◦ . Samples of Gaussian error with these standard deviations were added to body-fixed frame ( B 4 ) ele vation and azimuth measurements calculated as described in Section III. These were con verted to DO A measurements referenced to the INS frame B 3 . The SDP+O algorithm was used to obtain R and t using the agents’ position coordinates in their respectiv e nav- igation frames and the noisy DOA values. The reconstructed trajectory p A 1 B is plotted in Figure 3 with the dotted black line. Position data of the reconstructed trajectory p A 1 B are tabulated in I. Remark 1. The accuracy of the SDP+O solution in the noiseless case was observed to deteriorate when the condition number of the true solution for X is high. This is due to a form of inherent re gularisation in the SDP solver Y almip [16]. When the appr oximate magnitude of the norm || t B 2 A 1 || is known, one appr oach is to straightforwar dly intr oduce a scaling coefficient before entries t i for i = 1 , 2 , 3 in equations (9) and (10) equal to the appr oximate norm of || t B 2 A 1 || . Furthermor e, if an appr oximation of t B 2 A 1 is known a priori as e t B 2 A 1 , the following controlled shifting algorithm may be applied to reduce the effect of re gularisation error . W e define shifted positions p s B 2 B ( k ) = p B 2 B ( k ) − e t B 2 A 1 . By substituting p s B 2 B for p B 2 B in the SDP, the vector t obtained thr ough SDP is an estimate of t B 2 A 1 − e t B 2 A 1 . V . M A X I M U M L I K E L I H O O D E S T I M A T I O N This section presents a maximum likelihood estimation (ML) method to refine estimates R and t obtained using the 7 SDP+O algorithm. The MLE refinement uses the series of DO A measurements expressed with respect to the body-fixed frame B 4 , and the known values for σ Θ and σ Φ describing the expected distribution of DO A errors. A non-linear log- likelihood function for DO A measurement error is deriv ed. The minimum of the log-likelihood function cannot be found analytically , so we employ an iterative gradient descent ap- proach instead. A. Likelihood Function Derivation In this section, DOA values are always expressed with respect to the body-fixed frame of Agent B ( B 4 ) to exploit the independence of azimuth and elev ation measurement errors. This is a change from Sections III and IV, in which DOA measurements were generally expressed with respect to the local INS frame B 2 . The transformation between coordinate frames B 2 and B 4 is kno wn to Agent B. Suppose body-fixed frame measurements of azimuth and elev ation Θ( k ) and Φ( k ) are contaminated by zero mean Gaussian noise as follows: • e Θ( k ) = Θ( k ) + ξ Θ , ξ Θ ∼ N (0 , σ Θ 2 ) • e Φ( k ) = Φ( k ) + ξ Φ , ξ Φ ∼ N (0 , σ Φ 2 ) T o calculate noiseless azimuth and elev ation measurements, an expression must be derived for the position of Agent A in Agent B’ s body-fixed frame B 4 . Observe that p B 4 A ( k ) = R B 4 B 2 ( k )( R B 2 A 1 p A 1 A ( k ) + t B 2 A 1 ) + t B 4 B 2 ( k ) (24) T o help distinguish coordinate reconstructions based on es- timates of R and t from true coordinates, reconstructed positions will be explicitly expressed as functions of R and t : p B 4 A ( k , R , t ) = R B 4 B 2 ( k )( Rp A 1 A ( k ) + t ) + t B 4 B 2 ( k ) (25) By definition of azimuth and elev ation in Section II: θ B 4 ( k , R , t ) = arcsin  p B 4 A ( k , R , t ) z || p B 4 A ( k , R , t ) ||  (26) φ B 4 ( k , R , t ) = atan2  p B 4 A ( k , R , t ) y , p B 4 A ( k , R , t ) x  (27) where p B 4 A = [ p B 4 A x , p B 4 A y , p B 4 A z ] > . The likelihood function for the set of DO A measurements is defined as follows: L ( p A 1 A , p B 2 B | R , t ) = 1 σ Θ √ 2 π K Y k =1 exp h − ( e θ B 4 ( k ) − θ B 4 ( k , R , t )) 2 2 σ 2 Θ i × 1 σ Φ √ 2 π K Y k =1 exp h − ( e φ B 4 ( k ) − φ B 4 ( k , R , t )) 2 2 σ 2 Φ i (28) It can be shown that maximising L ( p A 1 A , p B 2 B | R , t ) is equiv- alent to minimising K X k =1 h ( e θ B 4 ( k ) − θ B 4 ( k , R , t )) 2 2 σ 2 Θ + ( e φ B 4 ( k ) − φ B 4 ( k , R , t )) 2 2 σ 2 Φ i (29) B. Gradient descent and adaptive step size Possible parametrisations for the rotation matrix R include Euler angles, quaternion representation and Rodrigues rotation formula. In this paper we parametrise R by a 3-vector of Euler angles, and t is a 3-vector . This defines a mapping from R 6 → R , t , and the gradient of (29) can be expressed as a vector in R 6 . The log-likelihood function is non-linear with respect to this R 6 parametrisation of R and t . As a result, this function may be non-con vex, meaning the equation D log L = 0 may hav e multiple solutions, with only one of these being the global minimum. A gradient descent algorithm is therefore initialised using the result of the SDP+O method, and is used to conv erge tow ards a local minimum, which it is hoped will be the global minimum or close to it. Instead of selecting a constant step size, which may lead to ov ershooting local minima or excessiv e computation time, an adaptiv e step size approach is adopted. The backtracking line search algorithm discussed in [24] selects an optimally large step size satisfying a constraint placed on the av erage gradient ov er the step. C. Example of ML refinement of SDP+O solution In this subsection, we demonstrate the benefits of maximum likelihood estimation. ML was performed using the real flight trajectory data presented in Sections III and IV. The resulting reconstructed trajectory p B 2 A is presented in Figure 3 as the solid black line, and its coordinates are tabulated in T able I. Additionally , in this section we present the decrease and con vergence in the value of the negativ e log-likelihood func- tion (29), frame rotation error and reconstructed position error 9 ov er successive iterations of the gradient descent algorithm in Figures 4, 5 and 6. The error in INS frame rotation is reduced by over 60%, and the reconstructed position error of Agent B is reduced by over 70% by iterating the gradient descent algorithm. This represents a significant gain with respect to the SDP+O estimate, which served as the initialisation point of the gradient descent. Monte Carlo simulations cov ering a large set of trajectories are presented in Section VI. V I . S I M U L A T I O N R E S U L T S In this section, metrics are defined for performance ev alu- ation of the localisation algorithm. Monte Carlo simulations of realistic 10 flight trajectories are performed to ev aluate the effect of errors in body-fixed frame azimuth and elev ation measurements. W e also inv estigate the expected incremental improv ement in localisation accuracy as the number of DO A measurements K increases. Finally , trajectories of Agents A and B which are unsuitable for localisation of Agent B are discussed. In the preliminary conference paper [21], we compared the performance of the LS+O and SDP+O methods The LS+O method collapsed when small amounts of noise were introduced to DO A measurements, whereas rotation error 9 Metrics are defined in the sequel, see Section VI-A below . 10 These trajectories satisfy a set of assumptions detailed in Section VI-B. 8 0 2000 4000 6000 8000 10000 12000 Gradient Descent Iteration 10 -2 10 -1 10 0 10 1 10 2 Function value Fig. 4. Conver gence of negative log-likelihood function using ML for real trajectory pair 0 2000 4000 6000 8000 10000 12000 Gradient Descent Iteration 0 5 10 15 20 25 Rotation error (degrees) Fig. 5. Improvement in rotation error in degrees using ML for real trajectory pair increased linearly with respect to DOA measurement noise when using the SDP+O method. The SDP+O method is the superior 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]. All sequences of rotations in three dimensions can be expressed as one rotation about a single axis [19]. The geodesic metric on S O (3) defined by d ( R 1 , R 2 ) = arccos  tr ( R > 1 R 2 ) − 1 2  (30) is the magnitude of angle of rotation about this axis [27]. Where R B A is known, the error of rotation R is defined as d ( R , R B A ) . Position error is defined as the average Euclidian distance between true global coordinates of Agent B, and estimated global coordinates over the K measurements taken, divided (to secure normalisation) by the a verage distance between aircraft. p A 1 B ( k ) = R > ( p B 2 B − t ) (31) er ror ( p A 1 B ) = P k || p A 1 B ( k ) − p A 1 B ( k ) || K d (32) where d represents the av erage distance between aircraft. 0 2000 4000 6000 8000 10000 12000 Gradient Descent Iteration 0 0.1 0.2 0.3 0.4 Position error (normalised) Fig. 6. Improvement in reconstructed position error using ML for real trajectory pair 2 4 6 8 10 12 14 16 18 20 Number of DOA measurements (K) 0 0.2 0.4 0.6 0.8 1 1.2 Median position error σ θ = 0.1 ° , M σ θ = 0.1 ° , S σ θ = 1 ° , M σ θ = 1 ° , S σ θ = 2 ° , M σ θ = 2 ° , S Fig. 7. Median er ror ( p A 1 B ) vs. number of DOA measurements used to solve SDP+O (S) and SDP+O+ML (M) from K = 2 to K = 20 . B. Monte Carlo Simulations on Random T rajectories using SDP+O and ML In this subsection, we summarise the results of Monte Carlo simulations to ev aluate the expected performance of the SDP+O method and the SDP+O+ML method. Pairs of realistic trajectories for Agents A and B are generated in accordance with the following assumptions: • Initial horizontal separation of 800m • Initial vertical separation of 50m (initial altitudes of 300 and 350 metres) • A verage speed of 50 metres per second (97 knots) 2 4 6 8 10 12 14 16 18 20 Number of DOA measurements (K) 0 20 40 60 80 100 120 Median angle error in degrees σ θ = 0.1 ° , M σ θ = 0.1 ° , S σ θ = 1 ° , M σ θ = 1 ° , S σ θ = 2 ° , M σ θ = 2 ° , S Fig. 8. Median d ( R , R B A ) vs. number of DOA measurements used to solve SDP+O (S) and SDP+O+ML (M) from K = 2 to K = 20 9 • Measurements taken ev ery 5 seconds • Initial compass heading of each U A V in the xy plane follows a uniform distribution from 0 to 360 degrees. This is expressed as h ∼ U (0 , 360) degrees • Every 5 seconds, the recorded direction of each U A V in the xy plane changes by δ degrees where δ ∼ N ( c, 30 2 ) . The v alue c ∼ U ( − 40 , 40) is constant for a giv en trajectory and corresponds to an av erage curve for a trajectory • At each time measured, the rate of climb of each U A V in degrees is RoC ∼ N (0 , 5 2 ) T o represent the drift in the INS of Agent B, rotations R B 2 A 1 were generated by independently sampling three Euler angles α, β , γ where α, β , γ ∼ U ( − π , π ) , and translations t B 2 A 1 = [ t 1 , t 2 , t 3 ] > were generated by sampling entries t 1 , t 2 , t 3 ∼ U ( − 600 , 600) . As discussed in Section IV -E, we assume the standard deviation in elev ation measurement error in the body-fixed frame B 4 is four times the standard deviation in azimuth measurement error in B 4 , i.e. σ Φ = 4 σ Θ . W e vary the DO A error by σ Θ = [0 . 1 ◦ , 1 ◦ , 2 ◦ ] . Errors in the order of σ Θ = 0 . 1 ◦ are representativ e of an optical sensor, whereas the larger errors are representati ve of antenna-based (RF) measurements. For each value of σ Θ studied, and for each number of DOA measurements K from 2 to 20, we simulated 100 unique and realistic U A V trajectory pairs (Agent A and Agent B). For each trajectory pair , localisation was performed using the SDP+O method, and metrics d ( R , R B A ) and er r or ( P A 1 B ) , were calculated. The ML method was then used to enhance the result of the SDP+O method, and the error metrics were re- calculated. After all simulations were completed, the median 11 values of d ( R , R B A ) and er ror ( P A 1 B ) for both the SDP+O and SDP+O+ML methods were calculated across each set 100 simulations. The results of the Monte Carlo simulations are plotted in Figures 7 and 8. Median d ( R , R B A ) and er ror ( P A 1 B ) errors decrease signif- icantly when 4 or more DO A measurements ( K ) are used. Both metrics show an asymptotic limit to performance across all three noise le vels as the number of DO A measurements ( K ) increases. Median rotation error d ( R , R B A ) and er r or ( p A 1 B ) appear to exhibit similar asymptotic performance gain over the number of DOA measurements K up to 20. Remark 2. The maximal parametrisation for R , t consists of 12 entries, and the lar gest set of independent quadratic constraints consists of 6 r elations. P olynomial equation sets of n independent relations in n unknowns will generically have multiple solutions if some r elations ar e quadr atic. The addition of a scalar measur ement generically yields a unique solution. Ther efore 4 DO A measur ements are r equir ed to obtain the minimum of (12 − 6) + 1 = 7 scalar measur ements. C. Unsuitable trajectories for localisation In this subsection we are moti vated to identify trajectories of Agents A and B which may lead to multiple solutions for 11 For asymmetric distributions such as errors (which are nonnegativ e by definition and contain extreme outliers), the median is a superior measure of central tendency than the mean [7, 13]. 100 150 400 altitude [m] 300 200 y coordinate [m] 100 0 300 x coordinate [m] 250 200 150 -100 100 50 0 Fig. 9. Illustration of trajectory pairs leading to equal DO A measurements (disconnected blue lines) over K measurement instants. SDP+O+ML algo- rithm unable to discern distance from which Agent B (solid blue) observes Agent A (red). 0 50 100 500 altitude [m] x coordinate [m] -200 -100 0 y coordinate [m] 100 200 0 Fig. 10. Illustration of straight line motion of Agent A (red, trajectory gi ven by ( x ( t ) , y ( t ) , z ( t )) = (100 t, 0 , 0) ). Agent B (blue) observes Agent A through an unaligned INS frame. In this figure, the solid blue trajectory is the actual path of Agent B. Howe ver, each dotted blue line is also an admissible solution. R and t in the noiseless case, and consequently unreliable solutions in the noisy case. One can prove that if Agent A ’ s motion is restricted to a plane, a set of three columns of matrix A in Eqn. (11) become linearly dependent, and therefore a unique solution cannot be obtained using the LS method. When quadratic constraints are included in the SDP+O method, rank deficiency of A no longer automatically implies the existence of multiple solutions. For example, the SDP+O method obtains a unique solution if Agent A ’ s motion is planar and Agent B’ s motion is arbitrary . Multiple solutions never - theless can exist for certain non-generic unsuitable trajectories for the SDP+O method. W e begin by considering the case where DO A measure- ments expressed with respect to the Local INS frame B 2 are equal at each time instant. This is illustrated by an example in Figure 9. A similar problem is expected in the far field case, where the distance between Agents A and B is sufficiently large that DOA measurements become approximately equal despite each Agent’ s trajectory remaining arbitrary . In these cases, multiple solutions exist for t B 2 A 1 . Multiple solutions may also arise if Agent A ’ s trajectory appears similar from multiple perspectiv es. The localisation process may be incapable of determining the direction from which DO A measurements were taken with respect to the global frame. For example, if Agent A follows a straight line, a set of recorded DOA measurements may be achiev ed by 10 viewing Agent A from any direction in a circle perpendicular to Agent A ’ s motion and centred at Agent A ’ s trajectory . This is illustrated in Figure 10. V I I . T H R E E - A G EN T E X T E N S I O N A N D B E Y O N D This section explores in a preliminary way how the SDP+O+ML algorithm may be extended to localise two GPS- denied agents efficiently . T rivially , each GPS-denied aircraft could measure DOA of the GPS-equipped agent’ s broadcast of its position, and use the SDP+O+ML algorithm independently of each other to estimate drift in their local frames. W e are motiv ated to determine whether a trilateral 12 algorithm may be more resilient to DO A measurement error and/or unsuitable trajectories, and may perhaps require fewer DOA measurements from each aircraft than simply repeating the two-agent localisation algorithm with each GPS-denied agent. W e introduce a GPS-denied Agent C, whose local INS frame has rotation and translation parameters R C 2 A 1 and t C 2 A 1 with respect to the global frame. W e conclude this section by discussing the challenges inv olved in generalising our findings to arbitrary n -agent networks. A. Measur ement pr ocess in three-a gent network T o describe measurements within a network of more than two agents, one minor notation change is required: • DO A measurements made by Agent I tow ards Agent J will henceforth be expressed in the INS coordinate frame of Agent I as ( θ J I 2 , φ J I 2 ) At each time instant k in the discrete-time process: • Agents A and B interact as per the two-agent case. • Agent C receiv es the broadcast of Agent A ’ s global coordinates, and measures this signal’ s DOA with respect to frame C 2 , which we denote ( θ A C 2 , φ A C 2 ). • Agent C broadcasts its position with respect to its INS frame p C 2 C , as well as the measurement ( θ A C 2 , φ A C 2 ) to Agent B, who also takes a DOA measurement tow ards Agent C. This measurement is denoted ( θ C B 2 , φ C B 2 ). All DOA and position measurements are therefore relayed to Agent B, who performs the localisation algorithm presented in this section. B. F orming system of linear equations in thr ee-agent network In Section III, the linear system A Ψ = b was formed using relations stemming from the collinearity of the vector ( p B 2 A − p B 2 B ), and the vector in the direction of DO A measurement ( θ A B 2 , φ A B 2 ). W e refer to this system of equations as S AB , where the subscript references the agents in volv ed. A similar system S AC can be constructed independently using Agent C’ s DO A measurements towards Agent A and p C 2 C . In the three-agent network, Agent B also measures the DO A towards Agent C’ s broadcast, with respect to Agent B’ s local INS frame B 2 . T o exploit the collinearity of the vectorial representation of the DO A measurement ( θ C B 2 , φ C B 2 ) 12 In this section we relax the condition preventing GPS-denied agents from broadcasting signals and ( p B 2 C − p B 2 B ), an expression for the position coordinate vector p B 2 C is required. As achiev ed in equations (7) and (8) in Section III, this position may be expressed in terms of entries of R B 2 C 2 and t B 2 C 2 , and the linear system S B C may be defined similarly to S AB in Section III. Systems S AB , S AC and S B C can be assembled, forming a large system of linear equations S AB C with 36 scalar unknowns (9 rotation matrix entries and 3 translation vector entries for each distinct agent pair). At each time instant k for k = 1 , ..., K , two linear equations are obtained from each DO A measurement of ( θ A B 2 , φ A B 2 ), ( θ A C 2 , φ A C 2 ) and ( θ C 2 B 2 , φ C 2 B 2 ). As a result, 6 linear equations are obtained at each time instant. Performing the measurement process 6 times ( K = 6) produces 36 linear equations. Gener- ically , in the noiseless case, a unique solution therefore exists for K = 6 time instants. When using only the LS method, the three-agent localisation problem requires the same minimum number of time instants as solving two independent two-agent localisation problems concurrently , yet requires more DO A measurements than the sum of the number of measurements required in two separate two-agent localisation problems. Howe ver , quadratic relationships between R B 2 A 1 , t B 2 A 1 , R C 2 A 1 , t C 2 A 1 , R C 2 B 2 and t C 2 B 2 significantly reduce the required number of time instants ( K ) at which measurements occur . C. Quadratic constraints in three-a gent network and example In the two-agent case, 21 linearly independent quadratic constraints were identified in order to determine the rotation and translation between two frames. In the three-agent case, when the underlying undirected graph is a clique, we identify 21 × 3 = 63 linearly independent quadratic constraints using rotation matrix properties of R B 2 A 1 , R C 2 A 1 , and R C 2 B 2 . In the three-agent case, additional quadratic constraints can be identified due to relationships between rotated and translated reference frames. • Applying the rotation R C 2 A 1 is equiv alent to applying rotations R B 2 A 1 and R C 2 B 2 successiv ely . This relationship is expressed in equation (33). Setting the entries of the left hand side to zero yields 9 quadratic constraints. • The difference between the global frame representation of vectors t C 2 A 1 and t B 2 A 1 is equal to the global frame representation of t C 2 B 2 . This relationship is expressed in equation (34). Setting entries of the left hand side to zero yields 3 quadratic constraints. R C 2 A 1 − R C 2 B 2 R B 2 A 1 = 0 (33) ( R A 1 C 2 t C 2 A C − R A 1 B 2 t B 2 A 1 ) − R A 1 C 2 t C 2 B 2 = 0 (34) As mentioned in Section IV -C, dependent constraints which are not linearly dependent are included to improve the accu- racy of the SDP solver . The relationship between rotations described in equation (33) may be expressed in 3 distinct coordinate frames ( A 1 , B 2 and C 2 ), and hence 9 × 3 = 27 linearly independent quadratic constraints may be derived using the relationship in equation (33). Similarly , the rela- tionship between translations described in equation (34) may be expressed in 3 distinct coordinate frames ( A 1 , B 2 and C 2 ), 11 0 200 400 600 z altitude [metres] 800 500 y coordinate [metres] 500 0 x coordinate [metres] 0 -500 -500 -1000 P A A 1 P B A 1 P C A 1 Result P B A 1 Result P C A 1 Fig. 11. Illustration of example of successful localisation within three-agent network in noiseless case for K = 3 and hence 3 × 3 = 9 linearly independent quadratic constraints may be deri ved using the relationship in equation (34). In total, we hav e derived (21 × 3) + (9 × 3) + (3 × 3) = 99 linearly independent quadratic constraints for a system of 36 unknown variables. These constraints may be expressed in inner-product form as performed in Section IV. Rank-relaxed semidefinite programming can be used to obtain solutions for each INS frame’ s rotation and translation with respect to the global frame, and the Orthogonal Procrustes algorithm can be applied to each individual resulting rotation matrix. This defines the three-agent SDP+O method. T o illustrate successful localisation in the three-agent case, realistic trajectories were defined for Agents A, B and C for K = 3 time instants. These are presented in Figure 11. Only Agents B and C were assigned random INS frame rotations and translations as prescribed in Section VI, and the three-agent SDP+O method was used to obtain estimates of R B 2 A 1 , t B 2 A 1 , R C 2 A 1 and t C 2 A 1 . Each directional measurement consists of two scalar measurements, and hence a total of 3 × 2 × K = 18 scalar measurements were obtained. Locali- sation was successful, which demonstrates that only 3 time instants ( K = 3 ) are required for the three-agent SDP+O algorithm to obtain the exact solution in the noiseless case. Earlier , it was established that a minimum of 6 time instants were required to achiev e a unique solution in the three-agent case using LS+O, and a minimum of 4 time instants were required to achiev e a unique solution in the two-agent case using SDP+O. W e have therefore demonstrated that a trilateral algorithm can achie ve localisation of two GPS-denied agents in less measurement time instants than applying the bilateral algorithm twice independently . W e note that this extension to three-agents is not applicable if the measurement graph is a tree because measurements are required between each pair of agents 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 achie ved in fewer measurement time instants K when a three-agent extension is used, formal- ising an extension to arbitrary n -agent networks presents a significant theoretical challenge. There exist comprehensive works such as [31] and [29] on bearing rigidity of an arbitrary network in R 2 where all agents share the same reference frame, and some concepts are generalised to R n in [32]. Ho we ver , bearing rigidity theory for networks in R n when agents do not share a reference frame is comparativ ely underdev eloped when compared to distance- based rigidity . For example, a general theory does not exist for the minimum number of measurements required for rigidity . W e also note the risk of an e xplosion in computational complexity when extending our algorithm to n -agent netw orks. The relati ve pose of INS frames of any two agents linked by an edge in the underlying undirected measurement graph must be determined in order to use Eqn. (8). W e cannot substitute entries of Ψ relating to the relativ e pose of two INS frames (such as entries of R C 2 B 2 and t C 2 B 2 in the three-agent case) with associated quadratic expressions using relationships such as Eqn. (38) or Eqn. (39), or else the objecti ve in Eqn. (15) will cease to be quadratic and the SDP method will not be applicable. As a result, extending the algorithm to a large number of agents risks an exponential increase in the number of v ariables to be determined. It is not clear exactly how many measurements between the n agents would be required to obtain a result in the noiseless case. V I I I . C O N C L U S I O N This paper studied a cooperative localisation problem be- tween a GPS-denied and a GPS-enabled U A V . A localisa- tion algorithm was de veloped in two stages. W e showed that a linear system of equations built from six or more measurements yielded the localisation solution for generic trajectories. The second stage considered the inclusion of quadratic constraints due to rotation matrix constraints. Rank relaxed semidefinite programming was used, and the solution adjusted using the Orthogonal Procrustes algorithm. This gav e the algorithm greater resilience to noisy measurements and unsuitable trajectories. Maximum likelihood estimation was then used to improve the algorithm’ s results. Simulations were presented to illustrate the algorithm’ s performance. Finally , an approach was outlined to extend the two-agent solution to a three agent network in which only one agent has global localisation capacity . Future w ork may include implementation on aircraft to perform localisation in real time and validate our Monte Carlo analysis on measurement noise. W e also hope to extend our trilateral algorithm to larger networks by establishing further theory on bearing rigidity when agents do not share a common reference frame. R E F E R E N C E S [1] M. Arie-Nachimson, S. Z. Ko valsky , I. Kemelmacher- Shlizerman, A. Singer , and R. Basri. Global motion estimation from point matches. In Pr oceedings of the 2012 Second International Conference on 3D Imaging, Modeling, Pr ocessing, V isualization & T ransmission . [2] H. Bai and R. W . Beard. Relativ e heading estimation for target handoff in gps-denied environments. In American Contr ol Confer ence (ACC), 2016 , pages 336–341. IEEE, 2016. 12 [3] G. Balamurugan, J. V alarmathi, and V . Naidu. Surve y on uav navig ation in gps denied en vironments. In Signal Pr ocessing, Communication, P ower and Embedded Sys- tem (SCOPES), 2016 International Confer ence on , pages 198–204. IEEE, 2016. [4] P . Batista, C. Silvestre, and P . Oliv eira. Navigation systems based on multiple bearing measurements. IEEE T ransactions on Aer ospace and Electr onic Systems , 51 (4):2887–2899, Oct 2015. [5] H. Bayram, J. V . Hook, and V . Isler . Gathering bearing data 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-tar get geometries in passi ve localization: Part 1 - bearing-only localization. In Intelligent Sensors, Sensor Networks and Information, 2007. ISSNIP 2007. 3rd International Confer ence on . [7] S. Boslaugh. Statistics in a nutshell: A desktop quick r eference . O’Reilly Media, Inc., 2012. [8] K. Dogancay . Self-localization from landmark bearings using pseudolinear estimation techniques. IEEE T ransac- tions on Aerospace and Electronic Systems , 50(3):2361– 2368, July 2014. [9] Y . Duan, R. Ding, and H. Liu. A probabilistic method of bearing-only localization by using omnidirectional vision signal processing. In Intelligent Information Hiding and Multimedia Signal Pr ocessing (IIH-MSP), 2012 Eighth International Confer ence on , pages 285–288, July 2012. [10] D. Eggert, A. Lorusso, and R. Fisher . Estimating 3-d rigid body transformations: a comparison of four major algorithms. Machine V ision and Applications , 9(5):272– 290, Mar 1997. [11] C. Forbes, M. Evans, N. Hastings, and B. Peacock. Statistical Distrib utions . John Wile y & Sons, 2011. [12] M. Gavish and A. J. W eiss. Performance analysis of bearing-only target location algorithms. IEEE T ransac- tions on Aer ospace and Electr onic Systems , 28(3):817– 828, Jul 1992. [13] H. L. Harter . The method of least squares and some alternativ es: Part ii. International Statistical Re view / Revue Internationale de Statistique , 42(3):235–282, 1974. [14] D. Q. Huynh. Metrics for 3d rotations: Comparison and analysis. Journal of Mathematical Imaging and V ision , 35(2):155–164, 2009. [15] J. J. Koenderink and A. J. v an Doorn. Affine structure from motion. J . Opt. Soc. Am. A , 8(2):377–385, Feb 1991. [16] J. L ¨ ofberg. Y almip : A toolbox for modeling and optimization in matlab . In In Pr oceedings of the CACSD Confer ence , T aipei, T aiwan, 2004. [17] Z. Luo, W . Ma, A. M. So, Y . Y e, and S. Zhang. Semidefi- nite relaxation of quadratic optimization problems. IEEE Signal Pr ocessing Magazine , 27(3):20–34, May 2010. [18] J. Morales, P . Roysdon, and Z. Kassas. Signals of opportunity aided inertial navigation. In Pr oceedings of ION GNSS Conference , pages 1492–1501, 2016. [19] B. Palais, R. Palais, and S. Rodi. A disorienting look at euler’ s theorem on the axis of a rotation. American Mathematical Monthly , 116(10):892–909, 2009. [20] J. Reis, P . Batista, P . Oliv eira, and C. Silvestre. Source localization based on acoustic single direction measure- ments. IEEE T ransactions on Aer ospace and Electr onic Systems , pages 1–1, 2018. [21] J. S. Russell, M. Y e, B. D. Anderson, H. Hmam, and P . Sarunic. Cooperativ e localisation of a gps-denied uav in 3-dimensional space using direction of arriv al measurements. IF AC-P apersOnLine , 50(1):8019 – 8024, 2017. 20th IF A C W orld Congress. [22] J. Saunderson, P . A. Parrilo, and A. S. W illsky . Semidef- inite relaxations for optimization problems ov er rotation matrices. In 53r d IEEE Conference on Decision and Contr ol , pages 160–166, Dec 2014. [23] S. Sohn, B. Lee, J. Kim, and C. Kee. V ision-based real- time target localization for single-antenna gps-guided uav . IEEE T ransactions on Aer ospace and Electr onic Systems , 44(4):1391–1401, Oct 2008. [24] P . S. Stanimirovi ´ c and M. B. Miladinovi ´ c. Accelerated gradient descent methods with line search. Numerical Algorithms , 54(4):503–520, 2010. [25] L. G. T af f. T ar get localization from bearings-only obser- vations. IEEE T ransactions on Aer ospace and Electr onic Systems , 33(1):2–10, Jan 1997. [26] O. T ekdas and V . Isler . Sensor placement for triangulation-based localization. IEEE T ransactions on Automation Science and Engineering , 7(3):681–685, July 2010. [27] R. Tron, J. Thomas, G. Loianno, K. Daniilidis, and V . Kumar . A distributed optimization framew ork for lo- calization and formation control: Applications to vision- based measurements. IEEE Control Systems , 36(4):22– 44, Aug 2016. [28] M. Y e, B. D. O. Anderson, and C. Y u. Bearing-only measurement self-localization, velocity consensus and formation control. IEEE T ransactions on Aerospace and Electr onic Systems , 53(2):575–586, April 2017. [29] D. Zelazo, A. Franchi, and P . R. Giordano. Rigidity theory in se(2) for unscaled relative position estimation using only bearing measurements. 2014 Eur opean Con- tr ol Confer ence (ECC) , pages 2703–2708, 2014. [30] L. Zhang, M. Y e, B. D. O. Anderson, P . Sarunic, and H. Hmam. Cooperative localisation of uavs in a gps- denied en vironment using bearing measurements. In 2016 IEEE 55th Confer ence on Decision and Contr ol (CDC) , pages 4320–4326, Dec 2016. [31] S. Zhao and D. Zelazo. Bearing rigidity and almost global bearing-only formation stabilization. IEEE T rans- actions on Automatic Contr ol , 61(5):1255–1268, May 2016. [32] S. Zhao and D. Zelazo. Localizability and distributed protocols for bearing-based network localization in arbi- trary dimensions. Automatica , 69:334 – 341, 2016. 13 A P P E N D I X A. F orms of A and b The matrix A is defined in Section III-A as follows: A (2 k − 1 , 1) = u A ( k ) sin( φ ( k )) A (2 k − 1 , 2) = v A ( k ) sin( φ ( k )) A (2 k − 1 , 3) = w A ( k ) sin( φ ( k )) A (2 k − 1 , 4) = 0 A (2 k − 1 , 5) = 0 A (2 k − 1 , 6) = 0 A (2 k − 1 , 7) = − u A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − 1 , 8) = − v A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − 1 , 9) = − w A ( k ) cos( θ ( k )) cos( φ ( k )) A (2 k − 1 , 10) = sin( φ ( k )) A (2 k − 1 , 11) = 0 A (2 k − 1 , 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 ( k )