Aiding Navigation for Groups of Aircraft with Bearing and Distance Measurements

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Aiding navigation for groups of aircraft with bearing

and distance measurements

Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet

av Mattias Olsson LiTH-ISY-EX--18/5174--SE

Linköping 2018

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Aiding navigation for groups of aircraft with bearing

and distance measurements

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan vid Linköpings universitet

av

Mattias Olsson LiTH-ISY-EX--18/5174--SE

Handledare: Martin Skoglund

ISY, Linköpings universitet

Zoran Sjanic

SAAB Aeronautics

Daniel Magnusson

SAAB Aeronautics

Examinator: Fredrik Gustafsson

isy, Linköpings universitet

(4)

(5)

Avdelning, Institution Division, Department

Automatic Control

Department of Electrical Engineering SE-581 83 Linköping Datum Date 2018-09-30 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-151887

ISBN — ISRN

LiTH-ISY-EX--18/5174--SE

Serietitel och serienummer Title of series, numbering

ISSN —

Titel Title

Navigeringsstöttning för grupper av flygplan med bäring- och distansmätningar Aiding navigation for groups of aircraft with bearing and distance measurements

Författare Author

Mattias Olsson

Sammanfattning Abstract

Det här examensarbetet vidareutvecklar en befintlig algorithm för navigeringsstöttning av grupper av flygplan, främst inriktat på SAAB JAS 39 Gripen. Genom att kombinera mätdata från olika flygplan kommer vi gå igenom hur man kan förbättra prestanda genom applicering av consensusfilter. Så länge ett plan har GPS-signal är positionering enkelt. Dock är den relativt lätt att störa ut, vilket gör alternativa lösningar för positionering nödvändiga. Att använda interna sensorer som mäter accelerationer och vinkelhastigheter fungerar utmärkt på kort sikt, men ger en drift över en längre tidsperiod.

För att lösa de här problemen utvärderar vi olika möjligheter att förbättra navigationspre-standan genom att kombinera mätdata från olika flygplan med hjälp av ett consensusfilter. Vi visar att prestandan kan förbättras genom att använda distans- och vinkelmätningar inom gruppen med distribuerade filter. Filtret är implementerat i Matlab med olika scenarier och jämförExtended Kalman-Consensus Filter (EKCF) med den föregående lösningen med ett Extended Kalman Filter (EKF).

Nyckelord

(6)

(7)

Sammanfattning

Det här examensarbetet vidareutvecklar en befintlig algorithm för navigerings-stöttning av grupper av flygplan, främst inriktat på SAAB JAS 39 Gripen. Genom att kombinera mätdata från olika flygplan kommer vi gå igenom hur man kan förbättra prestanda genom applicering av consensusfilter. Så länge ett plan har GPS-signal är positionering enkelt. Dock är den relativt lätt att störa ut, vilket gör alternativa lösningar för positionering nödvändiga. Att använda interna sen-sorer som mäter accelerationer och vinkelhastigheter fungerar utmärkt på kort sikt, men ger en drift över en längre tidsperiod.

För att lösa de här problemen utvärderar vi olika möjligheter att förbättra na-vigationsprestandan genom att kombinera mätdata från olika flygplan med hjälp av ett consensusfilter. Vi visar att prestandan kan förbättras genom att använda distans- och vinkelmätningar inom gruppen med distribuerade filter. Filtret är implementerat i Matlab med olika scenarier och jämförExtended Kalman-Consensus Filter (EKCF) med den föregående lösningen med ett Extended Kalman Filter (EKF).

(8)

(9)

Abstract

This thesis extends previous work on navigational aiding of groups of aircraft, primarily intended for the fighter SAAB JAS 39 Gripen, as long as an aircraft gets GPS signals, it is easy to estimate position, but the GPS is relatively easy to jam, rendering alternative methods of positioning necessary. To use internal sensors measuring accelerations and angular velocities is a good replacement on short terms, but gives a drift in positioning over longer time periods.

To resolve these issues, we review different possibilities to improve navigation performance by combining measurement data from different aircraft using a con-sensus filter. We show that the performance can be improved by using measure-ments of distance and angles to other aircraft within the group in a distributed filter. The filter is implemented in Matlab and evaluated in different scenarios, and this Extended Kalman-Consensus Filter (EKCF) is compared to a previously proposed solution using an Extended Kalman Filter (EKF).

(10)

(11)

Acknowledgements

I would like to thank my examiner Fredrik Gustafsson and Saab for providing me with the opportunity to perform this thesis.

Many thanks to my supervisors Martin Skoglund and Zoran Sjanic, for all your support and patience during this thesis work.

(12)

2.5.1 Filtering . . . 11 3 Modeling 13 3.1 Simulation model . . . 13 3.1.1 Coordinate system . . . 14 3.1.2 Aircraft kinematics . . . 15 3.2 Sensor modeling . . . 16 3.2.1 INS measurements . . . 17 3.2.2 GPS measurements . . . 18 3.2.3 TOA measurements . . . 18 3.2.4 DOA measurements . . . 19 3.3 Estimation . . . 21 3.3.1 Filter states . . . 21 3.3.2 Kinematic model . . . 22 viii

(13)

Contents ix 3.3.3 Observation matrices . . . 24 4 Results 29 4.1 Simulation environment . . . 29 4.1.1 Scenario 1 . . . 29 4.1.2 Scenario 2 . . . 29 4.1.3 Scenario 3 . . . 30 4.1.4 Scenario 4 . . . 31 4.2 Evaluation . . . 32 4.3 Results . . . 32 4.3.1 Scenario 1 . . . 32 4.3.2 Scenario 2 . . . 34 4.3.3 Scenario 3 . . . 40 4.3.4 Scenario 4 . . . 44 5 Conclusions 53 5.1 Discussion . . . 53 5.2 Future work . . . 54 Bibliography 57

(14)

(15)

Notation

Abbreviations

Abbreviation Meaning

INS Inertial Navigation System GPS Global Positioning System EKF Extended Kalman Filter

EKCF Extended Kalman-Consensus Filter TOA Time Of Arrival

DOA Direction of arrival

IR Infrared

RMSE Root Mean Squared Error NAV Navigation reference frame BODY Body reference frame

(16)

(17)

1

Introduction

When a group of fighter aircraft flies in hostile territory and the Global Position-ing System (GPS) is jammed, it puts a lot of demands on the navigation system. Dead reckoning using only an Inertial Navigation System (INS) is viable only for short periods of time due to drift in the sensors.

The goal of thesis seeks to improve performance under above said conditions. Even if the estimate of absolute position is wrong it might still be valuable to know the relative position of other aircraft within the group. The following chap-ter shows an overview of what is done in this thesis, and discusses the problem formulation, goals and related work.

1.1 Background

A navigation solution is a necessity in aircraft, both in terms of positioning and knowing where you are heading. Today a solution using the INS aided by the GPS is widely used by both civilian and military aircraft. When data from the GPS is unavailable, other solutions of positioning is required when such situations occur, as positioning performance using only INS tends to decrease over time due to in-tegration of noisy measurements. Using local filters has been shown to improve positioning both in absolute and relative terms when the GPS is jammed, using each airplane in a group as nodes in a sensor network. Especially for fighter aircraft, robustness within the navigation system is also desired besides perfor-mance.

(18)

2 1 Introduction

1.2 Motivation

When it comes to navigation in fighter aircraft, no system can be too robust or perform too well. The GPS is quite easy to jam, and when it is jammed, a backup solution is required. Distributed filtering algorithms have recently become pop-ular because of their scalability to large networks and robustness compared to centralized solutions.

1.3 Aim and Goal

The goal of this thesis is to further develop and evaluate an existing navigational algorithm used for positioning when the GPS is jammed. This thesis aims specifi-cally for the aircraft fighter JAS 39 Gripen, it uses an existing implementation of a simulation environment with existing models, communication links, TOA esti-mations and measurement updates. By extending the current environment with new filters, modified models and new states using an IR-camera, it is hoped to achieve improved robustness of positioning when jamming of the GPS is occur-ring. To do this, we introduce a few variants of the consensus filter adapted to the current models. By using measurements and estimates from all aircraft in a distributed filter, a better result is hoped to be achieved.

The results will ultimately be evaluated and compared to previous solutions, the aim is to achieve better results in form of better positioning. In the end the re-sults may be used as decision material whether further work will be put into this or not.

1.4 Problem formulation

The goal of this thesis is to improve the positioning performance during flight for a specific airplane and its relative distances to other airplanes. The study can be broken down into the following question formulations:

• Can the Kalman-Consensus filter be used to achieve a better estimation of an airplanes position compared to the EKF-filter?

• Can the Kalman-Consensus filter be used to achieve a better estimation of an airplanes relative distances to other airplanes?

• How much can the positioning and distance estimation be improved if bear-ing angles from an IR-camera is added to the measurements?

The inertial navigation system is subject to drift, where a small bias error over time is integrated into a bigger error and thus gives an unreliable position esti-mate after some time. To achieve better result, a group of aircraft can estiesti-mate relative distances between each other. By communicating such distances and ap-plying a distributed filter solution we hope to achieve a good position estimate for

(19)

1.5 Related work 3

all aircraft. In some scenarios all aircraft has lost their GPS signal, in others one or a few aircraft or beacons still has the GPS available. Given that one or more aircraft or beacons still have GPS, they may be used as “anchors” to improve posi-tioning of all aircraft, using filter solutions that take advantage of measurements and estimations from multiple aircraft.

This thesis work is based on a previous thesis work. With the extensions of an IR-camera and a new filter, where the IR-camera measures the bearing angles of planes in front of it and the current (EKF) filter is replaced with a Kalman-consensus filter.

1.5 Related work

The problem of aided navigation for a network of airplanes has already been solved using a set of local EKF filters [6], and this thesis seeks to extend it by us-ing filters in a more distributed way. Usus-ing a centralized Kalman filter is one way of dealing with the same problem, but might put unreasonably high demands on the communication bandwidth and is susceptible to node failure. De-centralized Kalman filters is a different approach using local Kalman filters, though require all-to-all communication links, which still put high requirements on communica-tion bandwidth for large networks.

Consensus filtering for sensor networks has recently been a hot topic for a num-ber of researchers, as they have a broad range of applications in sensor networks. Instead of using a centralized scheme, a separate filter is run by each node while communicating their states to other nodes and adjusts its own state estimate itera-tively. The benefits compared to centralized- and de-centralized schemes are that consensus algorithms only requires each sensor node to communicate with its di-rect neighbours. The concept of consensus filters has been widened into Kalman-Consensus algorithms, which has been thoroughly investigated by a number of authors, with the idea to apply local micro-Kalman filters at each node. While the KCF is mainly adapted to linear systems, it was generalized in [11] to the non-linear case by non-linearizing the models used in a similar way as the EKF.

Consensus filters generally requires the tracked states to be observable by each node in the near neighbourhood, otherwise the time required to reach consensus among the nodes may increase significantly. A proposal to solve this problem is the Kalman Consensus Filter [10], in which the neighbours prior state estimate is weighted by the covariance matrices of their state estimates.

1.6 Limitation

This thesis main purpose is to evaluate solutions for aided navigation problems. The focus lies on sensor fusion and filtering, therefore some dynamics of the

(20)

4 1 Introduction

airplanes and its environment is not modeled and some characteristics are sim-plified. The measurements used are assumed to be pre-filtered (e.g. the INS-unit gives angle velocity- and accelerations as output instead of modified pulse lengths) to be used directly as input to the filtering solutions. While there might be many different solutions to solve this problem, this thesis focuses on only a few of them using the same models.

Characteristics and dynamics that are not modeled in this thesis include the cur-vature of the earth, which in practice needs to be modeled for correct navigation. The airplane modeling does not include air resistance, side drifting (the velocity vector is always assumed to be aligned with the body of the aircraft) and roll angle characteristics, also the communication links are simplified, the effects of sending data over a bus is neglected and the message is sent directly and unen-crypted via the modeled link. These characteristics are deemed as unnecessary for testing aided navigation solutions.

1.7 Approaching Method

The pre-study can essentially be broken down into two parts, literature and exist-ing solution. As a base-frame of reference the literature described in related work will be used and studied. While the previous thesis report and implementation code is quite substantial and needs to be examined and investigated if extensions and modifications are to be applied.

1.7.1 Methodology

The methodology of this thesis work can roughly be divided into two parts. The first part concerns the implementation of the extended Kalman-Consensus fil-ter while the other concerns extending the existing work by including modeling and simulating measurements of an IR-camera. The following steps describes an overview of the methodology:

• Implementation, develop and implement a filter algorithm suited to the existing models.

• Simulate and validate the accuracy and performance of the various filters, including tuning of parameters.

• Study relevant literature concerning IR-cameras and its characteristics. • Develop a model of an IR-camera suited for the current implementation

and extend the current environment with it.

• Simulate the extended implementation of the simulation environment. • Analyze the performance and correctness of the results.

(21)

1.7 Approaching Method 5

Implementation

The main strategy concerning the implementation is to use existing code, written in an object-oriented manner in Matlab, as this thesis seeks to extend the previous thesis with new filters, measurements and measurement models. Each filter will be implemented and simulated separately for each scenario. The object-oriented way of writing code will be continued, allowing for easy extensions in possible future works.

Evaluation

The main goal of this thesis is to improve navigation performance for each air-craft both in absolute and relative terms. In absolute positioning the interesting result is how well the position of an aircraft can be pinpointed on a map, while in relative positioning the estimated distance to other planes within the group are evaluated.

As a quantitative measurement of the performance, the Root Mean Square Error (RMSE) will be used to determine the standard deviation. RMSE will be calcu-lated separately for each aircraft in terms of its absolute position and its relative estimate of each other aircraft within the group. The result will then be presented using plots of the estimation error over time.

(22)

(23)

2

Estimation Theory

2.1 Background

From an estimation perspective in this thesis, we have a set of aircraft and station-ary targets with known locations (referred to as landmarks) represented as nodes in a sensor network. Each node has their own set of sensors and means of com-munication. In this section we will discuss some filtering methods of estimating the states of each node.

2.2 Kalman filter

The Kalman filter [5] is a common way to estimate states in a system from noisy measurements, it is the optimal way of filtering in the sense of least mean square error, given that the system is linear and the noise is independent and Gaussian. An advantage of the Kalman filter is that it is easy and practical to implement, since it uses the discrete state space model [3] defined as:

xk+1= Axk+ Buk+ vk (2.1a)

yk = Cxk+ Duk+ ek (2.1b)

where x are the internal states, u is the input vector, v is the process noise,

e is measurement noise with Gaussian distribution, A, B, C and D are

time-independent matrices which describe the system. With this model the Kalman filter can be implemented straightforwardly. Some disadvantages would be that the noise needs to be uncorrelated to the signal and the system has to be linear.

(24)

8 2 Estimation Theory

2.3 The extended Kalman filter

In real applications, the system to be filtered is often non-linear, in which case the Kalman filter is not directly applicable. One way of dealing with such systems is to linearize the observation- and covariance matrices around an working point using Taylor expansions, and then applying the Kalman filter. The result is then called the Extended Kalman Filter (EKF) [7]. The EKF works well as long as the system is not highly non-linear, or the signal to noise ratio is high. In contrast to the Kalman filter, the EKF cannot be shown to be the optimal solution in minimiz-ing the estimated variance [4]. The used state space model has time-dependent system matrices and non-linear dynamics and measurement equation, which can be expressed as:

xk+1 = f (xk, uk, vk) (2.2a)

yk = h (xk, uk, ek) (2.2b)

2.4 Kalman-Consensus filtering

One intuitive way of handling and applying a filter to a sensor network is to gather measurements from all nodes to a main node and then applying a Kalman filter. The solution is then usually referred to as the Centralized Kalman Filter (CKF) [5]. However, this approach is susceptible to node failure and requires a master node, which may not be desired for a group of fighter aircraft.

The consensus filter is a distributed filter where each sensor node estimates their own state and the state of neighboring nodes. It is in many ways beneficial to per-form filtering in a distributed way, compared to having a centralized filter. Such advantages include the requirements of less communication bandwidth for each node (each node only needs to communicate with their neighbors), and more ro-bustness to node failure. The added roro-bustness comes from having each state estimated by multiple nodes simultaneously, therefore adding some redundancy to help countering node failure [13].

A Kalman-Consensus filter merges the Kalman filter with the idea of a consen-sus filter. The result is a decentralized filter for systems with multiple nodes. An area which has been explored by a number of authors [2] and has gained popular-ity for sensor networks over the past decennia.

2.4.1 Filtering

The main function of the filter is to create a distributed way of Kalman filtering while all node estimates converges to a common state estimate [8] . Each node communicate their own state vector, measurement covariance and covariance ma-trix to neighboring nodes. After communication, each node compute state esti-mates and predict for the next time update [10]. The Kalman-Consensus filter

(25)

2.4 Kalman-Consensus filtering 9

uses the common state-space models for discrete time [4]:

xk+1= Axk+ Bwk (2.3)

with the sensing model:

yk = Hkxk+ vk (2.4)

Where A and B is the system model matrices (which will be described in Chapter 3), where v and w are Gaussian distributed noise wk ∼N (0, Q) , v ∼ N (0, R) and

H is the sensor model for the used sensors. The KCF requires each node i to be able to communicate its covariance weighted measurement vector ui, inverse

covariance matrix Uiand its estimated state ˆxi. ui and Ui are defined as:

uk = HkTR −₁ yk (2.5a) Uk = HkTR −1_H k (2.5b)

These are sent as a message to each neighbouring node, since the principle of a consensus filter is to communicate states instead of direct measurements to avoid minimizing the nodes own covariance with the same measurements multi-ple times. Note that the inverse of the covariance is used, also called information form. One benefit of the information form is that it is additive, making the av-eraging later on simple. The set of neighbours is defined as Ji = NiS{i}, where

Ni is the set of all objects [n1, n2, . . . , ni] within the scenario. For this algorithm,

all-to-all communication is not needed [9]. The following message is sent to all neighbouring nodes:

msgi = (ui, Ui, ˆxi) (2.6)

where ˆxi is the filter estimation of the nodes own state vector. All messages are

then summed together, forming a weighted measurement vector yi and

covari-ance matrix Si: zi = X j∈Ji uj (2.7a) Si = X j∈Ji Uj (2.7b)

where the own node is denoted i. Once the measurement covariance and covari-ance matrices has been computed the measurement update is defined as:

Mi = P_i−1+ Si −1 (2.8a) ˆ xi = ˆx + Mi(yi−Sixˆi) + Mi X j∈Ni ˆ xj−xˆi (2.8b)

(26)

The term in (2.8b) is called the consensus gain and is a tuning parameter. A higher gain gives faster convergence of the node states. A gain too high could make the system unstable, while it might not converge at all if set too low. As a standard choice it is chosen as the sampling time T initially. The equivalence of Kalman gain in the Kalman filter is stated in (2.8a), which weighs the measure-ments by the covariance matrices P and S, and makes the estimate land some-where between the measured and estimated states. A large covariance means high uncertainty in the measurements, giving more weight to the predicted esti-mates from the previous time instance and vice versa. The complete algorithm for Kalman-Consensus filter is presented in Algorithm 1.

Algorithm 1Kalman-Consensus Filter

1: initialization: Pi = P0, ˆx = x(0) 2: whilenew data exists do

3: Locally aggregate and send data and covariance matrices, each node commu-nicates its states uj, Ujand ˆxjto all neighbouring nodes Ji.

For each j do Ji = Ni∪ {j} (2.9a) uj = HjTR −₁ j zj, ∀j ∈ Ji , yi = X j∈Ji uj (2.9b) Uj = HjTR −₁ j Hj, ∀j ∈ Ji , Si = X j∈Ji Uj (2.9c)

4: Compute the Kalman-Consensus estimate:

5: Update the state of the Kalman-Consensus filter:

Pi[t + 1|t] = AMiAT + BQBT (2.11a)

ˆ

xi[t + 1|t] = A ˆxi[t|t] (2.11b)

end for

6: Predict for next time step (t + 1)

(27)

2.5 Extended Kalman-Consensus filtering 11

2.5 Extended Kalman-Consensus filtering

The Kalman-Consensus filter works with linear systems as previously stated, and in many applications the system is not necessarily linear. In the case of a non-linear system the Extended Kalman-Consensus filter (EKCF) is analogously with the EKF linearized around a working point with Taylor expansion [11]. This filter is also the main subject of this thesis. Linearization of the observation matrices is done in the next chapter and is based on work done in the previous thesis.

2.5.1 Filtering

While the algorithm is similar to the Kalman-Consensus, there are differences that we will sort out in this section. To begin with, the observation matrices are linearized each time step around the latest state estimate ˆx. Using the same

state space model as in (2.4) but with different notation, the measurement vector becomes:

y (k) = h (x (k)) + v (k) (2.12)

To catch the non-linear dynamics of the systems, the new sensor model matrices are linearized using Taylor expansion analogously to the EKF as:

h (x) = h ( ˆx) + dh ( ˆx)

d ˆx (x − ˆx) (2.13)

H = dh ( ˆx)

d ˆx (2.14)

whereas H is the Jacobian of h ( ˆx), giving the characteristics of the system around

the observation point x. Instead of using z directly, the difference between the measured and the predicted measurement is used as input to the filter. Via direct substitution (2.5a) can be rewritten to

u = HTR−1(z − h ( ˆx)) (2.15)

where each node calculates the predicted measurements h ( ˆx) using its own state

vector ˆxi. The non-linear time-uptade of the state vector becomes

ˆ xi+1= ˆxi + Mi(yi−Sixˆi) + γ Pi X j∈Ni ˆ xj−xˆi (2.16) where the Kalman gain Mi is the same as in (2.10a) but is based on a linear h (x).

The new consensus gain γ is weighted by the nodes own covariance matrix and is defined as

γ = _k

Pik+ 1

(28)

where is the same consensus gain used as a tuning parameter as in the KCF. The complete algorithm for the Extended Kalman-Consensus filter is presented in Algorithm 2

Algorithm 2Extended Kalman-Consensus Filter

1: initialization: Pi = P0, ˆx = x(0) 2: whilenew data exists do

3: Locally aggregate data and covariance matrices For each node j do:

Ji = Ni∪ {i} (2.18a) Hj = δhj( ˆx) δ ˆx (2.18b) uj = HjTR −₁ j zj−hj( ˆx) , ∀_{j ∈ J}_i _(2.18c) Uj = HjTR −₁ j Hj , ∀j ∈ Ji (2.18d) gj = X j∈Ni ui−Ui ˆ xj−xˆi (2.18e)

4: Compute the Kalman-Consensus estimate:

Mi = P_i−1+ Si −1 (2.19a) ˆ xi+1= ˆxi + Migi + γ Pi X j∈Ni ˆ xj− ˆxi (2.19b)

5: Update the state of the Kalman-Consensus filter:

Pi+1= AMiAT + BQBT (2.20a)

ˆ

xi+1 = A ˆxi (2.20b)

end for

6: Predict for next time step (t + 1)

(29)

3

Modeling

In this chapter the models used in the simulation environment are presented. Be-ginning with the aircraft motion modeling, with descriptions of how the aircraft trajectory is simulated. We continue with the sensor modeling, where the sensors are modeled to simulate quantities with added measurement noise and bias. The last part concerns the estimation model, linearization of the observation matrices and measurement prediction. The measurement and measurement predictions are then used as input to the filtering algorithm. An overview of how the simula-tions are performed is shown in Figure 3.1.

Figure 3.1: Introductional overview of the complete simulation environ-ment. From reference trajectory input to estimation.

3.1 Simulation model

In this section we will describe how the true reference motion of all aircraft are calculated from reference input to kinematic quantities, which is then sent to the simulated sensor environment. A flow chart describing this model is shown in Figure 3.2.

(30)

14 3 Modeling

Figure 3.2:As reference input, aircraft acceleration and angular velocity are used as input, out of which the rest of the kinematic quantities are calcu-lated.

3.1.1 Coordinate system

Before we explore the kinematics of the aircraft motion we need to introduce some coordinate systems. A reference frame which assumes a flat earth, contain-ing east, north and up as reference vectors is used here.

Within the local coordinate system we need to use two additional frames to re-late the aircraft kinematics to our earth reference system. The navigation frame (NAV) and the body frame (BODY). The base vectors of the NAV frame are point-ing in the earth east (xN), north (yN) and up (zN) directions, with the origin in the aircraft center of mass. The body frame also uses the center of mass as origin, but the base vectors are pointing in the aircraft forward (xB_{), left (y}B_{) and up z}B

direction. A visualization of the NAV and BODY frames with conventional names of the relative angles are shown in figure 3.3.

(31)

3.1 Simulation model 15

Figure 3.3:Visualization of BODY coordinates and its relative angles to the NAV frame

3.1.2 Aircraft kinematics

The aircraft motion model is based on a polar coordinated turn model [12], where the vertical (z) direction is decoupled from the 2D horizontal (x and y) motion. The input to the model are 1D reference signals, designated a, azand ω (t). a is

the acceleration in the aircraft xB direction, az is the acceleration in the altitude

direction. ω is the angular velocity around the zB axis (yaw angle direction in Figure 3.3). ˙xN(t) = vB_h(t) cos (Ψ (t)) , (3.1a) ˙yN(t) = vB_h(t) sin (Ψ (t)) , (3.1b) ˙zN(t) = vBz (t) , (3.1c) ˙ Ψ (t) = ω (t) , (3.1d) ˙vB_h(t) = aB(t) , (3.1e) ˙vB_z (t) = az(t) , (3.1f) θ (t) = arcsin v B z (t) v (t) ! , (3.1g) vB_h(t) = v (t) cos (θ (t)) (3.1h)

vhis the aircraft velocity in xBdirection in the horizontal plane. v (t) is the

veloc-ity in the aircraft motion direction. vzis the velocity in the zBdirection, Ψ is the

yaw angle (see Figure 3.3) defined from the xBaxis. θ (t) is the angle in the pitch angle direction (Figure 3.3) defined from the xBaxis. Since no characteristics of

(32)

16 3 Modeling

the airplanes roll rate is considered in this simulation model, it is not included in the equations.

The system can be discretized using xk = x (kT ) for a sampling time T :

xN_k+1= xN_k + 2vh,k ωk sin _ω kT 2 cos _ω kT 2 + Ψk , (3.2a) y_k+1N = Y_kN +2vh,k ωk sin _ω kT 2 sin _ω kT 2 + Ψk , (3.2b) z_k+1N = z_kN+ T vz,k , (3.2c) Ψ_k+1= Ψk+ T ωk , (3.2d) vh,k+1= vh,k+ T ak , (3.2e) v_z,k+1N = vz,k+ T az,k, (3.2f) θk = arcsin       v_z,kB vk       , (3.2g) vh,k = vkcos (θk) (3.2h)

Relation between BODY and NAV

To transform coordinates from BODY to NAV the following rotational matrix can be used: R(Ψ ) =         cos Ψ −_{sin Ψ} ₀ sin Ψ cos Ψ 0 0 0 1         (3.3a) Using this rotational matrix, coordinates in the BODY frame can be expressed in the NAV frame as:

h x1 x2 x3 iN = R (Ψ )hx1 x2 x3 iB (3.4a) The rotational matrix R (Ψ ) is used when generating INS measurements described in the next section.

3.2 Sensor modeling

The kinematic quantities from (3.2a) are used as input to the sensor models. An overview of how this is done is shown in 3.4, where the input are sent to differ-ent blocks with differdiffer-ent sensors. Each sensor block simulates measuremdiffer-ent by adding noise and bias, a remark is that all sensors are not used at every time instance. The generated measurement vector is then used as input to the filter algorithm. The derivation for GPS, INS and TOA is already done by Magnusson in [6] but is presented here to give the reader a complete view of the models used:

(33)

3.2 Sensor modeling 17

Figure 3.4:The true kinematic quantities are used as input to get the mea-surement vector.

3.2.1 INS measurements

The INS measurements simulates accelerators and a gyro, they are sampled in the BODY frame and gives the acceleration in three dimensions and angular velocity in one dimensions, with added noise and bias according to:

ωk = ωTk + b I N S,ω 0 + e I N S,ω k , (3.5a) aB_k = ak+ bI N S,a0 + e I N S,a k (3.5b) aN_k = R (Ψk) aBk , (3.5c) eI N S,ω_k ∼ N_{(0, σ}_ω₎ _(3.5d) eI N S,a_k ∼ N_{(0, σ}_a₎ _(3.5e)

Where ωkdenotes measured angular velocity, aBk denotes measured acceleration

in BODY frame, aN_k denotes measured acceleration in NAV frame, b0 denotes

INS bias error.The complete measurement vector for INS measurements is finally defined as:

y_ka= aB_k, (3.6a)

y_kω = ωk , (3.6b)

y_kI N S =hy_ka y_kωiT (3.6c)

(34)

18 3 Modeling

3.2.2 GPS measurements

The GPS measurements simulates the situation where the aircraft has connection with GPS-satellites and generates measurement noise- and bias. The bias error typically comes from varying atmospheric properties. The bias in the simulation environment is uniformly distributed and together with measurement noise the maximum position error that can be generated is set to 3 meters in a random direction.

p_kGP S= [ xk yk zk]T (3.7a)

bGP S,p₀ = [ bxGP S,p bGP S,py bGP S,pz ]T (3.7b)

e_ki,p∼ N _{0, σ}_p _(3.7c)

Adding these vectors together we finally gain the measurement vector yGP S_k

yGP S_k = pk+ bGP S,po + eGP S,p_k (3.8)

where pk denotes true position, b GP S,p

o denotes bias error for GPS position, and

eGP S,p_k denotes white gaussian noise. Finally yk is the measurement vector used

as input to the filtering environment.

3.2.3 TOA measurements

The TOA measurements are built in the radio link used in the simulation environ-ment. This radio link resembles the LINK-16-standard which is used by NATO and the US air force [1]. The LINK-16 has built in support for TOA measure-ments. In short, LINK-16 measures the time arrival of transmissions and uses the time differences to calculate the relative distances. This requires the network to be synchronized with accurate clocks. In this thesis the clocks are assumed to already be synchronized. The TOA model for measurements is defined as:

r_ki,T OA= rk+ boi,r+ eki,r (3.9a)

rk = kpo−pik (3.9b)

e_ki,r ∼ N_{(0, σ}_r₎ _(3.9c)

where ri represents the distance to aircraft i. By summing these together, the

measurement vector y_kT OAis given as:

yT OA_k = r_ki,T OA , i = 1 ... Nac−1 (3.10)

which is used as input to the filter. A visualiztion of the TOA measurements is shown in Figure 3.5.

(35)

3.2 Sensor modeling 19

Figure 3.5:The TOA measurements measure the distance ri,j from aircraft i

to j

.

3.2.4 DOA measurements

TOA via radio link is an excellent tool when it comes to estimating range. How-ever, it lacks accuracy when it comes to estimating angle to a tracked object. To estimate angle to a tracked object while using a TOA-solution, you have to rely heavily on your model to estimate the angle. While the distance to an object is accurately estimated, some drift may be caused due to incomplete modeling or a change in the objects velocity vector.

The opposite situation would be to instead measure the angle to the tracked ob-ject, such a solution is usually called direction of arrival (DOA). Meaning that the angle to the object is well estimated while the filter relies on the model used to estimate the range to the tracked object. A sensor capable of doing such thing is the electro-optical camera (EO-camera), which measures light beams and con-verts them to electrical signals.

IR-camera model

An EO-camera can work with light on different wavelengths, if the wavelengths detected are within a ranges higher than 750 nm, it is typically called an IR-camera. Detecting wavelengths on the IR-band is beneficial when it comes to fighter aircraft, since jet-engines produce a lot of heat and a IR-camera is a pas-sive sensor. The camera being paspas-sive means that there is no way for the target to know if you are looking on it with the camera, in contrast to active radars, which sends out an electromagnetic signal that can be detected on the other end. For each aircraft in range, the angles in the xy- and yz-plane is measured as:

(36)

20 3 Modeling " βi αi # =       φi +e_kI R,p θi +e_kI R,p       (3.11a) e_kI R,p∼N_{(0, σ}_{I R}₎ _(3.11b)

where φi and θi is defined as

"φi θi # =         arctan y0 x0 arctanz_r00         (3.12)

and the angles can be visualized in figure 3.6 and 3.7

Figure 3.6:The DOA measurements measure the relative angle between the aircraft nose direction (xB) and the measured aircraft. Here the horizontal angle is shown.

Figure 3.7:The DOA measurements measure the relative angle between the aircraft nose direction (xB) and the measured aircraft. Here the vertical angle is shown.

(37)

3.3 Estimation 21

3.3 Estimation

The last step in the filtering environment is to use the generated measurements in the filter algorithm to estimate the aircraft states and predict for next time update. In this section we will define the state vector, measurement prediction vector, observation matrices and the kinematic model used in Algorithm 2

Figure 3.8: The measurements from the sensor models and the predicted measurements are used as input to the filtering environment.

3.3.1 Filter states

Each aircraft estimates 16 own states, plus 16 states per tracked aircraft. The own states estimated includes 9 states for position, velocity and acceleration. Two states represent angle in the horizontal plane and its angular velocity. Four states are used to estimate INS-bias and one state to estimate TOA-bias. The states for estimating position, velocity and acceleration is defined as:

pi = [ pxi pyi pzi] T _, _(3.13a) vi = [ vxNi v N yi v N zi ] T _, _(3.13b) ai = [ aNxi a N yi a N zi] T _, _(3.13c) (3.13d)

(38)

22 3 Modeling

Where p,v and a represents position, velocity and acceleration for a single aircraft with index i. The states used to estimate INS-bias is denoted as following:

bI N S_i =hbax bay baz bωx

iT

(3.14a)

Where each b is used to estimate INS-bias for measured acceleration and angular velocity. The complete state vector for each tracked aircraft is then denoted as:

xi =

h

p_iT vT_i aT_i Ψi ωi bI N S,T_i bT OA_i

i

(3.15a)

Where xi denotes the complete state vector for aircraft i and bT OAi denotes the

estimated TOA-bias. The complete state vector used in the filtering environment can then be defined as

xo = [ xT₁ , ... , x_NT_ac]T (3.16) where o denotes own state vector of the local filter and Nac is the number of

tracked aircraft.

Using Algorithm 2 both the measurement vector y and measurement prediction

h (x) are needed. The measurement prediction ˆy = h ( ˆx) is calculated using ˆxt|t−1.

3.3.2 Kinematic model

The kinematic model is used to describe how each of the aircraft states change between measurements. First a continuous model is presented, which then is de-rived into a discrete time model using exact linearization. The result is a state space model of the form presented in (2.3). The first states presented below de-scribes the translational motion three dimensions:

˙xtr= Ftrxtr_k + Gtr_ω ωa_k (3.17a) Ftr=         0 I3 0 0 0 I3 0 0 0         (3.17b) Gωtr= h 0 0 I3 iT (3.17c) ω_ka∼ N _{(0, Q}a₎ _(3.17d)

and in discrete time using exact discretization, the kinematic translational model becomes

(39)

3.3 Estimation 23 xtr_k+1= Ftrx_ktr+ Gtrωwωk (3.18a) Ftr =          I3 T I3 T 2 2 I3 0 I3 T I3 0 0 I3          (3.18b) Gtrω = h_T3 6 I3 T 2 2 I3 T I3 iT (3.18c) ωa∼ N_{(0, Q}a₎ _(3.18d)

The following rotational model is used to describe how each aircraft rotates in the yaw axis (described in fig. 3.3)

˙xrot ="0 1₀ ₀ #

xrot+"0₁ #

ωω (3.19)

which with exact discretization becomes

x_k+1rot = Frotxrot_k + Grot_ω ωω_k (3.20a)

Frot ="1 T₀ ₁ # (3.20b) Grot_ω =hT2 2 T iT (3.20c) ωω ∼ N_{(0, Q}ω₎ _(3.20d)

in discrete time analogously as before. The error model for INS- and TOA-bias states (xb,I N S = [babωbT OA]T and wb = [ωb,aωb,ωωb,T OA] are defined in continu-ous time as:

˙xb= I5 (3.21)

and in discrete time:

xb_k+1= Fbxb_k+ Gbωωbk (3.22a) Fbxb_k =         I3 0 0 0 1 0 0 0 1         (3.22b) Gbω= h T I3 T T iT (3.22c) Qb=          Qb,a 0 0 0 Qb,ω 0 0 0 Qb,T OA          (3.22d)

(40)

24 3 Modeling

and the complete model in discrete time for a single aircraft is given by

xk+1= Fxk+ Gωωω (3.23a) F =         Ftr ₀ ₀ 0 Frot 0 0 0 Fb         (3.23b) Gω =         Gωtr 0 0 0 Gωrot 0 0 0 Gωb         (3.23c)

which in discrete time can be written

xk+1= Fxk+ Gωwk (3.24a) F =           F1 ... 0 .. . . .. ... 0 . . . FNac           (3.24b) Gω=           G1 ... 0 .. . . .. ... 0 . . . GNac           (3.24c)

which forms the system matrices used to describe aircraft motion.

3.3.3 Observation matrices

Since non-linear filtering is used here, the observation matrix H needs to be lin-earized, the observation matrix is defined as [4]:

H ( ˆxk) ≡ ∂h ( ˆxk) ∂ ˆxk _{x= ˆ}_x (3.25a) INS

For the INS measurement prediction, the following measurement model is used:

hB,I N S( ˆxk) =

h

ao_k+ ba_k ωk+ bo,ωk

i

(3.26) Where ak, ba, ωk and bo,ωk is taken from state vector

(41)

3.3 Estimation 25 Ho,I N S= ∂h o,I N S_(x) ∂x _{x= ˆ}_x k|k−1 (3.28a)

The observation matrix is then retrieved by derivation with respect to accelera-tion and angular velocity

Ho,a=hI3 03×2 I3 03×1 i (3.29a) Ho,ω=h01×4 1 01×3 1 i (3.29b)

Ho,I N S0=hHo,a Ho,ωiT (3.29c) which put together yields the matrix;

Ho,I N S=h03×6 Ho,I N S

0

03x1

i

(3.30a) where Ho,I N Sis the observation matrix used in the filtering environment.

GPS

The measurement prediction for GPS is defined as:

hGP S( ˆx) = po_k (3.31a)

where pkis taken from the own state vector. Using (3.25a) the observation matrix

of the GPS measurements becomes:

Ho,GP S =hI3 03x13

i

(3.32) and that is the sensor model used for filtering.

TOA

The measurement prediction for TOA measurements is defined as:

hT OA( ˆx) = kpo_k−_pi

kk+ b o,T OA

(42)

26 3 Modeling

Where po, pi and bo,T OAare retrieved from the state vector. Deriving this with respect to p we get the following equations:

H_kLink,r = [ H_k0 H_kr H_kb] (3.34a) H_ko = [ H_k1,o . . . H_kN ,o]T (3.34b) H_kr = [ H_k1,r . . . H_kN ,r]T (3.34c) H_kb= diagH_k1,b . . . H_kN ,b (3.34d) H_ki,o= " (p_ko−_pi k) T k_po k−pikk 01×12 # (3.34e) H_ki,r = − " 01×3(i−1) ( po_k−_pi k) T k_po k−pikk 01×3(i−1) # (3.34f) H_ki,b= 1 (3.34g) i = 1, . . . , Nac (3.34h)

where H_kLink,ris the observation matrix used for filtering.

DOA

The measurement prediction for the DOA measurements is defined analogously with the measurement model:

hDOA( ˆx (k)) = arctan y0 x0 arctanz_r00 (3.35a) x0= pi_x,k−_po x,k (3.35b) y0= pi_y,k−_po y,k (3.35c) z0= pi_z,k−_po z,k (3.35d) r0= r pi_x,k−_po x,k 2 +pi_y,k−_po y,k 2 (3.35e)

where positions p is taken from the own state vector for own (o) and other (i aircraft states. Deriving with respect to position gives the following observation matrices:

(43)

3.3 Estimation 27 HDOA=∂h DOA ∂x |x= ˆxk|k−1 (3.36a) H_pDOA0 x = x0 x0₂ + y0₂ (3.36b) H_pDOA0 y = y0 x02_{+ y}02 (3.36c) H_pDOA0 z = r0 r02_{+ z}02 (3.36d)

HDOA= diagHpDOAx0 H

DOA py0 H DOA pz0 (3.36e) where HDOAis the matrix used for filtering.

(44)

(45)

4

Results

4.1 Simulation environment

The simulation environment is largely based on the previous work [6]. The fol-lowing scenarios are the ones used for simulating different setups of aircraft and landmarks. In order to test the aircraft positioning performance under different conditions we present the different scenarios here. The defined scenarios are sim-ulated using Monte Carlo simulations.

4.1.1 Scenario 1

Scenario 1 is a reference scenario and consists of one aircraft using inertial nav-igation only. It will fly with constant speed of 350 m/s in a straight line until it reaches 800 km. The setup is shown in Figure 4.1.

4.1.2 Scenario 2

In this scenario a group of 4 aircraft is used, divided into two groups of two aircraft each, within the group the aircraft are located 200 meters in both the horizontal and vertical plane, while the attitude is kept the same. The initial distance between the groups is 50 km. Using the setup shown in Figure 4.1, the purpose is to study and evaluate the performance when the GPS signal is lost for three different aircraft at different time instances. The time instances are divided as follows:

GPS is lost for aircraft 1,2 and 3 when: • Aircraft 1: When aircraft 1 reaches 200 km. • Aircraft 2: When aircraft 1 reaches 400 km.

(46)

30 4 Results

Figure 4.1:Describing figure of Scenario 1

• Aircraft 3: When aircraft 1 reaches 600 km.

• Simulation is completed when aircraft 1 reaches 800 km

4.1.3 Scenario 3

In this scenario four aircraft is used, the purpose of this scenario is to study how the relative distances are kept when all aircraft loses their GPS signal. The initial geometry of the aircraft is the same as in scenario 1, but the occurring events is as follows:

• The GPS is lost for all aircraft when aircraft 1 reaches 200 km. • Simulation is completed when aircraft 1 reaches 800 km

(47)

4.1 Simulation environment 31

4.1.4 Scenario 4

In this scenario a group of four aircraft with the same initial positioning as in Scenario 1 is used together with two landmarks. The purpose of this scenario is to study how geometry of the landmarks affects the positioning performance of the aircraft. the changes occurring during the flight is as follows:

• GPS is lost for all aircraft when aircraft 1 reaches 200 km.

• Landmark 1 starts communicating with all aircraft when aircraft 1 reaches 500 km.

• Landmark 2 starts communicating with all aircraft when aircraft 1 reaches 700 km.

(48)

32 4 Results

4.2 Evaluation

The performance of the solutions in each scenario are evaluated using position data in two dimension and the relative distances between aircraft is compared to the true data. As a measure of the error, Root Mean Square Error (RMSE) is used, which is a statistical method used to estimate the standard deviation. The formula for calculating the average RMSE for all simulations ís shown in (4.1)

RMSE =

s PN

i=1(xt−xˆt)2

N (4.1)

Where N is the number of simulations, ˆxtis the estimated quantity and xtis the

true quantity.

4.3 Results

In this section results from the simulations are presented. In each subsection plots from both used filters are presented and discussed.

4.3.1 Scenario 1

For comparison, each filter will be compared to this basic scenario of pure inertial navigation, the randomized bias is the same as the other scenarios. In Figure 4.5 the result is presented, after 2285 seconds of simulation the position error is just above 500m, in this scenario only an EKF-filter has been used, as there is little meaning to use an EKCF for a single node.

(49)

4.3 Results 33

Figure 4.5:Pure inertial performance Scenario 1

In figure 4.6 all trajectories estimated by the aircraft in 20 simulations is shown, the trajectories drifts away in a way that can be expected of an inertial navigation system:

(50)

34 4 Results

Figure 4.6:Trajectories estimated in aircraft

4.3.2 Scenario 2

The results of the simulations of Scenario 2 is presented here, with some analyzed results from the plots.

Absolute position

While the filters performs similar to each other when the GPS is enabled, the EKCF outperforms the EKF in terms of RMSE at all time instances when the GPS is lost for each aircraft successively, as seen in Figure 4.7. The consensus filter seems to be much better at estimating position with TOA measurements as long as at least one plane has a GPS signal, since the consensus filter yields about the same RMSE as long as at least one aircraft has GPS signal.

(51)

4.3 Results 35

Figure 4.7:Absolute position RMSE Scenario 2

Relative positions

For relative positioning (Figure 4.8 we can see an increase in performance for between aircraft 1-2, while both filters perform similar for aircraft 1-3 and 1-4 after 1500 seconds. The reason for the difference in performance is likely due to the distance between them, with aircraft 1-2 being 283 meters apart and 1-3 and 1-4 being about 50000 meters apart.

(52)

36 4 Results

Figure 4.8:Relative positions Scenario 2

Covariance

In Figure 4.9 we can see the estimated position covariance plotted together with RMSE for the aircraft position. As we can see the covariance increases signifi-cantly when the GPS is lost for each aircraft. The uncertainty of each aircraft being large means each aircraft has large uncertainty of its position, even though the RMSE is small. Notably the EKF has lower covariance even though it has worse RMSE for absolute position for aircraft that has lost its GPS signal.

(53)

4.3 Results 37

Figure 4.9:Covariance P for each aircraft, Scenario 2

TOA bias characteristics

As we can see in Figure 4.10, the EKCF can estimate the TOA bias while the air-craft still has the GPS signal, with the EKF being slower and may not necessarily converge to the correct value. The bias estimate of the EKF tends to drift after losing GPS signal, likely following the drift in the estimated relative position.

(54)

38 4 Results

Figure 4.10:Estimated TOA bias Scenario 2

INS bias characteristics

The INS bias is relatively small compared to other states, typically somewhere in the magnitude of 10−7to 10−3, which makes this state a bit more susceptible to noise when calculating the predicted measurements. As we can see in Figure 4.11 the estimation is somewhat correct in the mean for both filters in most cases, but is very noisy.

(55)

4.3 Results 39

Figure 4.11:Estimated INS bias Scenario 2

Estimated horizontal angle

The estimated horizontal angle is estimated according to 4.12, it consists only of INS measurements and measurements from other planes are not taken into consideration for when calculating the angle.

(56)

40 4 Results

Figure 4.12:estimated angle and angular velocity, horizontal plane, Scenario 2

4.3.3 Scenario 3

For scenario 3, the scenario where all aircraft loses GPS at the same time, has its results from simulations presented here.

As can be seen in Figure 4.13 the absolute position error drifts away for both fil-ters, with both drifting away with what appears to be a quadratic trend. Based on these results one can’t say that either is clearly better than the other.

(57)

4.3 Results 41

Figure 4.13:Estimated absolute position error, Scenario 3

Relative position

With relative position the case is similar to absolute position, as absolute position accuracy decreases so does relative position (see Figure 4.14). The TOA measure-ments seems to not be able to adjust for the fault created by INS drifting (Figure 4.15).

(58)

42 4 Results

(59)

4.3 Results 43

(60)

44 4 Results

4.3.4 Scenario 4

The results of the simulations of Scenario 4 is presented here, with plots of vari-ous interesting results from the simulations. As before the scenario is simulated 20 times and the result shown is the average of them.

The RMSE of absolute position of each filter in Scenario 4 is shown in Figure 4.16. This scenario has three interesting events, when all aircraft loses GPS signal and starts communicating with the two landmarks consecutively. At the first point of interest, at t = 600, all aircraft losing their GPS signal makes their positioning error increase in a drifting manner for both filters, as now they only have their previous positions and TOA measurements to each other to rely on. The reason for drifts is mainly due to measurement bias and noise.

At both time instances when the landmarks starts broadcasting, we can see a jump in the RMSE for both filters. The reason for the jump is because new mea-surements are introduced that adds new information, with TOA ranges to- and GPS position from the landmarks. It may be possible that the RMSE would drop even further with different tuning of filter parameters and initial covariance of the landmarks. The EKCF outperforms the EKF in terms of RMSE in absolute po-sition at all times, mainly due to the consensus filter utilizing more information from multiple nodes.

Covariance

The covariance for each aircraft and filter is shown in Figure 4.19. The interesting events happens at the same time instances as the absolute position. Initially all aircraft have GPS signal, making the position uncertainty small which is reflected in the low covariance of both filters. As the GPS signal is lost and positioning performance starts to drift it is also reflected in the plots, analogously when the position estimate improves when the landmarks starts to broadcast, the covari-ance is decreased again. Notably the covaricovari-ance is much smaller for the EKCF than the EKF, and the covariance follows the position performance better for the EKF, as seen in Figure 4.20.

(61)

4.3 Results 45

(62)

46 4 Results

(63)

4.3 Results 47

(64)

48 4 Results

(65)

4.3 Results 49

(66)

50 4 Results

Relative positions

In figure 4.21 the relative positioning performance for scenario 4 is shown. The interesting events happens at the same time instances as previous section, when GPS is lost and landmarks start to broadcast. The EKF seems to have an offset when the GPS-signal is still present. The offset follows from the EKF filter only estimating the position of other aircraft, rendering the prediction for next time update the same as the current estimate, making the estimate "slip" one time in-stance. Notable here is that both filters behaviour resembles each other, with the RMSE being notably smaller for the EKCF. One could here expect better results with the EKCF, but the has a hard time utilizing TOA measurements fully, due to not being able to estimate the bias of those measurements properly, as will be explained in the next section.

(67)

4.3 Results 51

Figure 4.22:Scenario 4 estimated INS-bias

TOA-bias characteristics

The TOA-bias is well estimated for as long as each aircraft has GPS-signal, after which it starts to drift. The drift is more outstanding with the EKCF than the EKF in two out of four cases.

As with previous scenarios TOA-bias is well-estimated as long as the GPS-signal is available, after which it tends to drift. Interesting here is that overall the EKF is better at estimating the TOA bias than the EKCF.

INS bias characteristics

(68)

(69)

5

Conclusions

5.1 Discussion

In this thesis the efficiency of the EKCF has been tested, by putting up a simula-tion environment and comparing it to the more convensimula-tional extended Kalman filter using the same state space model. The idea of sharing data between nodes through a consensus filter sounds promising though the state vector might have been too large rendering the filter hard to tune.

Considering all of the simulations it can be beneficial to run a consensus filter, for example slightly better estimation and redundancy. Drawbacks would be that the EKCF puts a lot of strain on the computing side, larger matrix and an extra matrix inverse increases the requirements on the CPU compared to the EKF. The latter may be solved by investigating whether it is possible to only send the dif-ferences of the matrix, which may make it easier to compress it for saving some bandwidth on the communication system. Another solution to reduce the needed computing power would be to rewrite some equations to both increase accuracy and maybe reduce computational strain.

The consensus filter seems to not be as effective as the EKF on certain areas, such as estimating TOA-bias. This may be due to position error leaking over to the bias estimation, as the only measurement for the bias is previous estimated distance and new measured distance to other aircraft. Overall this kind of consensus filter may be inefficient, as the whole state vector is sent to all other nodes while all states are not observable from each node. A solution to this would be to reduce the number of states in the state vector or find some way of sending only a par-tial state vector to other aircraft, or to only track position and velocity of other aircraft.

(70)

54 5 Conclusions

5.2 Future work

For the consensus filtering, all states of all aircraft are included in the state vec-tor. All aircraft estimating every other planes states, including biases for their own TOA-measurements and inertial sensors might seem unnecessary. To free computational power and communication link strain, it would be interesting to estimate less of these internal states. A solution to this is to only use these states locally and only send a vector containing e.g. position and velocity to all other aircraft. Though this will not be correct consensus-filtering, as the corresponding states in the covariance matrix also has to be removed. This works fine as long as the cross-covariance between the kept and removed states are zero, which they typically are not. A possible future work of this thesis could be to investigate if it is possible to evaluate these cross-covariances within each aircraft, which could make the communicated state-vectors and matrices significantly smaller and ease the computational requirements.

(71)

Bibliography

[1] A. Brown and P. Sack. Navigation using link-16 gps-ins integration. http://www.navsys.com/papers/0309001.pdf. page 1, 2003. Cited on page 18.

[2] R. Carli, A. Chiuso, L. Schenato, and S. Zampieri. Distributed kalman filter-ing based on consensus strategies. Selected Areas in Communications, IEEE Journal on, 26(4):622–633, 2008. Cited on page 8.

[3] F. Gustafsson. Signal Processing. Studentlitteratur, 2011. Cited on page 7. [4] F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2012. Cited on

pages 8, 9, and 24.

[5] R. E. Kalman. A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 82:42, 1960. Cited on pages 7 and 8.

[6] D. Magnusson. A network based algorithm for aided navigation. pages 15– 41, 2008. Cited on pages 3, 16, and 29.

[7] J. Mochnac, S. Marchevsky, and P. Kocan. Bayesian filtering techniques: Kalman and extended kalman filter basics. In 2009 19th International Con-ference Radioelektronika, pages 119–122, 2009. Cited on page 8.

[8] R. Olfati-Saber. Distributed kalman filter with embedded consensus filters. In Proceedings of the 44th IEEE Conference on Decision and Control, pages 8179–8184, Dec 2005. Cited on page 8.

[9] R. Olfati-Saber. Distributed kalman filtering for sensor networks. In 2007 46th IEEE Conference on Decision and Control, pages 5492–5498, 2007. Cited on page 9.

[10] R. Olfati-Saber. Kalman-consensus filter : Optimality, stability, and perfor-mance. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, pages 7036– 7042, 2009. Cited on pages 3 and 8.

(72)

56 Bibliography

[11] A. Pellet. The extended kalman-consensus filter. B.S. University of Maine, 2009. Cited on pages 3 and 11.

[12] M. Roth, G. Hendeby, and F. Gustafsson. EKF/UKF maneuvering target tracking using coordinated turn models with polar/cartesian velocity. In In-formation Fusion (FUSION), 2014 17th International Conference on, pages 1–8. IEEE, 2014. Cited on page 15.

[13] W. Yu, G. Chen, Z. Wang, and W. Yang. Distributed consensus filtering in sensor networks. IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), 39(6):1568–1577, 2009. Cited on page 8.