Passive ranging

Institutionen för systemteknikDepartment of Electrical Engineering

Examensarbete

Angle-Only Target Tracking

Examensarbete utfört i Reglerteknikvid Tekniska högskolan i Linköping

av

Tina Erlandsson

LITH-ISY-EX--07/3904--SE

Linköping 2007

Department of Electrical Engineering Linköpings tekniska högskolaLinköpings universitet Linköpings universitetSE-581 83 Linköping, Sweden 581 83 Linköping


Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköpingav

Tina Erlandsson


Handledare: Per-Johan Norldund

isy, Linköpings universitet

Sören Molander

Saab Aerosystems

Examinator: Rickard Karlsson

isy, Linköpings universitet

Linköping, 23 February, 2007

Avdelning, Institution

Division, Department

Division of Automatic ControlDepartment of Electrical EngineeringLinköpings universitetSE-581 83 Linköping, Sweden

Datum

Date

2007-02-23

Språk

Language

Svenska/Swedish

Engelska/English

⊠

Rapporttyp

Report category

Licentiatavhandling

Examensarbete

C-uppsats

D-uppsats

Övrig rapport

⊠

URL för elektronisk version

http://www.control.isy.liu.sehttp://www.ep.liu.se/2007/3904

ISBN

—

ISRN


Serietitel och serienummer

Title of series, numberingISSN

—

Titel

TitleVinkelbaserad målföljning i en UAV


Författare

AuthorTina Erlandsson

Sammanfattning

Abstract

In angle-only target tracking the aim is to estimate the state of a target with useof measurement of elevation and azimuth. The state consists of relative positionand velocity between the target and the platform. In this thesis the trackingplatform is an Unmanned Aerial Vehicle (UAV) and the tracking system is meantto be a part of the platform’s anti-collision system. In the case where both thetarget and the platform travel with constant velocity the angle measurementsdo not provide any information of the range between the target and the plat-form. The platform has to maneuver to be able to estimate the range to the target.

Two filters are implemented and tested on simulated data. The first filteris based on a Extended Kalman Filter (EKF) and is designed for tracking non-maneuvering targets. Different platform maneuvers are studied and the influenceof initial errors and the geometry of the simulation scenario is investigated. Thefilter is able to estimate the position of the target if the platform maneuvers andthe target travels with constant velocity. Maneuvering targets on the other handcan not be tracked by the filter.

The second filter is an interacting multiple model (IMM) filter, designed fortracking maneuvering targets. The filter performance is highly dependent of thegeometry of the scenario. The filter has been tuned for a scenario where the targetapproaches the platform from the front. In this scenario the filter is able to trackboth maneuvering and non-maneuvering targets. If the target approaches theplatform from the side on the other hand, the filter has problems with distinguishtarget maneuvers from measurement noise.

Nyckelord

Keywords Angle-only, Target Tracking, MSC, EKF, IMM

Abstract

In angle-only target tracking the aim is to estimate the state of a target with use ofmeasurement of elevation and azimuth. The state consists of relative position andvelocity between the target and the platform. In this thesis the tracking platformis an Unmanned Aerial Vehicle (UAV) and the tracking system is meant to be apart of the platform’s anti-collision system. In the case where both the target andthe platform travel with constant velocity the angle measurements do not provideany information of the range between the target and the platform. The platformhas to maneuver to be able to estimate the range to the target.

Two filters are implemented and tested on simulated data. The first filteris based on a Extended Kalman Filter (EKF) and is designed for tracking non-maneuvering targets. Different platform maneuvers are studied and the influenceof initial errors and the geometry of the simulation scenario is investigated. Thefilter is able to estimate the position of the target if the platform maneuvers andthe target travels with constant velocity. Maneuvering targets on the other handcan not be tracked by the filter.

The second filter is an interacting multiple model (IMM) filter, designed fortracking maneuvering targets. The filter performance is highly dependent of thegeometry of the scenario. The filter has been tuned for a scenario where the targetapproaches the platform from the front. In this scenario the filter is able to trackboth maneuvering and non-maneuvering targets. If the target approaches theplatform from the side on the other hand, the filter has problems with distinguishtarget maneuvers from measurement noise.

v

Acknowledgments

I would like to thank Saab Aerosystems for an interesting thesis work. I haveenjoyed studying the problem and I have learned a lot.

Special thanks to my supervisors Sören Molander and Per-Johan Nordlund forsharing your ideas with me and specially for letting me try my own. Thanks to myexaminator Rickard Karlsson for many useful remarks on my report and for theadvice of how to use LaTeX. My opponent Joella Lundkvist has read the reportthoroughly and her questions were valuable to help me realize which parts thatneeded to be clarified.

Finally a great thanks to my family and friends for your love and for supportingme, even when you not understand what I am doing.

Linköping February 2007Tina Erlandsson

vii

Contents

1 Introduction 1

1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Existing Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 51.4 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.5 Outline of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Angle-Only Target Tracking 7

2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2 Dynamic Target Models . . . . . . . . . . . . . . . . . . . . . . . . 9

2.2.1 Constant Velocity Model (CV) . . . . . . . . . . . . . . . . 102.2.2 Constant Acceleration Model (CA) . . . . . . . . . . . . . . 102.2.3 Coordinated Turn Models . . . . . . . . . . . . . . . . . . . 11

2.3 Sensor Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.4 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.4.1 Cartesian Coordinates . . . . . . . . . . . . . . . . . . . . . 122.4.2 Modified Spherical Coordinates (MSC) . . . . . . . . . . . . 14

2.5 Platform Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . 152.5.1 Platform Maneuver Requirements . . . . . . . . . . . . . . 152.5.2 Choice of Platform Maneuver . . . . . . . . . . . . . . . . . 16

3 Tracking Filters 19

3.1 Single Model Filters . . . . . . . . . . . . . . . . . . . . . . . . . . 193.1.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . 193.1.2 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . 213.1.3 MSC-EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.2 Multiple Model Filters . . . . . . . . . . . . . . . . . . . . . . . . . 253.2.1 The IMM-Filter Algorithm . . . . . . . . . . . . . . . . . . 253.2.2 Choice of Subfilters . . . . . . . . . . . . . . . . . . . . . . . 29

4 Filter Performance Evaluation 31

4.1 Evaluation Simulations . . . . . . . . . . . . . . . . . . . . . . . . . 314.2 Performance Analysis . . . . . . . . . . . . . . . . . . . . . . . . . 32

ix

x Contents

5 Implementation 37

5.1 Implementation of the MSC-EKF . . . . . . . . . . . . . . . . . . . 375.1.1 Initialization of the State Vector . . . . . . . . . . . . . . . 375.1.2 Initialization of the Prediction Covariance Matrix P . . . . 385.1.3 Covariances Matrices for the Process Noise, Q, and the Mea-

surement Noise, R . . . . . . . . . . . . . . . . . . . . . . . 395.1.4 Correction of P . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.2 Implementation of the IMM Filter . . . . . . . . . . . . . . . . . . 405.2.1 Models in the Subfilters . . . . . . . . . . . . . . . . . . . . 405.2.2 Markov Transition Matrix and Initial Model Probabilities . 41

5.3 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 425.3.1 Simulation Scenarios . . . . . . . . . . . . . . . . . . . . . . 425.3.2 Simulation Parameters . . . . . . . . . . . . . . . . . . . . . 43

6 Simulation Results - MSC-EKF 47

6.1 Choice of Initial Prediction Covariance Matrix P . . . . . . . . . . 476.2 Choice of Platform Maneuver . . . . . . . . . . . . . . . . . . . . . 51

6.2.1 S-turns and SS-turns . . . . . . . . . . . . . . . . . . . . . . 516.2.2 Initial Time, ti, of the Platform Maneuver . . . . . . . . . . 526.2.3 Right or Left Platform Maneuver . . . . . . . . . . . . . . . 536.2.4 Conclusion on Choice of Platform Maneuver . . . . . . . . . 55

6.3 Influence of Ω and θ . . . . . . . . . . . . . . . . . . . . . . . . . . 576.4 Influence of Errors in the Initial State Vector . . . . . . . . . . . . 606.5 Target Maneuver . . . . . . . . . . . . . . . . . . . . . . . . . . . . 626.6 Monte Carlo Simulations . . . . . . . . . . . . . . . . . . . . . . . . 63

7 Simulation Results - IMM-Filter 67

7.1 Tracking Target Maneuvers . . . . . . . . . . . . . . . . . . . . . . 677.1.1 Non-Maneuvering Target . . . . . . . . . . . . . . . . . . . 687.1.2 Maneuvering Target . . . . . . . . . . . . . . . . . . . . . . 687.1.3 Target Maneuver with Time-Varying Angular Velocity . . . 72

7.2 Influence of Platform Maneuvers . . . . . . . . . . . . . . . . . . . 737.3 Influence of Initial Error in Range, ∆r . . . . . . . . . . . . . . . . 747.4 Monte Carlo Simulations . . . . . . . . . . . . . . . . . . . . . . . . 75

7.4.1 Scenario A . . . . . . . . . . . . . . . . . . . . . . . . . . . 757.4.2 Scenario B . . . . . . . . . . . . . . . . . . . . . . . . . . . 797.4.3 Scenario C . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

8 Conclusions and Futher Work 83

8.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 848.1.1 MSC-EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . 848.1.2 IMM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 868.2.1 Target Tracking in a Sense and Avoid Application . . . . . 87

Bibliography 89

Contents xi

A Relation between Cartesian Coordinates and MSC 91

A.1 Transformations from zmsc to xcar . . . . . . . . . . . . . . . . . . 91A.2 Transformations from xcar to zmsc . . . . . . . . . . . . . . . . . . 95

B The Coordinated Turn Model in Cartesian Coordinates 98

B.0.1 Platform maneuvers . . . . . . . . . . . . . . . . . . . . . . 98B.0.2 Platform and Target Maneuver . . . . . . . . . . . . . . . . 102

C Initialization of P with the Propagation of Uncertainty Formula103

D Simulation Scenarios 107

E RMSE Plots Showing the Influence of ∆r and ∆v 110

F Monte Carlo Simulations for a Non-Maneuvering Target 113

xii Contents

Notations

Variables and Parameters

α Angle describing the direction of vvvtar in azimuthβ Angle describing the direction of vvvtar in elevation∆r Error in the initial guess of range∆v Error in the initial guess of the target’s speede Measurement noiseε Innovation in the Kalman Filterϕ Azimuth angleµj Probability that the system is in model jP Prediction covariance matrixpj|i Transition probability from model i to model jQ Process noise covarinace matrixr Rangerrel Relative error in rangeR Measurement noise covariance matrixRMSEm Time mean of RMSEσ Standard deviationT Sample periodti Initial time for maneuverθ Elevation angleu Deterministic inputvvvtar Target’s velocity vectorvvvUAV UAV’s velocity vectorw Process noiseωu UAV’s angular velocityωt Target’s angular velocityxcar State vector in Cartesian coordinatesy Measurement vectorzmsc State vector in modified spherical coordinates

xiii

xiv Contents

State Vectors

xcar =(

ξ η ζ ξ η ζ)T

zmsc =(

1r ϕ θ r

r Ω θ)T

See page 13 and page 14.

Abbreviations

CA Constant AccelerationCPA Closest Point of ApproachCT Coordinated TurnCV Constant VelocityEKF Extended Kalman FilterEO-sensor Electro-Optical SensorIMM Interacting Multiple ModelKF Kalman FilterMSC Modified Spherical CoordinatesMSC-EKF Extended Kalman Filter using Modified Spherical CoordinatesRMSE Root Mean Square ErrorS&A Sense and AvoidTTG Time To GoUAV Unmanned Aerial Vehicle

Operators

x Time-derivate of xxk x(tk), x at time tkx Estimate of xxk|k−1 Estimate of x in tk given observations up to tk−1

Jf Jacobian of function fvvv The geometric vector vAT Transpose of matrix Adet(A) Determinant of matrix Av The magnitude of vector vvvdiag(α1, α2, . . . , αn) The diagonal matrix with α1, α2, . . . , αn in the main

diagonal and zeros everywhere elsePrA Propability of event APrA|B Conditional probability of event A assuming that B

has occurred

Chapter 1

Introduction

This chapter introduces the thesis work. First background and existing work inthe area are presented. Thereafter the problem is formulated and the limitationsof the thesis work are presented. Finally the thesis is outlined.

1.1 Background

Unmanned Aerial Vehicles (UAVs) are interesting for both military and civil appli-cations. UAVs can be built smaller and more fuel efficient than manned aircrafts.They can stay in the air for long periods of time and can fly in environments thatare dangerous for humans, for instance a damaged nuclear power plant. Futureapplications for UAVs are surveillance of forest fires or frontiers, minesweeping,tracking ground vehicles and fast transportation of human organs between hospi-tals. Figure 1.1 shows Neuron, a UAV that Saab is developing in cooperation withan European team.

Figure 1.1. In cooperation with an European team, Saab is developing an UAV, calledNeuron, with combat capabilities.

1

2 Introduction

Many UAV applications require that the UAV is allowed to fly in civilianairspace. This requires that the UAV is able to detect other vehicles in airspaceand to avoid collision. This is often referred to as Sense and Avoid (S&A). TheS&A-system in the UAV must be at least equally reliable as a human pilot.

The UAV can be equipped with sensors to detect other vehicles, targets, inairspace. The sensors studied in this thesis are video cameras, also known asElectro Optical sensors (EO-sensors). Using image processing the direction tothe target, also known as bearing, can be calculated. The direction is usuallyrepresented with the two angles, azimuth and elevation, between the target andthe UAV depictured in Figure 1.2 and Figure 1.3. These angles can be usedto estimate the position and velocity of the target and to calculate the collisionprobability. If the collision probability is high, the UAV must maneuver to avoidcollision.

θ

Figure 1.2. Elevation θ is the angle between the horizontal plane and the target.

ϕ

Figure 1.3. Azimuth ϕ is the angle in the horizontal plane between the axis pointingthrough the nose of the platform and the target.

The aim of this thesis work is to develop a target tracking system based onangle-only measurements. Assuming that a target has been detected the trackingsystem is provided with measurements of the angles between the target and theUAV. The tracking system estimates the position and velocity of the target. Thisinformation is sent to a collision avoidance system, which calculates the collisionprobability and, if necessary, initiates an evasive maneuver.

1.2 Existing Work 3

1.2 Existing Work

The problem of determining position and velocity of a target, with the use of mea-surements of azimuth and elevation is called angle-only target tracking, angle-onlytarget motion analysis or as passive ranging. The name passive ranging refersto the fact that the sensors are passive sensor as opposite to active sensors. Anactive sensor transmits for instance light or sound and measures the time it takesfor the reflection to return. A passive sensor on the other hand measures signalstransmitted from the target without transmitting anything self. A passive sensorcan measure the direction to the target, but not the range.

The problem of angle-only target tracking is well studied. In military applica-tions the use of passive sensors has the advantage of not revealing the position ofthe own platform, which has been important for instance in submarines. For UAVsthe motivation of using passive sensor is that they are cheaper than active sensors,such as radar. Another reason for the why the problem is studied is because it ischallenging.

"In the bearing-only problem, most of the estimation difficulties

that could be expected to arise do so" 1 [17]

The basics of target tracking is covered in [3]. [4] covers most asspects of trackingand devotes one chapter for the angle-only target tracking problem. [26] presentsand compares different tracking methods, with one chapter about angle-only targettracking applications. [1] derives a tracking filter that is well suited for angle-onlytarget tracking.

The absence of range measurements implies that it is troublesome to estimaterange. Consider Figure 1.4. From the angle measurements, lines can be drawn

sensor

y3

y2

y1

tr1

tr2

Figure 1.4. Two possible trajectories, tr1 and tr2 corresponding to the same anglemeasurement. How can range be estimated with use of angle-only measurements?

1According to [17] this citat is presented in [19].

4 Introduction

representing possible targets position. In the figure three angle measurements hasbeen received and three lines are drawn, y1, y2 and y3. Two possible target trajec-tories, tr1 and tr2 are also depicted. The target can either be close to the sensorand moving slow or be far away and move fast. How can the range to the targetbe estimate? If both the target and the UAV are traveling with constant velocity,the angle measurements do not provide any information of the range, and hencerange can not be estimated. Range is said to be unobservable. To gain observabil-ity in range the UAV has to maneuver. [17] investigates the problem of findingmaneuvers that will result in observability in range.

When the target travels with constant velocity, the tracking system can es-timate range if the UAV maneuvers. Assume that the range is known at someinstance of time. If the target travels with constant velocity it is easy to constructa model of the target’s dynamics. The target tracking system can use the model topredict the future range. But what happens if the target maneuvers? In Figure 1.5the system receives the same measurements as in Figure 1.4, but now the problemis how to distinguish between maneuvering targets and non-maneuvering targets.In a target tracking systems with full observability an inaccurate models of the

sensor

y3

y2

y1

tr1

tr1

Figure 1.5. Two possible trajectories, tr1 and tr2 corresponding to the same anglemeasurement. How can a target maneuver be detected and tracked with the use ofangle-only measurement?

target’s dynamics can be compensated for with accurate measurements. In thecase of unobservable range it is essential to have accurate dynamic models, sincethe measurements do not provide the system with information of the range. Thesystem must rely on the model. On the other hand it is hard to design a modelthat describes the dynamics of both maneuvering and non-maneuvering targets.Instead it is common to use multiple models, where each model describes one pos-sible target maneuver (including no maneuver). The multiple model approach isdescribed in for instance [3]. [25] presents algorithms for tracking maneuveringtargets using angle-only measurements.

1.3 Problem Formulation 5

The tracking system is meant to supply a collision avoidance system withinformation of the target. The fastness of the filter is essential of two reasons.First of all, the target can not be assumed to stay in the field of vision for a longperiod of time. The tracking system must be able to suppress initial errors inthe estimates before the target disappears from the field of vision, if the trackingsystem should be considered as good. Secondly, and probably most important,the collision avoidance system must be able to estimate the collision probability insuch a good time that an evasive maneuver can be performed if necessary. If thecollision is detected too late, the UAV will not be able to maneuver to avoid thecollision. [23] describes how the collision probability can be calculated and howthe UAV can maneuver to avoid to collide.

target

target

target

UAV

Figure 1.6. The UAV have detected three targets. When must the position estimatesbe good enough for the UAV to detect and avoid a collision?

1.3 Problem Formulation

In this thesis two tracking filters will be developed. The first filter is an ExtendedKalman Filter using Modified Spherical Coordinates, denoted as a MSC-EKF. Itis designed for tracking non-maneuvering targets. With this filter different plat-form maneuvers are evaluated. In literature it is common to find optimal platformmaneuvers. Since it is unrealistic to believe that a UAV have the maneuverabilityrequired for such maneuvers, the approach in this thesis is to evaluate platformmaneuvers that a UAV can perform. The filter is also tested for different filterinitializations and simulation geometries.

The second filter is an Interacting Multiple Model (IMM) filter that combinesthree MSC-EKFs assuming different models of the target’s dynamics. This filteris designed for tracking maneuvering targets. Instead of finding a model thatdescribes all possible target maneuvers, the maneuver is assumed to be describedas a combination of the models used in the filter. The filter is tested for differenttarget maneuvers and simulation geometries.

6 Introduction

1.4 Limitations

A target tracking system must deal with the problem of detecting a target andcancelling the tracking when the target travels out of sight. These two problemswill not be considered in this thesis work. It will be assumed that another systemstarts the filter when a target is detected and stops it when the target has disap-peared from the UAV’s field of vision. The image processing problem will not beaddressed. Instead measurements in form of elevation and azimuth will consideredas given from another system. The filter will not deal with the problem of multipletargets.

The target will be assumed to move in three dimensions, but target maneuverswill only be considered in the horizontal plane. This limitation may seem strange,but is due to the fact that the UAV’s field of vision is greater in the horizontalplane, than in the vertical plane.

1.5 Outline of the Thesis

This section outlines the thesis.

Chapter 2 presents the basis of angle-only target tracking. The observabilityproblem is discussed. Thereafter target models and sensor models are presentedtogheter with the coordinate systems that are used in the thesis. Different plat-form maneuvers are discussed and the S-turn and SS-turn that will be used in thesimulations are presented. Chapter 3 describes the theory behind the trackingfilters. Two of the filters will be implemented and tested in the thesis. The MSC-EKF is designed for tracking non-maneuvering targets and the IMM filter that isdesigned for maneuvering targets.

Chapter 4 discusses how the filter performance should be evaluated. Readerswho are not interested in the discussion can study the table in the end of the chap-ter, where several measures that will be used in the simulations are presented. InChapter 5 the implementation of the two filters are described and the simulationscenarios are presented.

Chapter 6 presents the simulation results from the MSC-EKF and in Chap-

ter 7 the simulation results from the IMM filter are included. Chapter 8 sum-marize the thesis and presents conclusions from the simulations. Ideas of futurework are also presented.

Appendix A, B and C present theory that have been omitted in the chap-ters. They are meant to be a help for readers who want to implement there ownfilters. Appendix D presents the scenarios used in the simulations. In Appendix

E and F simulation results that have been omitted from the simulation chaptershave been included.

Chapter 2


The target tracking problem can be described as using measurement data to es-timate the state of a target. All foreign vehicles are considered as targets. Thestate consists primary of position and velocity of a target. It is possible to includeother quantities in the state as well, such as angular velocity or class of target.

Target tracking can be performed by using sensors fixed on the ground or as inthis case with sensors placed on a platform. The platform is an Unmanned AerialVehicle (UAV) equipped with video cameras, also known as Elecrto Optical sensors(EO-sensors). From image processing measurements of the two angles elevation θand azimuth ϕ to the target can be obtained. Elevation is the angle between thehorizontal plane and the target and azimuth is the angle in the horizontal planemeasured from the axis pointing through the nose of the platform. Elevation isdepicted in Figure 2.1 and azimuth in Figure 2.2. The field of vision for the UAVis limited to −110 < ϕ < 110 and −15 < θ < 15.

θ

Figure 2.1. Elevation θ is the angle between the horizontal plane and the target.

In the case when both target and platform travel with constant velocity rangeis unobservable. This means that the measurements do not provide any informa-tion of the range between the target and the platform. To gain observability inrange the platform has to maneuver.

7

8 Angle-Only Target Tracking

ϕ

Figure 2.2. Azimuth ϕ is the angle in the horizontal plane between the axis pointingthrough the nose of the platform and the target.

This chapter starts with a discussion of the observability problem. Thereaftermodels of the target’s dynamics and the sensor are presented. Two different coor-dinate systems, Cartesian coordinates and Modified Spherical Coordinates (MSC),are described. Finally different platform maneuvers are discussed.

2.1 Observability

In [3] it is shown mathematically that in the case of both target and platform trav-eling with constant velocity range is unobservable. This can also be understoodby considering Figure 2.3 where three different angular measurement has beendepicted. From the angle measurement three lines y1, y2 and y3 can be drawn

sensor

y3

y2

y1

tr1

tr2

Figure 2.3. Two possible trajectories, tr1 and tr2 corresponding to the same anglemeasurement. When both the target and the platform travel with constant velocity thetrajectories are indistinguishable, meaning that range is unobservable.

representing possible target positions. Two possible target trajectories, tr1 andtr2 are also depicted. With the use of angle measurements only, these trajectoriesare indistinguishable. The target can either be close to the sensor and movingslow or be far away and move fast. From the measurements there is no possibility

2.2 Dynamic Target Models 9

to estimate range, meaning that range is unobservable.

The problem of unobservable range can be treated in several ways. If possiblethe platform can be equipped with additional sensors for instance radar whichmeasures range. If multiple platforms are available they can cooperate and theirmeasurements can be fused together according to a sensor fusion algorithm. An-other approach, which will be used in this thesis, is to let the platform maneuver togain observability. In Section 2.5 the problem of choosing an appropriate platformmaneuver will be addressed.

As will be seen in Section 2.4.2 the negative quotient between the range andrange velocity, also known as the Time To Go (ttg), is observable under the con-dition of constant velocity and non-zero angular velocity.

ttg = −r

r

ttg can be interpreted as time to collision in the case when the target and UAVis traveling towards each other, where positive ttg means that the target is ap-proaching and negative ttg that the range is increasing. A third possibility wouldbe to ignore the fact of unobservable range and focus on estimating the observableangles and ttg.

2.2 Dynamic Target Models

Since the target is moving it is not enough to only estimate the position of thetarget at one instance of time. It would be desirable to have a model that canpredict future positions if the present position is known. Models describing thedynamics of targets are by nature continuous and are often described by differentialequations. Consider a dynamic model written in the form

x(t) = fc (x(t), u(t), w(t))

where x(t) is the state vector containing the state of the system, for instanceposition and velocity, at time t. The vector notation used in this thesis is thatgeometric vectors are written in boldface, for instance vvv denotes the velocity vec-tor with magnitude v. Vectors that are not geometric, but neat ways to writesome quantities toghter, such as the state-, noise- and measurement vectors arenot boldfaced. u(t) is a known input vector to the system containing for instanceinformation of the tracking platform’s maneuver or the present weather condition,such as wind. The function fc describes how the state vector will propagate intime. Since the model is not a perfect description of reality, process noise willarise which is described as a stochastic process w(t). Readers not familiar withthe description of noise as stochastic process are encouraged to study [3] or [15]for references. The theory behind stochastic processes can be found in [7].

To implement a model in computers the model need to be discretized. Howthis is done is described in for instance [12]. Let tk = t0, t1, t2, ... describe the time


steps in which the model is discretized. In the case of equidistant time steps tkcan be written as tk = kT where T is the sample period. Let xk = x(tk) denotethe state vector in time tk. The dynamic model can be expressed in discrete timeas

xk+1 = fd (xk, uk, wk)

The subscripts c and d indicates that the function fc in the continuous modelis not identical with fd in the discrete model. In the case of a linear model thedynamic equation can be expressed as

xk+1 = Axk + Buuk + Bwwk

where A, Bu and Bw are matrices which may be time-dependent.

Since the target type is unknown beforehand it is troublesome to use sophis-ticated dynamic models. The target can be anything from a fighter aircraft to askydiver and the equations of motion for such a target varies. One might wantto only consider aircrafts but even for this restriction the problem will arise todecide the mass and momentum of inertia of the target. One possible solutionwould be to use some kind of classification system that identify the type of targetand choose an appropriate model. This requires advanced image processing sincethe target must be classified when it only covers a few pixels in the video camera.The approach used in this thesis is to use general models that are applicable forall targets.

2.2.1 Constant Velocity Model (CV)

A simple dynamic model is to assume that both the target and the observingplatform are traveling with constant velocity, i.e. straight-line trajectories. Tosimplify the model even more the platform’s velocity vector is assumed to point inthe same direction as the nose of the platform. Let ppp be the position vector and vvvthe velocity vector. The acceleration of the target is considered to be white noisew. The continuous model can be summerized as

ppp = vvv

vvv = w

This model is called the constant velocity (CV) model and is described in [4].The state vector includes position and velocity.

2.2.2 Constant Acceleration Model (CA)

The constant acceleration model (CA) is similar to the constant velocity modeland is also described in [4]. Instead of assuming the acceleration to be zero, theacceleration is considered to be constant, hence the time derivate of acceleration

2.3 Sensor Model 11

is considered to be white noise. The continuous model is

ppp = vvv

vvv = aaa

aaa = w

The state vector is extended to also include position, velocity and acceleration.

2.2.3 Coordinated Turn Models

The assumption of straight-line trajectories used in the CV-model will not holdwhen the target or the platform maneuver. In the coordinated turn (CT) model,presented in for instance [4], the aircraft is assumed to perform a turn with con-stant angular velocity ω. The change of the acceleration vector aaa is given by theexpression

aaa = −ω2vvv + w

where vvv is the velocity vector and w is a white noise vector representing the processnoise. The angular velocity is given by

ω =|vvv × aaa|

v2

When the platform maneuvers the angular velocity ω will be known for allinstances of time. This will simplify the implementation of the CT-model and themodel will degenerate to the CV-model with an additive known input signal. InAppendix B the equations for the CT-model with known ω are derived.

When the target maneuvers the same model can be used if target’s angularvelocity is known beforehand. If the angular velocity is unknown, the state vectorcan be extended to include ω. In [4] this is referred to as the horizontal turn

model with velocity states and in [21] as the nearly coordinated turn model. Thecoordinated turn is assumed to be performed in a fixed plan, i.e. the angularvelocity is considered to be a scalar. [27] derive the 3D coordinated turn called 3D

constant-turn, where the angular velocity is considered to be a vector with threeunknown components ωx, ωy and ωz. [4] also presents the nearly constant speed

horizontal turn model where both speed and angular velocity are used as states inthe state vector.

2.3 Sensor Model

The video camera is able to measure the two angles elevation θ and azimuth ϕ tothe target with great accuracy. When the target is close to the camera it is alsopossible to get measurements of the time-to-go, ttg, described above in Section 2.1.This requires that the target is big enough to cover more than only one pixel inthe camera, which means that good measurements of ttg will not be available untilthe end of the tracking. Since ttg is observable the filter will probably have better


estimates of ttg when the measurements are available and therefore this possibilitywill not be considered in this thesis.

The sensor can only perform measurements in discrete time, which means thatthe sensor model is time discrete. The relation between the measurement and thestate vector is given by

yk = h (xk, ek)

where yk is the measurement vector including ϕ and θ. ek is a stochastic processdescribing the measurement noise caused by errors in the sensor. A common modelfor measurements is to assume an additive white Gaussian noise model which willsimplify the implementation of the target tracking filter [21]. If the measurementrelation is linear this can be written as

yk = Cxk + ek

where C is a matrix. Since the measurements delivered by the video camera arevery accurate, the noise is assumed to be small in this thesis.

2.4 Coordinate Systems

The state vector consists of the relative position and relative velocity between thetarget and the observing platform. Using a coordinate system fixed in the observ-ing platform gives the advantage that the tracking system does not depend on theabsolute position of the platform, i.e. the tracking system is independent of theplatforms navigation system. To obtain the absolute position of the target, theposition and orientation of the platform must be known.

2.4.1 Cartesian Coordinates

A common choice of coordinate system is to use Cartesian coordinates 1 illustratedin Figure 2.4. The ξ-axis is pointing through the nose of the observing platform,i.e. the UAV here illustrated as an airplane. The η-axis points through the leftwing and the ζ-axis points through the back of the airplane. The η-axis pointsthrough the left wing and the ζ-axis points through the back of the airplane. Thestate vector in Cartesian coordinates is denoted xcar and is given by

xcar =(

ξ η ζ ξ η ζ)T

=(

x1 x2 x3 x4 x5 x6

)T

1Cartesian coordinates are commonly denoted by x, y and z. To avoid notation confusionbetween the Cartesian coordinates and the state- and measurement vector the Greek letters ξ, η

and ζ are used in this thesis.

2.4 Coordinate Systems 13

ξ

ηζ

Figure 2.4. Cartesian coordinates. The ξ-axis is pointing through the nose of theairplane. The η-axis points through the left wing and the ζ-axis points through the backof the airplane.

This choice of coordinates is appealing in order to describe the dynamics of thetarget. Using the CV-model from Section 2.2 the equations of motions becomes

xcark+1 =

1 0 0 T 0 00 1 0 0 T 00 0 1 0 0 T0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1

︸︷︷︸

A

xcark +

T 2/2 0 00 T 2/2 00 0 T 2/2T 0 00 T 00 0 T

︸︷︷︸

Bw

wk

The process noise in ξ- η- and ζ-direction are assumed to be independent. Ascan be seen the model is linear.

Also the CT-model with known angular velocity is linear. In Appendix B theequation of motion is derived for coordinated turns in the ξη-plane performed bythe platform and/or the target.

Describing the measurement relation in Cartesian coordinates yields the fol-lowing expression.

y =

(ϕθ

)

=

arctan(x2

x1)

arctan

(

−x3√x2

1+x2

2

)

+ e

Here the extended definition of arctan(x) is used, that describes angles in all fourquadrants. The observables, ϕ and θ, are coupled with the unobservable range,when the state vector is expressed in Cartesian coordinates. The measurementsgive some information about the states, but not enough information to fully esti-mate all states. It has been shown that this might cause an unstable behavior intracking filters [1].


2.4.2 Modified Spherical Coordinates (MSC)

An alternative to the Cartesian coordinates is to use Modified Spherical Coordi-nates (MSC). The state vector in MSC is

zmsc =(

1r ϕ θ r

r Ω θ)T

=(

z1 z2 z3 z4 z5 z6

)T

Ω = ϕ cos θ

where r is the range, ϕ the azimuth angle and θ is the elevation angle according toFigure 2.5. For θ = ±90 the state vector in MSC is singular [13]. Since the fieldof vision for the tracking platform is limited to ±15 this is not likely to cause anyproblems. Note that the fourth state is the negative inverse of ttg, introduced inSection 2.1.

ξ

η

ζ

r

ϕ θ

target

Figure 2.5. Modified Spherical Coordinates (MSC). The coordinate system is assumedto be fixed in the platform. r denotes the range to the target. ϕ is the azimuth definedas the angle between the ξ-axis and the projection of the target position in the horizontalplane. θ denotes the elevation defined as the angle between the horizontal plane and thetarget.

In order to distinguish the state vector in Cartesian coordinates and MSC thenotation xcar is used for the state vector in Cartesian coordinates and zmsc is thestate vector expressed in MSC. The measurement equation in MSC is simple

y =

(ϕθ

)

=

(0 1 0 0 0 00 0 1 0 0 0

)

zmsc + e

2.5 Platform Maneuvers 15

The continuous equation of motion is

z1 = −z1z4

z2 =z5

cos z3

z3 = z6 (2.1)

z4 = z26 + z2

5 − z24 − z1(aRT

− aRP)

z5 = z5(−2z4 + z6 tan z3) − z1(aHT− aHP

)

z6 = −2z4z6 − z25 tan z3 + z1(aVT

− aVP)

where aRT, aHT

and aVTare target acceleration in radial, horizontal and vertical

directions and aRP, aHP

and aVPare the corresponding platform acceleration. See

for instance [21].

It can be seen in (2.1) that when neither the target nor the platform maneuver,the last five states are decoupled from the first. This means that theoretically allstates except the first (the inverse of range) are observable. In practice howevernonzero angular velocity is required to observe the fourth state, r

r according to [4].

The advantage of implementing a target tracking filter in MSC is that the lackof range estimate does not degrade the estimates of the observable states in thecase of no maneuver. If the target is not maneuvering an accurate range estimateis produced upon platform maneuvering. The disadvantage of using MSC is thatsolving and discretization of (2.1) will yield a highly nonlinear dynamic model.

2.5 Platform Maneuvers

To gain observability in range the platform can in some cases perform a maneuver.This section begins with a discussion about when platform maneuvers can be usedto gain observability and requirements of platform maneuvers. Thereafter themaneuvers used in this thesis are presented.

2.5.1 Platform Maneuver Requirements

A first requirement upon a platform maneuver is that observability is gained whenthe maneuver is performed. [18] examines in which cases a maneuver is necessaryand/or sufficient to obtain observability in passive target tracking. For the caseof a target traveling with constant speed it is necessary for the platform to ma-neuver with non-zero acceleration, but this is not sufficient. The platform musthave a non-zero velocity component perpendicular to the line of sight. Necessaryand sufficient conditions for observability is given in [11]. If the target maneuversthe platform has to perform a maneuver with a higher non-zero time derivate.For instance, a target maneuvering with constant acceleration the platform has tomaneuver with non-zero acceleration. In [17] platform maneuvers are statistically


analyzed in accordance to their affect of the target tracking. The problem of find-ing a optimal maneuver is discussed for instance in [22].

To choose an appropriate platform maneuver the platform’s maneuverabilityhas to be taken into consideration. It is also important that the mission of theplatform is not prevented and that the maneuver does not lead to a collision. Ifthe platform is supposed to travel along a given trajectory the maneuver shouldnot differ too much from this trajectory. Finally the field of vision for the platformis limited. If the maneuver causes the target to disappear from the field of visionthe tracking will worsen. Since the field is wider in azimuth it is preferable toperform the maneuver in the ξη-plane. In this thesis different platform maneuversare evaluated, but the problem of when a platform maneuver should be performedis not addressed.

2.5.2 Choice of Platform Maneuver

Recall the Coordinated Turn (CT) model from Section 2.2.3. A CT is a good choiceof platform maneuver since it has an infinite number of non-zero time derivates.This means that a platform performing a CT gains observability if the targettravels with constant velocity or constant acceleration. If the target on the otherhand performs a CT, it is not possible to gain observability in range. By combiningtwo coordinated turns of the same length but with opposite signs of the angularvelocity ω a so called S-turn is performed, which can be seen in Figure 2.6.

After performing an S-turn the platform has the same course as before themaneuver but the position in η has changed. To achieve the same position in ηtwo S-turns can be combined into a SS-turn, which is shown in Figure 2.7.

The SS-turn might give better filter performance since the maneuver time isdoubled, but the maneuver might be confusing for the target and other platformsin the aerospace. If the target also is equipped with a collsion avoidance system,it is likely to tracking the platform as well. Both S-turns and SS-turns will beevaluated in this thesis.

2.5 Platform Maneuvers 17

ξ

η

CVCT

neg

CTpos

CV

Figure 2.6. The platform is performing an S-turn. An S-turn consists of a coordinatedturn with negative angular velocity (CTneg) followed by a coordinated turn with posi-tive angular velocity (CTpos). Before and after the maneuver the platform travels withconstant velocity (CV ).

0 ξ

η

Figure 2.7. The platform is performing an SS-turn. An SS-turn consists of two S-turns.

Chapter 3

Tracking Filters

In target tracking the aim is to estimate the target’s state vector from measure-ments. This chapter starts with single model filters. These filters are well suitedfor tracking non-maneuvering targets. For maneuvering target multiple model fil-ters are often used, which are presented in the end of the chapter. Readers who arenot familiar with the estimation filters are encouraged to study the block diagramsand the text, but skip the algorithms.

3.1 Single Model Filters

There are a number of filters that can be used for target tracking. In this sectionthe Kalman Filter (KF) will be presented, which is a commonly used filter. There-after the Extended Kalman Filter (EKF) which is suited for nonlinear systems willbe described. Finally an extended Kalman filter using modified spherical coordi-nates, here denoted as MSC-EKF, will be discussed. The MSC-EKF has beenimplemented and the simulation results can be found in Chapter 6. All thess fil-ters uses one model to describe the dynamics of the system, which means thatthey are suited for tracking non-maneuvering targets.

3.1.1 Kalman Filter

The system can be described by a dynamic model of the target and the relationsbetween the measurement and the state of the target. Combining a target modeland a sensor model from Chapter 2 the system can be described as

xk+1 = f (xk, uk, wk) (3.1)

yk = h (xk, ek)

where x is the state vector, u the input signal and y the measurement vector. wand e represent the process noise and measurement noise respectively.

19

20 Tracking Filters

For a linear model with additive noise the system can be rewritten as

xk+1 = Axk + Buuk + Bwwk (3.2)

yk = Cxk + ek

where A, Bu, Bw and C are matrices, possibly depending on time, but indepen-dent of the state vector x.

Assume that the noise realizations are Gaussian with expectation value zeroand independent with covariance matrices Cov(wk) = Qk and Cov(ek) = Rk. Un-der these assumptions it can be shown that the optimal solution to the problem ofestimating the state vector x based on the observations y is given by the Kalmanfilter.

Let xk|k be the estimate of the state vector for time tk given the measurementup to time tk, y0, y1 . . . , yk. In the same manner xk|k−1 denotes the estimate of

the state vector in time tk based on measurement up to time tk−1. Let P be theestimate of the prediction covariance matrix for the state vector. P representesthe uncertainty in the estimated state vector. Figure 3.1 shows a block diagramof the Kalman filter for one time step.

Update

Measurement

Update

Timexk−1|k−1 xk|k−1 xk|k

yk

Figure 3.1. Block diagram for the Kalman filter.

In the block called Time Update the model is used to predict the state vectorin the next time step and P is updated.When a new measurement is performedit is compared to the state vector in the block called Measurement Update. Theestimate of the state vector and the prediction covariance matrix is corrected. Thenew estimate is fed back to the block Time Update in the next time step. In Al-gorithm 1 the algorithm of the Kalman filter is given.

P does not depend on the measurements y meaning that it will not give anyinformation of how well the estimates fits the measurements. Instead P gives atheoretical value of how well the filter performs if the assumptions of Gaussiannoise are valid. The parameter K, known as the Kalman gain, decides how muchthe innovation εk = yk −Cxk|k−1 should be regarded. Large values in P meaning

3.1 Single Model Filters 21

Algorithm 1 Kalman Filter

1. Time Update

Predict the state vector and the prediction covariance matrix for the nexttime step

xk+1|k = Axk + uk

Pk+1|k = APk|kAT + Qk

2. Measurement Update

Correct the predicted state vector and prediction covariance matrix accord-ing to the measurement

εk = yk − Cxk|k−1

xk|k = xk|k−1 + Kεk

Pk|k = Pk|k−1 − KCPk|k−1

K = Pk|k−1CT S−1

S = CPk|k−1CT + Rk

the uncertainty in the estimate result in large values of K and the measurementwill have a great impact on the correction of the estimate. This will give a fastfilter that considers the measurements to be reliable. Small values in P and Kon the other hand results in a slower filter that is more robust against measure-ment noise. P depends on its initiatialization and of the values in the covariancematrices Q and R. The designer of the filter has to balance the robustness to mea-surement noise against the fastness of the filter by choosing appropriate values ofthe Q and R.

The Kalman filter is the optimal solution under the assumptions of Gaussiannoise. In real applications the assumption of Gaussian noise might not be valid.However assuming that the specified covariance matrices, Q, R and the initial-ization of P reflects the true second order moments the Kalman filter is the bestlinear filter [14]. A more detailed description of the Kalman Filter can be foundin for instance [14] ,[15] or [3].

3.1.2 Extended Kalman Filter

In the case of a non-linear model or measurement equation as in (3.1) an ExtendedKalman Filter (EKF) can be used. The idea is to linearize the system and apply aKalman filter. Unlike the Kalman filter EKF is not an optimal filter, but a based on

22 Tracking Filters

the optimal Kalman filter. It is therefore sometimes referred to as a suboptimalfilter. The implemention can be done in several ways, for instance discretized-linearization, which first linearize the non-linear continuous system and thereafterdiscretize the system. A derivation of the discretized-linearized EKF can be foundin [21]. The equations are presented in Algorithm 2. Compare Algorithm 2 forEKF with Algorithm 1 for the KF. The main difference is that the matrices A andC have been replaced with the Jacobians of the functions f and h in the updateof P .

Algorithm 2 Extended Kalman Filter

1. Time Update

Predict the state vector and the prediction covariance matrix for the nexttime step

xk+1|k = f(xk, uk)

Pk+1|k = Jf Pk|kJTf + Qk


Correct the predicted state vector and prediction covariance matrix accord-ing to the measurement

εk = yk − h(xk|k−1)

xk|k = xk|k−1 + Kεk

Pk|k = Pk|k−1 − KJhPk|k−1

K = Pk|k−1JTh S−1

S = JhPk|k−1JTh + Rk

Jf and Jh are the Jacobians of the functions f and h respectivly.

3.1.3 MSC-EKF

Recall the discussion from Section 2.4, describing the state vector in Cartesiancoordinates has the advantage of linear equations of motion. The disadvantage onthe other hand is the coupling between observable and unobservable states, whichlikely will degrade the performance of the tracking filter. Describing the statevector in MSC on the other hand solves the problem of decoupling the unobserv-able state, the inverse range, from the observable states to the cost of nonlinearequations of motion.

The MSC-EKF is a variant of the EKF developed to use the linear equations ofmotion in Cartesian coordinates without letting the coupling between observable

3.1 Single Model Filters 23

and unobservable states degrade the filter. It is presented in [1] for two dimensionsand in [2] [4] for three dimensions. Readers familiar with the EKF may realize thatthere is no difference between the MSC-EKF and an EKF filter that expresses thestate vector in MSC. The reason to present it as a separate filter in this thesis isthat this approach simplifies the implementation and is also easier to understandfor readers unfamiliar with the EKF.

The idea behind the MSC-EKF is to use Cartesian coordinates in the timeupdate but use MSC in the measurement update. In this way both the equationsof motion and the measurement equation become linear. Let xcar be the statevector in Cartesian coordinates and zmsc the state vector in MSC. Consider thesystem (3.3).

xcark+1 = Axcar

k + uk + wk

yk = Czmsck + ek (3.3)

xcar = fX(zmsc)

zmsc = fZ(xcar)

The first equation describes the dynamics of the system in Cartesian coordinatesand the second equation gives the relation between the measurements and the statevector in MSC. fX and fZ are functions that transform the state vector betweenCartesian coordinates and MSC. The explicit expressions for fX andfZ as well astheir Jacobians can be found in Appendix A.

Update

Measurement

Update

Timexk−1|k−1 zk−1|k−1 xk|k−1 zk|k−1zk|k

yk

MSC

MSC CAR

CAR

toto

Figure 3.2. Block diagram for an extended Kalman filter using modified sphericalcoordinates (MSC-EKF).

Figure 3.2 shows a block diagram over the filter. In the blocks Time update

and Measurement update the time update and measurement update are performedin the same way as in the Kalman filter. The tasks for the blocks MSC to Car isto transform the state vector from MSC to Cartesian coordinates while the blockCar to MSC do the opposite transformation. The algorithm for the MSC-EKFis given in Algorithm 3 where the superscripts car and msc has been suppressed.The prediction covariance matrix P and the measurement noise covariance matrixR are described in MSC and the process noise covariance matrix Q is expressedin Cartesian coordinates.

3.2 Multiple Model Filters 25

3.2 Multiple Model Filters

The filters presented in the previous section are based on a single model that de-scribes the system in all instances of time. For maneuvering targets it is difficult toconstruct models that describe all possible maneuvers. Since targets often travelapproximately along a straight line, it is important that the filter can handle thiscase, without getting unstable for maneuvering targets. The idea behind multiplemodel is to use multiple filters running parallel, where each filter uses a modeldescribing one possible maneuver. The outputs from the filters are then combinedto get an estimate.

Assume that the system in every time step obeys one of a finite number ofmodels and that the switching between models can be described as a Markovprocess. In a Markov process the probability for the system to switch to anothermodel (or stay in the same) at any instance of time depends only of which modelthe system obeyes.

“The future is independent of the past if the present is known” [3]

Markov processes are described in [7].

In [3] the optimal solution to this problem is presented. Since this solutionrequires an exponentially increasing number of filters two suboptimal techniquesare also presented, the Generalized Pseudo-Bayesian (GPB) filter and the Inter-acting Multiple Model (IMM) filter. IMM is conceptually similar to the secondorder of GPB, but requires fewer filters to run in parallel. This section presentsthe algorithm of the IMM filter. Thereafter the problem of choosing appropriatemodels is addressed. Readers who are not interested in the details are encouragedto study the block diagram to get a general idea of IMM and then read the sectionabout choosing models.

3.2.1 The IMM-Filter Algorithm

In this section the algorithm for the Interacting Multiple Model (IMM) will bepresented following [16] and [4], but alternative presentations can be found in [9],[21] and [26]. The idea behind IMM is to have multiple filters, here denoted assubfilters, running in parallel, one for every model mj = m1, m2 · · ·mN . Figure 3.3shows a block diagram of IMM. For simplicity only two subfilters are illustrated.In IMM four different probability variables are used to calculate which model thesystem obeys. They are summerized in Table 3.2.1. Let mj

k denotes the event thatthe system obeys model mj for time k. The probability that the system changesfrom model i to model j, pj|i = Prmj

k|mik−1, is a priori probability and is con-

sidered as a design parameter.

26 Tracking Filters

Table 3.1. The Probability variables that are used in the IMM algorithm

µjk Prmj

k Probability that the system is in model j at time step k.Based on measurements up to yk

Cjk Prmj

k+1 Probability that the system is in model j at time step k+1.Based on measurements up to yk

pj|i Prmjk+1|mi

k A priori probability that the system will make the transitionfrom model i to model j.

µi|jk Prmi

k|mjk+1 Conditional probability that the system was in model i at

time k given that it is in model j at time k + 1. Based onmeasurements up to yk

Filter1

Filter

Mixing

Merging

2

Probability

λ1 λ2

x1k−1|k−1 x2

k−1|k−1

x01k−1|k−1 x02

k−1|k−1

x1k|k

x2k|k

xk|k

µC

Figure 3.3. Block diagram for a IMM Filter.


In the blocks called Filter 1 and Filter 2 each subfilter produces a new esti-mate of the state vector xj

k|k according to the model the subfilter is using. The

likelihood of the model λjk is also calculated. λj

k is used in the block Probability

Update where the probability variables are updated. In the block Merging all xjk|k

are combined (merged). The estimates from the subfilters xjk|k are weighted by

their probabilities µjk to the total estimated state vector xk|k.

In the block called Mixing the model interacts to produce the inputs for Filter

j, called x0jk−1|k−1. Assume that the system is in model j at time k. Since it is

unknown in which model the system where at time k − 1 the estimates for theprevious time step are weighted together according to

x0jk−1|k−1 =

∑

i

µi|jk−1x

ik−1|k−1

µi|j is given by Bayes’ theorem

µi|jk−1 = Prmi

k−1|mjk =

Prmjk|mi

k−1Prmik−1

Prmjk

=pj|iµi

k−1

Cjk−1

The total IMM algorithm is given in Algorithm 4.


3.2.2 Choice of Subfilters

The IMM algorithm presented in the previous section describes how the subfiltersshould interact, but nothing has been said about the nature of these subfilters.This section addresses the problem of choosing appropriate subfilters.

The subfilter types used in IMM depends on which models that are used. Forlinear models Kalman filters can be used and for non-linear models EKF can beused, presented in the previous chapter. It is not necessary to use the same filtertype in all filter, for instance a KF with a CV-model can be combined with anEKF with a maneuvering model.

The models can differ in structure, state vectors or process noise. For modelswith different state vectors the mixing part of the filter has to be modified slightly.This is described in [16] for the general case and in the references below for themodels used there.

A simple approach is to use the same model in all filters and only vary theprocess noise. The subfilter with small process noise handles the non-maneuveringtargets. This subfilter will be robust to measurement noise. Maneuvering targetsare handled by subfilters with higher process noise. These models will be fast, buthave to rely on the measurement data to be accurate.

Recall Chapter 2 where different target models where presented. One choiceof subfilters is to extend the state vector for the maneuvering filters, i.e combinea constant velocity model with a constant acceleration model as in [25] or a meanjerk model described in [6] where the acceleration is assumed to be time correlated.This gives simple and general models and a straight forward implementation. [5]combines a CV-model and a CA-model where the CV-model is implemented usinga MSC-EKF.

More complex filter uses a CV-model to track non-maneuvering targets, one ormore turning models for maneuvering targets and possible one model that reflectsthe transition between maneuver and straight line. In Section 2.2.3 the coordi-nated turn was presented where the target was assumed to turn with a constantand known angular velocity. Two models with unknown angular velocity were alsopresented. Since the angular velocity of the target ωt is usually unknown, firstconsider the case where ωt is consider to be a state in the state vector. This isstudied in [3] for targets maneuvering in the horizontal plane and in [27] for tar-gets maneuverin in 3D. The models are highly non-linear meaning that suboptimalfilters must be used, for instance EKF. Consider a target traveling according to astraight line for some time and then performs a coordinated turn. During the non-maneuvering period the maneuvering filters are estimating the angular velocity,which will be small. When the target starts to maneuver the maneuvering filtershave large errors in their estimates of the angular velocity. If the maneuveringtime is small a slow filter might not estimate the correct angular velocity until the

30 Tracking Filters

end of the maneuver. In [3] this problem is solved by feeding the filter with anassumed angular velocity during the non-maneuvering part.

Now consider the coordinated turn with known angular velocity. The advantageof using this model is that it is linear in Cartesian coordinates. A very commonapproach for IMM is to find models that reflects the system for every instanceof time, i.e. the system is assumed to be described of only one of the models.According to this approach ωt must be known or at least there must be a goodguess of ωt available to use the CT-model. An alternative approach is to considerthe system to be in a mixture of the models. For example a turn with smallangular velocity can be seen as a mixture of a turn with large angular velocityand a straight line. Since the models interacts in the mixing part of the IMM andthe estimate is merged from the estimates from the subfilters IMM is well suitedfor this approach. By choosing a large value of the angular velocity used in themodel, ωmax all turns with smaller ωt can be seen as combinations of a CT-modelwith ωmax and a CV-model. In [10] an IMM with a CV-model and a CT-modelwith unknown angular velocity is compared to an IMM with a CV-model and twoCT-models with known angular velocities (positive and negative). The CT-modelwith unknown angular velocity has higher complexity than the CT-models withknown angular velocity, but the gain is that fewer filters are needed.

Chapter 4

Filter PerformanceEvaluation

This chapter discuss how the filter performance can be evaluated. The first sectiondiscuss which simulations that should be performed and the second section discusshow these simulations can be evaluated. Readers who are not interested in thesediscussion are recommended to only study Table 4.2 where the parameters thatare used in the rest of the thesis are summarized.

4.1 Evaluation Simulations

The performance of a tracking filter depends on a number of things, for instancethe trajectories of the target and the platform, the initialization of the filter andprocess- and measurement noise. In angle-only target tarcking the accuracy of theestimates highly depends on the geometry of the scenario studied, according to[17]. To test the filter for all possible trajectories and filter initializations requiresa huge number of simulations. Even if it was possible to simulate all scenarios,the result would be too large to survey. The alternative, to test the filter foronly a few scenarios, raises the question of how to choose appropriate scenarios.One risk is that the filter shows to good performance in the simulations, but inpractical use the filter performance is worse than expected, since the simulationscenarios differs too much from reality. Also the opposite risk must be considered,that a filter design is rejected since it performs bad in the simulations, but thesimulation scenarios have low probability to occur in reality. If knowledge of com-mon scenarios is available these scenarios can be used in the simulations. An evenbetter approach is to test the filter for scenarios where the filter performance ismost critical. If the filter performs well in critical situations, a worse performancecan probably be accepted during other scenarios. On the other hand filters thatperforms bad in critical scenarios can be rejected without more tests.

31

32 Filter Performance Evaluation

When knowledge about common scenarios is not available Monte Carlo simu-lations is a common method to evaluate filters. The idea is to consider all inputsto the filter as stochastic variables with known distributions. During a number ofsimulations inputs generated from these distributions are used. When the num-ber of simulation is large the performance of the filter is assumed to reflect thetrue behavior of the filter. The filter can be evaluated by Monte Carlo Simula-tions over different scenarios and the mean estimation error can be studied overa large number of simulations. This could be used as a measure of how well thefilter performs in general. More about Monte Carlo simulations can be read in [24].

However this is not a solution to the problem discussed above. Without goodinformation about the distributions there is a risk that scenarios with low prob-ability are considered too much, which can both result in an overestimate or anunderestimate of the filter performance. For a collision avoidance system the per-formance of the filter in a scenario with high collision probability is more interestingthan scenarios with low collision probability. The collision probability for a givenscenario can be calculated, but to find all scenarios with high collision probabilityis hard.

Another drawback for Monte Carlo simulations is that the result might behard to survey and to draw correct conclusions from. Information about how oneparameter influence the performance of the filter is hard to find in Monte Carlosimulation where all parameters vary. This information is easier to obtain if theinteresting parameter is varied in a deterministic way and all other parameters areheld constant.

In the simulations presented in this thesis process- and measurement noise issimulated as Gaussian. To study the influence of a parameter it will be variedin a deterministic way and all other parameters are held constant. A number ofsimulations are performed and the mean value of the estimation error is studied.Thereafter Monte Carlo simulations are performed for four different combinationsof target initial position and velocity where the errors in the initialization of thefilter are simulated as Gaussian distributed. For the IMM filter the target maneu-ver is also simulated as Gaussian distributed. This will give an indication of thefilter performance in general.

4.2 Performance Analysis

When the filter has been simulated on a number of scenarios, the next questionis how to evaluate the performance. Measure that tells whether the filter per-formance is acceptable or not are needed. This section presents the performancemeasures that will be used in this thesis. They are summarized in Table 4.2.Thereafter the question of how they should be used is discussed.

4.2 Performance Analysis 33

A common measure of the estimation error is the Root Mean Square Error(RMSE). Assume that N simulations has been performed and let xj be the estimateof the state with true value xj from simulation j. RMSE is defined as

RMSE(t) =

√∑N

j=1 ||xj − xj ||2N

Note that RMSE is defined as a function of time. However the time-dependencewill not be expressed explicitly in this thesis, unless necessary.

Studying RMSE has the advantage that the estimation error for all simula-tions can be studied at the same time. The alternative, to study the estimationerror from every simulation separately, requires at lot of time and the result ishard to survey. On the other hand information of single simulations can not beobtained from RMSE. Is the estimation error in all simulations close to RMSEor are there a few simulations with large estimation errors and small estimationerrors in all other simulations? What can be said about the fluctuation in theestimation error in time for given simulation? These two questions can not be an-swered by studying RMSE and for applications where they are important otherevaluation measures must be used.

Sometimes a more compact measure is wanted. To get a measure of the es-timate error in general the time mean of RMSE, RMSEm can be calculated.If RMSE is calculated for all instances of time t1, t2 · · · tmax the time mean ofRMSE is

RMSEm =

∑tmax

j=1 RMSE(j)

tmax

In many applications it is not only important to get estimates of the statevector, but also to know how certain these estimates are. The filters discussed inChapter 3 estimate the prediction covariance matrix P . The diagonal elements ofP gives the estimated variances of the states. The mean over N simulations ofthe estimated standard deviation for component i in the state vector, x(i) can becalculated as

σi =

√∑N

j=1 P j(i, i)

N

If the estimates of the state vector are unbiased, σ should reflect RMSE if thefilter estimates the uncertainty well.

Another interesting issue is in which states RMSE should be studied. If allstates are studied the question is how to compare two RMSE plots if the first plotshows low RMSE for one state and high in another, but the case is the opposite inthe other plot. Which states are most important? In this thesis the measurementof the angle states ϕ and θ are assumed to be accurate. A pragmatic approachwould be to ignore these states. Consider the hypothetical case where RMSE


is small in all other states than these two. It would then be possible to use themeasurements of the angles instead of the estimated states. On the other handlarge RMSE in ϕ and θ indicates that there is something wrong in the implemen-tation of filter and it would be wise to study the implementation closer to findprogramming errors or numerical problems.

The estimations of the first and fourth state are interesting to study, sincethese states are not measured. On the other hand it is difficult to tell how largeRMSE that are acceptable in these states. First consider the first state 1

r . Theerror in the first state may seem small if the true range is large, but what is theestimation error in range? Instead of studying the first state, the relative error inrange, rrel can be studied.

rrel =r − r

r=

(1

z1− 1

z1

)

z1

Studying RMSE for rrel is easier to interpret, but not trivial. Consider Table 4.1where the estimation error in 1

r , r and rrel has been calculated for two differentvalues of true and estimated range. rrel is the same for both set of r and r, but

Table 4.1. The error in 1

r, r and rrel for two different sets of values for the true and

estimated range.

|1r − 1r | |r − r| |rrel|

r = 500m r = 550m 1.8 · 10−4 50 0.1r = 300m r = 330m 3 · 10−4 30 0.1

the error in range is largest for the first set and the error in the inverse range islargest for the second. One has to decide which one is best or if the they are bothequally good.

Also the fourth state rr can be difficult to interpret. Instead the negative inverse

of the state can be studied, also known as time-to-go (ttg).

ttg = −r

r= − 1

z4

If both the target and the platform travel with constant velocity and are on colli-sion course, ttg is the time remaining to collision. Even when the vehicles are noton collision course, positive ttg means that the target is approaching the platformand negative ttg that the target is traveling away from the platform. The magni-tude of ttg indicates how far away the target is in time. Consider a target thatfirst approaches the platform and thereafter travels away from it. At the ClosestPoint of Approach (CPA) r = 0 meaning that ttg → ∞ and changes sign. Even ifthe estimation error in the fourth state is small in CPA, the estimation error of ttgcan be huge. If the target and platform maneuver there might be several CPAs in

4.2 Performance Analysis 35

a simulation and it might be better to study the fourth state.

In the IMM filter the parameter µj describes the probability that the target isdescribed by model j.

µj = Prmj

For understanding the behavior of the filter this parameter is interesting to study.A filter that estimates the model probability well is more likely to estimate thestate vector well, since it uses the right model.

The parameters that has been discussed above are summarized in Table 4.2.They will be used in Chapter 6 and 7 where the simulation results are presented.The next question is how to use the parameters presented above. It is desirablethat RMSE is small, that σ ≈ RMSE and that µj reflects which model the targetobeys. Which of these are most important? It has already been stated that it ismore important to get small RMSE in range and ttg than in ϕ and θ. It has alsobeen discussed that it probably is more important to have small estimation errorsin scenarios when the collision probability is high. Another issue is time. When isit important to have good estimates? Since it is only possible to track targets thatare in the field of vision of the platform, the tracking time is likely to be small, inthe order of 20 seconds or less. To avoid a collision it is important to estimate thecollision probability, before it is too late for the platform to maneuver, if necessary.In some cases a worse final estimate error is to prefer from a case with good finalestimate error, but large convergence time. How small estimation errors and shortconvergences times that are needed depends on the collision avoidance system.

Table 4.2. Performance parameters used to evaluate the filter performance.

Parameter Expression Description

RMSE(t)

√∑Nj=1

(xj−xj)2

N Root mean square error (time-dependent)

RMSEm

∑ tmaxj=1

RMSE(j)

tmaxTime mean of RMSE

σi

√∑Nj=1

P j(i,i)

N Estimated standard deviation of state i

rrelr−r

r =(

1z1

− 1z1

)

z1 Relative error in range

ttg − rr = − 1

z4

Time to go

µj Prmj Probability that the system is in model j

Chapter 5

Implementation

Two filters have been implemented in Matlab, an MSC-EKF for tracking non-maneuvering targets and an IMM-filter for tracking maneuvering targets. To eval-uate the performance of the filters, they have been tested on simulated data. Thischapter discusses the implementation of the MSC-EKF and the IMM-filter. There-after the simulation scenarios are presented. In Chapter 6 simulation results forthe MSC-EKF are presented and Chapter 7 presents the simulation results for theIMM filter.

5.1 Implementation of the MSC-EKF

The implementation of the MSC-EKF is straightforward from Algorithm 3 inChapter 3. The state vector has to be initialized with a guess of the initial state andthe prediction covariance matrix should be initialized according to the uncertaintyof this guess. The covariance matrices for process noise Q and measurement noiseR must be set and the update of the prediction covariance matrix P is correctedto prevent numerical errors in the filter.

5.1.1 Initialization of the State Vector

Let vvvuav be the velocity vector of the UAV and vvvtar be the velocity vector ofthe target, see Figure 5.1. Let the speed, i.e. the magnitude of the velocity, bedenoted by vUAV and vtar respectively. The UAV is initially considered to fly withconstant velocity, vvvuav in the ξ-direction, i.e. the velocity vector points throughthe nose of the UAV. The target is assumed to be detected at the distance rinit

from the UAV and flying with speed vtar.

The state vector is initialized with a guess of the initial position and velocity ofthe target in MSC. To initialize the angles ϕ and θ the first measurements ϕmeas

and θmeas are used. The angular velocity is assumed to be zero. To study theimpact of initial error in range, the range is initialized with rinit + ∆r where rinit

is the true initial range and ∆r is the error in initial range. In the same way the

37

38 Implementation

UAVtarget

vvvuavvvvtar

rinit

Figure 5.1. The assumed initial position and velocity vectors for the target and theUAV.

speed of the target is initialized with vtar + ∆v. In Table 5.1 a summary of theparameters can be found.

Recall the state vector in MSC

zmsc =(

1r ϕ θ r

r Ω θ)T

Altogether this gives the initial estimate of the state vector

zmsc0|−1 =

(1

rinit+∆r ϕmeas θmeas − vUAV +vinit+∆vrinit+∆r 0 0

)T

In this initialization vvvtar is assumed to point towards the UAV, i.e. the angu-lar velocity is zero. Another possibility is to use two measurement of elevation, θ0

and θ1, and initialize θ with the difference divided with the sample period. ForΩ measurement of both ϕ and θ can be used in a corresponding way. This wouldgive better initializations of the fifth and sixth state, particularly if the angularvelocity is large. By using more measurement the initialization could be evenmore accurate. For large angular velocity it may be necessary to also correct theinitialization of the fourth state, otherwise the assumed speed of the target will betoo large.

In this thesis the initialization with the angular velocity equals zero is used.This initialization is faster since the filter does not have to wait for more thanone measurement. It is also appealing because it assumes the worst case scenario.To use the tracking filter in a collision avoidance system it is important to getaccurate estimates when the probability for collision is high and this is the casewhen the angular velocity is small.

5.1.2 Initialization of the Prediction Covariance Matrix P

The prediction covariance matrix P is also initialized in MSC. The error in theinitialization of the state vector depends on the true position and velocity of thetarget. Two different initializations of P will be tried, here denoted as P 1

0|−1 and

P 20|−1. In both cases P is initialized as a diagonal matrix with the variances in its

main diagonal and zeros everywhere else. This means that the errors in the statesare assumed to be independent. The first initialization is based on calculations

5.1 Implementation of the MSC-EKF 39

approximating the variance of the initial state vector with the propagation of un-certainty formula, described in [7]. The calculations can be found in Appendix C.In Section 5.3 Scenario A is presented, which is the scenario that will be used inmost simulation in the next two chapters. For this scenario P is initialized as

P 10|−1 = diag( 1.1 · 10−9 10−8 10−8 2.2 · 10−5 1.8 · 10−6 1.8 · 10−6 )

where the parameters in Table 5.1 have been used. The standard deviation of theangles σϕ1 and σθ1 are assumed to equal the measurement noise.

The second initialization is based on [20] who states that P can be initializedwith

P 20|−1 = diag

(

2rinit+σr

(4σϕ2)2 (4σθ2)

2 γ(

4√

2σϕ2

T

)2 (4√

2σθ2

T

)2)

γ = 4(z24 z

21

(rinit + σr

2

)2

+ 300z21)

In this initialization the standard deviation of the angles, σϕ2 and σθ2, areassumed to be larger than in the first initialization. With the numerical values inTable 5.1 the initialization becomes

P 20|−1 = diag( 2.5 · 10−7 0.12 0.12 3.6 · 10−3 4.9 4.9 )

P 20|−1 is significantly larger than P 1

0|−1 in all states.

5.1.3 Covariances Matrices for the Process Noise, Q, andthe Measurement Noise, R

The covariance matrices for the process noise Q and the measurement noise R areassumed to be constant over time. Q is expressed in Cartesian coordinates and isset to

Q = σ2q

T 3

3 0 0 T 2

2 0 0

0 T 3

3 0 0 T 2

2 0

0 0 T 3

3 0 0 T 2

2T 2

2 0 0 T 0 0

0 T 2

2 0 0 T 0

0 0 T 2

2 0 0 T

where T is the sample period and σq = 0.1 · 10−3.

The measurement noise in ϕ and θ are assumed to be independent. R is adiagonal matrix with its components corresponding to ϕ and θ.

R =

(σ2

ϕ1 00 σ2

θ1

)

where σϕ1 and σθ1 is the standard deviation of the measurement noise.

40 Implementation

Table 5.1. Parameters needed to be set to initialize the filter.

Parameter Description ValuevUAV Speed of the UAV 50m/svtar Speed of the target 100m/srinit Detection distance of the target 3kmσϕ1 Standard deviation of measurement noise in azimuth 0.1mradσθ1 Standard deviation of measurement noise in eleva-

tion0.1mrad

σϕ2 Assumed standard deviation of measurement noisein azimuth in P 2

0|−1

5

σθ2 Assumed standard deviation of measurement noisein elevation in P 2

0|−1

5

σr Standard deviation in detection distance 300mσv Standard deviation in target speed 10m/sT Sample time 0.1sσq Standard deviation of the process noise 0.1mm/s2

5.1.4 Correction of P

The prediction covariance matrix P is symmetric. Due to numeric errors theestimates of P might be asymmetric. To prevent this, P is corrected after everyupdate with

P =1

2

(

P + PT)

5.2 Implementation of the IMM Filter

The IMM filter consists of a number of subfilters running in parallel. The imple-mentation follows Algorithm 4 presented in Chapter 3.2 where the filtering partis accomplished according to the algorithm of the subfilter. The subfilters are ini-tialized according to the previous section. This section discusses the models usedin the subfilters, the Markov transition matrix and the initialization of the modelprobability vector.

5.2.1 Models in the Subfilters

The IMM filter should track targets flying along straight lines and targets ma-neuvering in the horizontal plane. One possible choice of models is to combinea CV-model with a CT-model with unknown angular velocity, where the angularvelocity is a state in the state vector. These models were described in Section 2.2.This combination has the disadvantage of having different state vectors in theCV-model and the maneuvering model. The CT-model with unknown angularvelocity is non-linear meaning that some kind of linearization filter must be used,

5.2 Implementation of the IMM Filter 41

for instance EKF.

Recall the discussion from Chapter 3.2. A CT with angular velocity ωt canbe seen as a combination of a CT with a higher angular velocity ωmax and a CV.This motivates the use of a CV-model combined with CT-models with known an-gular velocity, ωmax, which will be used in this thesis. For target performing CTswith lower angular velocity, the IMM-filter estimates the target to be in both theCV-model and the CT-model with ωmax. The filter must be able to track targetturning both left and right and therefore two CT-filters are used with angularvelocity ±ωmax. Since the IMM-filter can not track targets with higher angularvelocity than ωmax, ωmax must be chosen large enough to track all interestingtargets. A too high value of ωmax on the other hand will likely degrade the fil-ter for small angular velocities. The advantage of using CT-models with knownangular velocities is that the same state vector can be used in the CT-models asin the CV-model. Another advantage is that the CT-model is linear in Cartesiancoordinates, meaning that the MSC-EKF can be used as subfilters.

Three MSC-EKFs are implemented according to the previous section with onefilter using the CV-model and two filters using the CT-model, as depicted inFigure 5.2. The angular velocity used in the filters is set to ωmax = 20/s.

Mixing

Merging

CT CTCVposneg

xk|k

Figure 5.2. The IMM filter implemented with three subfilters. Two of the subfilters areusing a CT-model with known angular velocity and the third subfilter uses a CV-model.

5.2.2 Markov Transition Matrix and Initial Model Proba-bilities

Let m1 stand for the CV-model and let m2 and m3 stand for the CT-models withpositive and negative angular velocity respectively. The Markov transition coeffi-cients pj|i = Prmj

k+1|mik is the priori probabilities for the system to make the

42 Implementation

transition from model mi to model mj . They are illustrated in Figure 5.3 andsummarized in (5.1).

pj|i = p(j, i) =

0.9 0.1 0.10.05 0.8 0.10.05 0.1 0.8

(5.1)

CTpos

CTneg

0.9

0.1

0.8

0.05

0.05

0.1

0.80.1

0.1

CV

Figure 5.3. The Markov transition coefficients.

The model probabilities µjk = Prmj

k is initialized with the priori probabilityof the system being in model mj . The initial model probabilities is summarizedin (5.2).

µj0 = µ0(j) =

0.90.050.05

(5.2)

5.3 Simulations

This section describes the simulations performed to evaluate the filters. First thesimulation scenarios are described. Thereafter the parameters that will be heldconstant during the simulations are presented.

5.3.1 Simulation Scenarios

In all simulations the target starts with flying in a straight line trajectory. In sim-ulations where the target maneuvers, the maneuver is performed after 8 seconds.To specify a scenario parameters describing the initial position and velocity forthe target are needed to be set. A summary over all parameters can be found inTable 5.2. The initial position of the target are set with azimuth ϕ0 and elevationθ0. The initial range is rinit = 3km for all simulations and the speed of the tar-get is vtar = 100m/s. The angles α and β describes the direction of vvvtar in thecoordinate system fix in the UAV. β corresponds to elevation and α correspondsto azimuth, but is rotated 180, as depicted in Figure 5.4. If the target starts in aposition right ahead of the UAV (ϕ0 = 0 and θ0 = 0) and α = β = 0 the target

5.3 Simulations 43

Table 5.2. Parameters needed to be set to define a scenario

Parameter Descriptionϕ0 Initial azimuth angleθ0 Initial elevation angleα Angle describing the direction of the velocity of the

target in azimuthβ Angle describing the direction of the velocity of the

target in elevation

are traveling towards the UAV. If neither the target nor the UAV maneuver, theycollide after 20 seconds.

UAVtarget

αβ

vvvtar

ξ

Figure 5.4. The angles α and β describing the direction of the target’s initial velocityvector vvvtar. α and β are measured from the ξ-axis.

Figure 5.5 shows the scenario that will be mostly used in the simulations in thisthesis, denoted Scenario A. In the scenario both target and UAV might maneuver.The target starts right ahead of the UAV and is flying with a velocity vectorpointing left and down seen from the target. The parameters can be found inTable 5.3. All scenarios used in this thesis can be found in Appendix D.

Table 5.3. Parameters defining Scenario A.

Parameter Valueϕ0 0

θ0 0

α 2

β 2

5.3.2 Simulation Parameters

This section describes the parameters that will be held constant during all simu-lations, unless other stated. All parameters can be found in Table 5.4.

44 Implementation

0 500 1000 1500 2000 2500 3000−80

−60

−40

−20

0

20

UAVTarget

0 500 1000 1500 2000 2500 3000−80

−60

−40

−20

0

20

ξ

ξ

ηζ

Scenario A

Figure 5.5. Trajectories for the target and UAV in Scenario A, θ0 = ϕ0 = 0, α = β =

2.

Table 5.4. Default simulation parameters.

Parameter Description DefaultT Sample Period 0.1sTend Simulation time 25sN Number of simulations 30qsim Process noise used in simulations (standard devia-

tion)0.1mm/s2

rsim Measurement noise used in simulations (standard de-viation)

0.1mrad

ωu Angular velocity for the UAV’s maneuver 4/sωt Angular velocity for the target’s maneuver 10/s∆r The difference between the true range and the esti-

mated initial range300m

∆v The difference between the true velocity of the targetand the estimated velocity

10m/s

ϕlim Field of vision in azimuth 110

θlim Field of vision in elevation 15

θbreak Break condition for θ 80

Tbreak Break condition for tracking without measurements 2s

5.3 Simulations 45

The sample time T is set to 0.1s, meaning that the sensors are measuring witha rate of 10 Hz. The simulations are performed during Tend = 25s. The input tothe filter is simulated as Gaussian noise. To get a general idea of the behavior ofthe filter N = 301 simulations are performed.

The trajectories for the target and the UAV are simulated with added Gaus-sian process noise with standard deviation qsim = 0.1mm/s2. The noise in theξ-, η- and ζ-directions is independent. To get measurement data the azimuth ϕand elevataion θ are calculated for all samples and Gaussian noise are added withstandard deviation rsim = 0.1mrad.

When the UAV maneuvers according to the CT model, it has the angular ve-locity ωu = 4/s. In the simulations where the target maneuvers it has the angularvelocity ωt = 10/s. In Monte Carlo simulation ωt is simulated as Gaussian noisewith mean 10/s and standard deviation 5/s.

To simulate initial errors in the state vector, initial errors are added to theinitial guess of range and velocity as described above. The error in range is∆r = 300m and the error in speed is ∆v = 10m/s. In Monte Carlo simula-tion ∆r and ∆v are simulated as Gaussian noise with zero mean and standarddeviation 300m and 10m/s respectively.

The field of vision for the UAV is limited with −110 < ϕ < 110 and −15 <θ < 15. When the target disappears outside the field of vision, i.e. |ϕ| > ϕlim

or |θ| > θlim, the filter uses only the prediction state to track the target. Twoconditions terminate the tracking.

• The target has not reappeared into the field of vision after Tbreak = 2s.

• θ exceeds θbreak = 80.

The second condition prevents divergence in the filter due to the fact that the MSCare singular for θ = ±90. Usually the tracking is terminated before θ > θbreak

from the first condition.

1In the Monte Carlo Simulations N = 200

46 Implementation

Chapter 6

Simulation Results -MSC-EKF

This chapter presents the results from the simulations performed to evaluate theMSC-EKF. The filter is designed for non-maneuvering targets and is mainly testedon scenarios where the target travels along a straight line. First the filter is testedon Scenario A to get an idea of its behavior, with two different initializations ofP . Thereafter different platform maneuvers are tested and their influence on thefilter performance evaluated. Two platform maneuvers are chosen and used in thefollowing simulations which evaluate the influence of errors in the initial state vec-tor and the influence of the direction of target’s velocity vector. The filter is alsotested for the case where the target performs a small maneuver. Finally MonteCarlo simulations are performed to evaluate the performance of the filter in general.

6.1 Choice of Initial Prediction Covariance Ma-trix P

Simulations were made for Scenario A, presented in Chapter 5. To gain observabil-ity in range the platform maneuvered after 5 seconds. Figure 6.1 shows RMSE forall six states (dark lines) together with the estimated standard deviation σ (brightline). P is initialized with P 1

0|−1 from Section 5.1.2. The dotted part of the plotsrepresents the time where the target has moved outside the field of vision and thefilter only uses its prediction state.

RMSE is close to σ in the beginning of the simulation for all states. For theangles RMSE is smaller than σ and for the other states RMSE is close to σ. Whenthe target travels out of sight both RMSE and σ increases fast.

Instead of studying all states it is interesting to study RMSE in ttg andrrel = r−r

r which is shown in Figure 6.2. RMSE in rrel remains around 0.1 during

47

48 Simulation Results - MSC-EKF

0 5 10 15 200

2

4

6

8x 10

−4

[m−

1 ]

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

[deg

]

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

[deg

]

0 5 10 15 200

0.02

0.04

0.06

0.08

0.1

[s−

1 ]

0 5 10 15 200

0.005

0.01

0.015

0.02

[deg

/s]

0 5 10 15 200

0.005

0.01

0.015

0.02

[deg

/s]

ϕ

θ

1r

Ω θ

rr

Time [s]Time [s]

Figure 6.1. RMSE (dark) and σ (bright) for the state vector. The dotted part representsthe time where the target is out of sight. Scenario A. P is initialized with P 1

0|−1 fromSection 5.1.2.

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

RM

SE

0 2 4 6 8 10 12 14 16 18 200

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

rrel = r−rr

ttg

Time [s]

Figure 6.2. RMSE for rrel and ttg. The dotted part represents the time where thetarget is out of sight. Scenario A. P is initialized with P 1

0|−1 from Section 5.1.2

6.1 Choice of Initial Prediction Covariance Matrix P 49

the whole simulation. Since the platform maneuvers, RMSE for rrel is expected todecrease after the platform maneuver, but this is not the case. With small valuesof σ the estimates of the states are considered to be good and the measurementshave small affect of the estimates. Since the filter does not get indications thatthe range estimates are bad, the filter does not try to correct the estimates andthe initial error remains. RMSE for ttg decreases in the end of the simulationsbut the convergence time is large. After 20 seconds the target reaches the closestpoint of approach, CPA, where ttg changes sign. Before CPA ttg is negative, butafter CPA is reached the target is traveling away from the UAV and ttg is positive.This means that it is hard to estimate ttg in CPA.

The alternative initialization of P , P 20|−1 from Section 5.1.2 was used on the

same scenario. Figure 6.3 and Figure 6.4 shows the result. RMSE is significantlysmaller for all states with this initialization of P , but σ is large compared to theestimates. When the measurements are not available RMSE stays small, meaningthat the estimates are good, but the estimated uncertainty grows.

0 5 10 15 200

2

4

6

8x 10

−4

[m−

1 ]

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

[deg

]

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

[deg

]

0 5 10 15 200

0.02

0.04

0.06

0.08

0.1

[s−

1 ]

0 5 10 15 200

0.005

0.01

0.015

0.02

[deg

/s]

0 5 10 15 200

0.005

0.01

0.015

0.02

[deg

/s]

ϕ

θ

1r

Ω θ

rr

Time [s]Time [s]

Figure 6.3. RMSE (dark) and standard deviation (bright) for the state vector. Thedotted part represents the time where the target is out of sight. P is initialized withP 2

0|−1 from Section 5.1.2.

RMSE in rrel decreases when the platform maneuvers and is below 0.03 after10 seconds. RMSE in ttg decreases after 6 seconds. The estimates do not worsenfrom the lack of measurement data (dotted line). RMSE in both rrel and ttg con-verge faster than in Figure 6.2 where P 1

0|−1 was used. Even though P 20|−1 gives bad


0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

RM

SE

0 2 4 6 8 10 12 14 16 18 200

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

rrel = r−rr

ttg

Time [s]

Figure 6.4. RMSE for rrel and ttg. The dotted part represents the time where thetarget is out of sight. P is initialized with P 2

0|−1 from Section 5.1.2.

estimates of σ it will be used during the rest of this chapter, since the estimatesof rrel and ttg are better. Only the time when the target is in the field of visionwill be studied further.

6.2 Choice of Platform Maneuver 51

6.2 Choice of Platform Maneuver

To gain observability in range the platform has to perform a maneuver. In thissection different platform maneuvers are tested.

6.2.1 S-turns and SS-turns

Recall Section 2.5 where the S-turn and SS-turn were presented. Figure 6.5 showsRMSE for rrel and ttg for scenario A when the platform performs S-turns and SS-turn of various lengths. All maneuvers are performed after 5 seconds. Performinga S-turn during only 1 second or a SS-turn during 2 seconds (dotted) does notimprove the estimation of the rrel compared with the no maneuvering case (black).The S-turn during 3 seconds improves the result and the S-turn during 5 secondsshows even better results. For the estimates of ttg the maneuvers slightly improvethe convergence time.

0 2 4 6 8 10 12 14 16 180

0.05

0.1

RM

SE

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

noneS−turn 1sS−turn 3sS−turn 5sSS−turn 2sSS−turn 6sSS−turn 10s

rrel = r−rr

ttg

Time [s]

Figure 6.5. Comparison of RMSE in rrel and ttg where S-turns (dark) and SS-turns(bright) are used as different platform maneuvers. Scenario A.

Since the S-turns (dark) performs as well as the SS-turns (bright) even thoughthe maneuvering time for the SS-turns is twice as long it is motivated to useS-turns.


6.2.2 Initial Time, ti, of the Platform Maneuver

The filter is started when a target is detected. The initial time, ti, is defined asthe time between the target is detected and the platform starts to maneuver. Fig-ure 6.6 shows RMSE in rrel and ttg when the platform performs a S-turn during4 seconds with different ti in Scenario A. The initial time of the maneuver, ti,varies. RMSE in rrel converges faster with smaller ti to the cost of a worse finalRMSE. In the case of no platform maneuver, RMSE for ttg decreases between 6to 10 seconds. If the platform maneuver in this time interval the estimate of ttgworsen.

0 2 4 6 8 10 12 14 16 180

0.05

0.1

RM

SE

noneti=0.2s

ti=1s

ti=5s

ti=11s

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

rrel = r−rr

ttg

Time [s]

Figure 6.6. Comparison of RMSE in rrel and ttg for different initial times, ti, for theplatform maneuver, ∆r = 300m, ∆v = 10m/s. Scenario A.

In a collision avoidance system the convergence time is often more importantthan the accuracy of the estimates, which motivates to perform a platform ma-neuver as soon as possible after detection of a target. Performing a maneuver toearly on the other hand might give inaccurate estimates.


6.2.3 Right or Left Platform Maneuver

An S-turn can be performed left or right. In Figure 6.7 the influence of right andleft S-turns are studied for different values of ti. Whether the right (dark) or left(bright) turn is to prefer seems to be depending on the initial time.

0 2 4 6 8 10 12 14 16 180

0.05

0.1

RM

SE

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

noneright t

i=1s

right ti=3s

right ti=5s

left ti=1s

left ti=3s

left ti=5s

rrel = r−rr

ttg

Time [s]

Figure 6.7. Comparison of RMSE in rrel and ttg when the platform performs left(bright) and right (dark) S-turns during 4 seconds. Scenario A.

To evaluate the difference between right and left turns more thoroughly themaneuvers were tested on Scenario B. The result is shown in Figure 6.8. In thisscenario the target starts to the left of the UAV (ϕ = 30) and travels towards theUAV (α = 5). Scenario B is depicted in Appendix D.

Also in this case it is clear that right turns and left turns give different per-formance. Right turns give a slower filter but the final RMSE is smaller than inthe case of left turns. For time t = 18s CPA is reached, meaning that ttg changesign at this moment. The filter gives bad estimates of ttg in this point, but theestimates converges fast afterward.


0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

RM

SE

0 2 4 6 8 10 12 14 16 18 200

0.5

1

1.5

2

RM

SE

noneright t

i=1s

right ti=3s

right ti=5s

left ti=1s

left ti=3s

left ti=5s

rrel = r−rr

ttg

Time [s]

Figure 6.8. Comparison of RMSE in rrel and ttg when the platform performs left(bright) and right (dark) S-turns during 4 seconds. Scenario B.


6.2.4 Conclusion on Choice of Platform Maneuver

The difference between using an S-turn or an SS-turn is not significant. The S-turn has the advantage of shorter maneuvering time, and will therefore be usedduring the rest of this thesis. To get fast filter convergence in range the platformshould maneuver as soon as possible after detecting a target. However the esti-mates might be too inaccurate if the maneuver is performed to early. The choiceof performing a left or a right turn might affect the performance of the filter. Tofind rules for when the platform should turn left or right the problem need to beinvestigated more. For the rest of this chapter the platform maneuver will alwaysbe a right S-turn during 4 seconds, with ti = 5s or ti = 1s.

It is interesting to study how well the filter performs for a scenario with highcollision probability. Figure 6.9 shows the performance of the filter with theseplatform maneuvers in Scenario C were the target is traveling towards the UAVleading to a collision. When the platform maneuver the collision is avoided butthe target and the UAV passes close to each other.

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

X: 19.5Y: 0.05227

RM

SE

0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0.6

0.8

1

RM

SE

nonetI = 1s

tI = 5s

rrel = r−rr

ttg

Time [s]

Figure 6.9. Comparison of RMSE in rrel and ttg when the platform performs a S-turnduring 4 seconds. Scenario C.

In this scenario the angular velocity is zero meaning that ttg is unobservable.When the platform has manuvered Ω 6= 0 and ttg become observable for the rest ofthe simulation. The error in rrel increases if the platform does not maneuver. Thefilter performs best when the initial time is large. At time t = 20s the target collide


with the UAV meaning that the estimates after that time is not of any interest.In the previous scenarios the performance of the filter might be considered assatisfying even in the case of a non-maneuvering platform. In this scenario on theother hand a maneuver is necessary, otherwise the estimates of range diverge andthe initial error in the estimate of ttg remains.

6.3 Influence of Ω and θ 57

6.3 Influence of Ω and θ

To study the influence of the direction of the target’s velocity vector vvvtar the initialposition of the target was chosen to ϕ0 = 0 and θ0 = 0 (as in Scenario A andScenario C). To specify the direction of vvvtar the angles α and β is defined as inFigure 6.10. For this scenario Ω = 0 if α = 0 and θ = 0 if β = 0.

UAVtarget

αβ

vvvtar

ξ

Figure 6.10. The angles α and β describing the direction of the target’s initial velocityvector vvvtar.

Figure 6.11 shows RMSE for rrel and ttg for different values of α when β = 0.In the case of platform maneuver (left) RMSE for rrel decreases from 0.1 to 0.01 forall values of α. For small α the convergence time is large. In the non-maneuveringcase (right) the overshoot of RMSE for rrel is larger for small values of α. Theestimates of ttg is better with larger values of α, meaning that the angular velocityis larger. When the platform maneuvers the estimates of ttg worsen during thetime the maneuver is performed for α = 5 and α = 10 but for α = 0.1 themaneuver improves the estimates.

Similar results can be seen in Figure 6.12 where β = 2. For the case of non-maneuvering platform the overshoot in RMSE for rrel is smaller when β = 2 thanwhen β = 0.


0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

RM

SE

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

RM

SE

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

α=0.1°

α=5°

α=10°

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

Time [s]Time [s]

rrel with maneuver, ti = 5s rrel without maneuver

ttg with maneuver, ti = 5s ttg without maneuver

Figure 6.11. RMSE in rrel and ttg for β = 0 and α = 0.1 (solid), α = 5

(dashed)and α = 10

(dotted). Comparison of a case with platform maneuver with ti = 5s (left)and without platform maneuver (right).

6.3 Influence of Ω and θ 59

0 2 4 6 8 10 12 14 160

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

RM

SE

0 2 4 6 8 10 12 14 160

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

RM

SE

0 2 4 6 8 10 12 14 160

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

α=0.1°

α=5°

α=10°

0 2 4 6 8 10 12 14 160

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

RM

SE

Time [s]Time [s]

rrel with maneuver, ti = 5s rrel without maneuver

ttg with maneuver, ti = 5s ttg without maneuver

Figure 6.12. RMSE in rrel and ttg for β = 2 and α = 0.1 (solid), α = 5

(dashed)and α = 10

(dotted). Comparison of a case with platform maneuver with ti = 5s (left)and without platform maneuver (right).


6.4 Influence of Errors in the Initial State Vector

To study the influence of errors in the initial state vector Scenario A was simulatedfor different values of ∆r and ∆v. In Figure 6.13 only errors in the initial rangeare simulated, i.e. ∆v = 0. The platform is performing an S-turn with ti = 5sand ti = 1s respectively.

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

8

RM

SE

∆ r=0

∆ r=200

∆ r=600

∆ r=1200

0 2 4 6 8 10 12 14 16 180

2

4

6

8

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

8

RM

SE

Time [s]Time [s]

rrel with maneuver, ti = 5s


rrel without maneuver

ttg with maneuver, ti = 5s


ttg without maneuver

Figure 6.13. RMSE in rrel and ttg for different values of ∆r. Scenario A with ∆v = 0.Comparison of platform maneuver with ti = 5s (top), ti = 1s (middle) and no platformmaneuver (bottom).

When ti = 5s (top) RMSE increases for rrel before the platform maneuver.After the maneuver RMSE decreases and has decreased from 0.4 to less than 0.1after 7 seconds when ∆ = 1200m. In the case with ti = 1s (middle) RMSE de-creases during the platform maneuver, but after the maneuver RMSE increasesbefore decreasing again. After 15 seconds the performance is as good as in thecase with ti = 5s. When the platform is not maneuvering (bottom) RMSE doesnot decrease. For ttg the difference between the maneuvering case and the non-maneuvering case is not significant. In Appendix E the same simulations arepresented with ∆v = 10m/s and ∆v = −10m/s.

In Figure 6.14 the time mean of RMSE, RMSEm, is shown for different valuesof ∆r and ∆v. The performance of the filter is best when ∆r and ∆v have thesame signs, i.e. if the initial guess is that the target is closer and moving slowerthan in reality or farther away and moving faster. The platform maneuver withti = 5s gives the best performance seen over time, particularly if ∆r and ∆v have

6.4 Influence of Errors in the Initial State Vector 61

−500 0 500 10000

10

20

30

40

50

60

70

80

90

∆ v=−30

∆ v=−10

∆ v=0

∆ v=10

∆ v=30

−500 0 500 10000

10

20

30

40

50

60

70

80

90

−500 0 500 10000

10

20

30

40

50

60

70

80

90

∆r∆r∆r

RM

SE

m

RM

SE

m

RM

SE

m

rrel with maneuver, ti = 5s rrel with maneuver, ti = 1s rrel without maneuver

Figure 6.14. Time mean of RMSE, RMSEm, in rrel for different values of ∆r and ∆v.Scenario A.


different signs.

6.5 Target Maneuver

Simulations with the target maneuvering in Scenario A were performed to studythe filters behavior. The target was performing an S-turn, with angular velocityωt, in the horizontal plane at time 8-14 seconds. Two platform maneuvers wereperformed before and after the target’s maneuver, at time 3-7 s and 14-18s. Thesame platform maneuvers are used in the simulations in the next chapter wherethe IMM filter is evaluated. Figure 6.15 shows RMSE for rrel and ttg for differentvalues of the target’s angular velocity ωt.

0 2 4 6 8 10 12 14 160

0.2

0.4

0.6

0.8

1

RM

SE

0 2 4 6 8 10 12 14 160

1

2

3

4

5

6

7

8

9

RM

SE

rrel = r−rr

ttg

Time [s]

Time [s]

0/s1.25/s2.5/s

Figure 6.15. RMSE in rrel and ttg for Scenario A where the target performs a S-turnwith ωt

= 1.25/s (dashed) and ωt= 2.5/s (dotted)and is not maneuvering (solid) The

platform performs two maneuvers to gain observability.

When the target has maneuvered for only one second RMSE for rrel increasesfast. After the maneuver the error in rrel is large and the filter is not able toestimate range well during the rest of the simulation. RMSE in ttg grows largeduring the maneuver and the simulation ends before the filter can reduce theestimation error. The filter is designed for tracking non-maneuvering targets andit is not able to track maneuvering targets, even though the maneuver is as smallas ωt = 1.25/s.

6.6 Monte Carlo Simulations 63

6.6 Monte Carlo Simulations

Monte Carlo simulations were performed for Scenario A-D to get a more generalidea of the filter’s performance. All scenarios are depicted in Appendix D. Foreach scenario 200 simulations were performed where the platform maneuvered withti = 1s and ti = 5s. The case with no platform maneuver was also simulated. ∆rand ∆v were simulated as Gaussian noise with zero mean and standard deviation300m and 10m/s respectively. In this section RMSE plots of rrel and ttg areincluded. In Appendix F RMSE plots for all states together with the estimateduncertainty σ are included for all scenarios.

Figure 6.16 shows RMSE for rrel and ttg for Scenario A. The plots show similarresults as the previous RMSE plots for the same scenario. When the platform isnot maneuvering, the initial error in rrel remains. A few seconds after the platformmaneuver is initialized RMSE in rrel decreases. A larger ti gives faster conver-gence after the maneuver. The estimates of ttg are not affected by the platformmaneuver and RMSE decreases after 7 seconds.

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2

RM

SE

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

2.5

3

RM

SE

No maneuverti=1s

ti=5s

ttg

Time [s]Time [s]

rrel = r−rr

Figure 6.16. Monte Carlo simulation for Scenario A. The platform is maneuvering withti = 5s (dotted), with ti = 1s (dashed) and is not maneuvering (solid).

Figure 6.17 shows RMSE plots for Scenario B where the target starts to theleft of the platform. In this scenario it is hard to tell whether it is best to performa maneuver with high or low initial time. It depends on when it is most importantto achieve small estimation errors. Compared to Scenario A ti = 5s gives larger


convergence time, but for ti = 1s the convergence time is smaller than in Scenario

A. RMSE in ttg converges faster than in Scenario A, since Ω is larger. After 18seconds CPA is reached and RMSE in ttg is large. After CPA RMSE decreases fast.

0 5 10 15 200.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2

RM

SE

0 5 10 15 200

0.5

1

1.5

2

2.5

3

3.5

4

RM

SE

No maneuverti=1s

ti=5s

ttg

Time [s]Time [s]

rrel = r−rr

Figure 6.17. Monte Carlo simulation for Scenario B. The platform is maneuvering withti = 5s (dotted), with ti = 1s (dashed) and is not maneuvering (solid).

In Scenario C the target and the UAV collide if no maneuver is performed.This means that the Ω = 0 and θ = 0. Figure 6.18 shows RMSE for rrel and ttgfor this scenario. The platform maneuver with ti = 5s leads to faster convergencefor RMSE in both rrel and ttg than the platform maneuver with ti = 1s. If theplatform is not maneuvering the RMSE for rrel diverge. For ttg RMSE does notdiverge when the platform is not maneuvering, but the initial error remains. Dur-ing and after the platform maneuver Ω 6= 0, meaning that ttg is observable. After20 seconds the target and UAV collide, meaning that ttg changes sign. Due toinitial errors in the state vector, ∆r and ∆v, the filter misscalculates the collisiontime, if ttg is unobservable, i.e. if the platform does not maneuver. Since ∆r and∆v are simulated as noise the filter calculates the collision time to be different ineach simulation. This explains the fluctuations in the end of the RMSE plot forttg with no platform maneuver.

In Scenario D the target only stays in the field of vision for the UAV during7.2 seconds. The platform maneuvers improves the estimates of rrel but worsenthe estimates of ttg.


0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2R

MS

E

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

2.5

3

RM

SE

No maneuverti=1s

ti=5s

ttg

Time [s]Time [s]

rrel = r−rr

Figure 6.18. Monte Carlo simulation for Scenario C. The platform is maneuvering withti = 5s (dotted), with ti = 1s (dashed) and is not maneuvering (solid).

0 1 2 3 4 5 6 70.05

0.1

0.15

0.2

RM

SE

0 1 2 3 4 5 6 70

0.5

1

1.5

2

2.5

3

RM

SE

No maneuverti=1s

ti=5s

ttg

Time [s]Time [s]

rrel = r−rr

Figure 6.19. Monte Carlo simulation for Scenario D. The platform is maneuvering withti = 5s (dotted), with ti = 1s (dashed) and is not maneuvering (solid).

Chapter 7

Simulation Results -IMM-Filter

This chapter presents the results from simulations performed to evaluate the IMM-filter. First the IMM-filter is compared with the MSC-EKF in the case of a non-maneuvering target. Thereafter the IMM-filter is evaluated for cases where thetarget is performing S-turns with different angular velocities. The necessity ofplatform maneuvers and the influence of intial error in range are also studied. Fi-nally Monte Carlo Simulations are used to evaluate the performance of the filterin general for other scenarios.

For the MSC-EKF two different initializations of the estimated prediction co-variance matrix P were tested, P 1

0|−1 and P 20|−1 described in Section 5.1.2. P 1

0|−1

resulted in a filter that predicted the uncertainty in the estimated state vectorwell, but the convergence time was large. P 2

0|−1 resulted in a faster filter, but theestimated uncertainty σ was significant larger than RMSE. For the MSC-EKF thesecond initialization was used, since it resulted in the best estimates. In the IMM-filter the estimated uncertainty is of great importance to decide whether the targetmaneuvers or follows at straight line, i.e. to estimate the model probabilities µj .Therefore P 1

0|−1 is used in the IMM-filter.

7.1 Tracking Target Maneuvers

The IMM-filter is designed for tracking maneuvering targets, but must also be ableto track non-maneuvering targets. In this section different target maneuvers aresimulated, including the case of a non-maneuvering target.

Consider Scenario A used in most simulations in the previous chapter and de-picted in Appendix D. This scenario is modified to let the target perform S-turnswith different angular velocity. The S-turns is performed in the horizontal plane.The target is maneuvering at time 8-14 seconds starting with a CT with negative

67

68 Simulation Results - IMM-Filter

angular velocity. To obtain observability the platform performs S-turns at time3-7 seconds and 14-18 seconds both starting right. See Table 7.1. The platformand the target will maneuver during these time intervals in all simulations in thischapter unless other stated.

Table 7.1. Maneuvering times for target and UAV.

Time (sec) 0-3 3-7 7-8 8-11 11-14 14-18 18-Target CV CV CV CTneg CTpos CV CVUAV - S-turn - - - S-turn -

7.1.1 Non-Maneuvering Target

According to Chapter 6 the MSC-EKF performs well when the target is travelingalong a straight line. In Section 6.5 in the previous chapter the MSC-EKF was eval-uated for the case when the target performed a small maneuver and the platformmaneuvered as in Table 7.1. The estimation errors from the MSC-EKF increasedfast when the target maneuvered. Hence the MSC-EKF tracks non-maneuveringtargets well, but can not track maneuvering target, not even if the maneuver issmall. The IMM-filter is designed for maneuvering targets, but must also be ableto handle non-maneuvering targets. Figure 7.1 shows RMSE for rrel and ttg forboth IMM-filter (solid) and MSC-EKF (dashed) where the target is traveling alonga straight line. The probabilities for the different models, µj

k = Prmjk used in

the IMM-filter is also shown (bottom). As expected the MSC-EKF performs best,since it is designed for non-maneuvering targets. RMSE for rrel is close to zeroin the MSC-EKF after only 6 seconds. In the IMM-filter RMSE decreases slow.For estimating ttg the IMM-filter is slower than the MSC-EKF but the estimatesare overall better. The probability for the CV-model is large during the entiresimulation, meaning that the filter is using the right model. In this simulationthe IMM-filter uses P 1

0|−1 and the MSC-EKF uses P 20|−1. The RMSE plot for the

IMM-filter is similar to the RMSE plot in the previous chapter where the MSC-EKF used P 1

0|−1. With a better initialization of P it might be possible to obtaina faster IMM-filter.

7.1.2 Maneuvering Target

The IMM-filter is designed for straight line trajectories and coordinated turnswith ωt = ±20/s, but must handles targets that maneuvers with lower angularvelocity. Figure 7.2 shows RMSE for simulations where the targets is performingS-turns with ωt = 5/s and ωt = 10/s (top) and ωt = 15/s and ωt = 20/s(bottom). Figure 7.3 shows µj for the models.

All maneuvers can be described as combinations of the CV-model and ±20/s-CT-models with and the filter combines the models well. In the beginning of the

7.1 Tracking Target Maneuvers 69

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

RM

SE

0 2 4 6 8 10 12 14 160

0.5

1

1.5

RM

SE

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

rrel = r−rr , no target maneuver

ttg, no target maneuver

µ, no target maneuver

S-turn ωS-turn

CV

CTpos

CTneg

IMMMSC

Time [s]

Figure 7.1. RMSE for rrel =r−r

rand ttg for IMM (solid) and MSC-EKF (dashed) and

the probabilities µj for the filters models. The target is traveling along a straight line,Scenario A.

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

RM

SE

0 2 4 6 8 10 12 14 160

0.5

1

1.5

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

RM

SE

0 2 4 6 8 10 12 14 160

0.5

1

1.5rrel = r−r

r

rrel = r−rr

ttg

ttg

5/s

10/s

15/s

20/s

Time [s]Time [s]

Figure 7.2. RMSE for rrel =∆rr

and ttg. The target is performing S-turns withωt

= 5/s and ωt

= 10/s(top) and ωt

= 15/s and ωt

= 20/s(bottom), Scenario A.


0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

S-turn

µ, S-turn ωt = 5/s µ, S-turn ωt = 10/s

µ, S-turn ωt = 15/s µ, S-turn ωt = 20/s

CV

CTpos

CTneg

Time [s]Time [s]

Figure 7.3. The probabilities for the different models µj used by the IMM. The target isperforming S-turns with ωt

= 5/s, ωt

= 10/s, ωt

= 15/s and ωt

= 20/s respectively,

Scenario A.

7.1 Tracking Target Maneuvers 71

5/s-S-turn the probability of the negative 20/s-CT model increases, but theCV-model dominates. For the 15/s turn the case is the opposite, the negative20/s-CT model dominates but the probability of the CV-model is around 20%and the estimates of the CV-model contributes to the total estimates. RMSE forrrel and ttg is smallest in the 5/s and 20/s case but is acceptable for all maneu-vers.

Figure 7.4 shows RMSE in rrel and ttg and the model probabilities µj whenthe target performs S-turns with ωt = 25/s and 30/s. The filter is not designedfor tracking targets maneuvering faster than ωt = 20/s and the estimates are notsatisfying. S-turns with ωt = 25/s and ωt = 30/s can not be described as mix-

0 2 4 6 8 10 12 14 160

0.2

0.4

0.6

0.8

1

RM

SE

0 2 4 6 8 10 12 14 160

1

2

3

4

5

RM

SE

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

0 2 4 6 8 10 12 14 160

2

4

6

0 2 4 6 8 10 12 14 160

5

10

15

20

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

target maneuv

rrel = r−rr , S-turn ωt = 25/s rrel = r−r

r , S-turn ωt = 30/s

ttg ttg

µ µ

CV

CTpos

CTneg

Time [s]Time [s]


rand ttg and the model probabilities µj . The target

is performing an S-turn with ωt= 25

/s (left) and ωt= 30

/s (right).

ings of a CV and a 20/s-CT. In the beginning of the maneuver the probability forthe negative CT-model increases and in the end of the maneuver the probabilityof the positive CT-model is high. After the maneuver the probabililty for the CVmodel increases and RMSE in rrel and ttg decreases. The estimation errors thatoccurred during the maneuver highly worsen the performance of the filter evenafter the maneuver.


7.1.3 Target Maneuver with Time-Varying Angular Veloc-ity

The same scenario was simulated again with the target maneuver described inTable 7.2, where ωt = 0 means that the target is traveling with constant velocity.The angular velocity of the target maneuver changes every second during themaneuvering period, instead of the maneuvers studied previous where the angularvelocity where constant for three seconds.

Table 7.2. True angular velocity for the target maneuver in Section 7.1.3.

Time (sec) 0-8 8-9 9-10 10-11 11-12 12-13 13-14 14-ωt 0 −5/s −10/s −5/s 5/s 10/s 5/s 0

Figure 7.5 shows RMSE for rrel and ttg and the model probabilities µj for thiscase.

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

RM

SE

0 2 4 6 8 10 12 14 160

0.2

0.4

0.6

0.8

1

RM

SE

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

rrel = r−rr

ttg

µ

CV

CTpos

CTneg

Time [s]


rand ttg and the model probabilities µj . The target

is performing a maneuver where the angular velocity varies every second according toTable 7.2, Scenario A.

The filter follows the target maneuver well and the probabilities for the maneu-vering models increase during the maneuver. RMSE for both rrel and ttg decreaseduring the maneuver and remain small after the maneuver.

7.2 Influence of Platform Maneuvers 73

7.2 Influence of Platform Maneuvers

When the target travels with constant velocity observability in range is gained ifthe platform maneuvers. For a target performing a coordinated turn range is un-observable even though the platform maneuvers. This section studies the influenceof platform maneuvers to see when the platform should maneuver.

In Figure 7.6 RMSE for rrel and ttg are shown for the same scenario as inthe previous section when the target performs an S-turn with ωt = 10/s. Theplatform performs no maneuver, one maneuver (at 3-7s) or two maneuvers (at 3-7s and 14-18s). The case where the platform is repeating S-turns and maneuversduring the entire simulation is also shown.

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

0.2

0.25

RM

SE

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

2.5

3

3.5

RM

SE

no man1 man2 manmaneuvering

rrel = r−rr

S-turn ω

ttg

Time [s]


rand ttg for no platform maneuver (solid), one platform

maneuver (solid,bright), two platform maneuvers (dashed) and platform maneuvers dur-ing the entire simulation (dotted). The target is performing an S-turn with ωt

= 10/s,

Scenario A.

When the platform maneuvers during the entire simulation RMSE in rrel andttg is low before the target maneuvers, but during and after the target maneuverthe RMSE is high compared to the other cases. In the case of only one or twoplatform maneuvers, the first maneuver decreases RMSE in rrel during the rest ofthe simulation time. RMSE in ttg is smaller after the platform maneuver especiallyduring the target maneuver. The second platform maneuver is performed in theend of the simulation and its influence of the estimate is small, but can be seen inRMSE for rrel.


7.3 Influence of Initial Error in Range, ∆r

To study the influence of errors in the initial guess of range the same scenario wassimulated again with the platform performing two maneuvers. Figure 7.7 showsRMSE for rrel and ttg for different values of ∆r with ∆v = 0.

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

0.8

1

RM

SE

0 2 4 6 8 10 12 14 16 180

1

2

3

4

5

6

7

8

RM

SE

∆ r=1200

∆ r=600

∆ r=200

∆ r=0

rrel = r−rr

ttg

Time [s]

Figure 7.7. RMSE for r−rr

and ttg for different values of the initial error in range ∆r,∆v = 0m/s. The target is performing an S-turn with ωt

= 10/s.

In all cases the RMSE in rrel < 0.25 and in ttg < 1s at the end of the simulation.For ∆r = 1200m the RMSE does not fall below 0.25 until after 14 seconds, butfor smaller initial errors the performance is better. Even in the case of no initialerror, ∆r = 0, the maneuver introduce an estimation error in both rrel and ttg.The influence of initial errors was studied more in the previous chapter for theMSC-EKF. The results are similar, but the convergence time in the IMM-filter islarger.


7.4 Monte Carlo Simulations

Monte Carlo simulations were performed for Scenario A-C, with 200 simulationfor each scenario, to get a more general idea of the filter performance. The targetmaneuver was simulated as an S-turn with the angular velocity as Gaussian noisewith mean 10/s and standard deviation 5/s. Single coordinated turns were alsosimulated as target maneuvers. ∆r and ∆v were simulated as Gaussian noise withstandard deviation 300m and 10m/s respectively. The scenarios are depicted with-out target maneuver in Appendix D. The platform is performing two maneuversaccording to Table 7.1.

The initial prediction covariance matrix P calculated in Appendix C is basedon Scenario A. When this initialization is used for other scenarios the IMM-filtergets unstable with small values in the likelihood of the models λj

k. This was solvedby an alternative way of calculating λ

λjk =

1

|εjk| + ελ

(7.1)

εjk is the innovation calculated in the measurement update in the filtering. ελ is a

design parameter which prevent the denominator to be zero, here set to 10−7. Thisdegrades the filter for Scenario A as will be seen in the simulations, but preventsthe filter to become unstable for other scenarios.

7.4.1 Scenario A

In Figure 7.8 RMSE for rrel and ttg is shown together with the probabilities ofthe models, µj for Scenario A with λ calculated as described in (7.1).

The filter predicts the models well during the entire simulation. RMSE for ttgdecreases after four seconds and is less than one second after seven seconds of thesimulation. RMSE for rrel remains almost constant until the target maneuver,where it increases slightly. During the entire simulation RMSE in rrel < 0.25.

In Figure 7.9 RMSE for all states (dark) are shown together with the estimatedprediction standard deviation σ of the states (bright). RMSE is close to the σ forall states. When RMSE in ϕ and Ω increases and decreases the σϕ and σΩ follows.For the first state 1

r RMSE increases faster than σ 1

rduring the target maneuver.

After the target maneuver RMSE for 1r remains almost constant, but is larger than

σ 1

r.

Figure 7.10 shows RMSE in rrel and ttg together with µj for Scenario A wherethe target is performing a single coordinated turn with negative angular velocity.Since the maneuvering time is shorter and the model changes are fewer, the esti-mation error introduced by the maneuver is smaller than in the case of a S-turn.RMSE for rrel < 0.2 during the entire simulation and RMSE for ttg shows the


0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

0.25R

MS

E

0 2 4 6 8 10 12 14 160

1

2

3

4

RM

SE

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

rrel = r−rr

ttg

µ

S-turn

CV

CTpos

CTneg

Time [s]


rand ttg and the probabilities for different models µj .

Scenario A with the target performing an S-turn.

0 2 4 6 8 10 12 14 160

1

2

3

4x 10

−4

[m−

1 ]

0 2 4 6 8 10 12 14 16

4

6

8

10x 10

−3

[deg

]

0 2 4 6 8 10 12 14 160

0.002

0.004

0.006

0.008

0.01

[deg

]

0 2 4 6 8 10 12 14 160

0.002

0.004

0.006

0.008

0.01

[s−

1 ]

0 2 4 6 8 10 12 14 16

0.05

0.1

0.15

[deg

/s]

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

[deg

/s]

RMSE

ϕ

θ

1r

Ω θ

rr

Time [s]Time [s]

σ

Figure 7.9. RMSE (dark) and σ (bright) for the state vector. Scenario A with thetarget performing an S-turn.


0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

0.2

0.25R

MS

E

0 2 4 6 8 10 12 14 160

1

2

3

RM

SE

0 2 4 6 8 10 12 14 160

20

40

60

80

100

%

rrel = r−rr

ttg

µ

S-turn ω

CV

CTpos

CTneg

Time [s]


rand ttg and the probabilities for different models

µj . Scenario A with the target performing a single negative coordinated turn.

same behavior as in the previous simulation.

Figure 7.11 shows RMSE (dark) and σ (bright) for all states. RMSE for 1r is

smaller during and after the target maneuver than in the case where the targetperforms a S-turn. During the target maneuver RMSE in ϕ and Ω increase. σϕ

and σΩ follows RMSE during the simulation.

Compare Figure 7.8 and Figure 7.10 with Figure 7.2 where λ was calculatedaccording to the IMM algorithm presented in Chapter 3.2. Calculating λ with(7.1) results in a slower and more robust filter.


0 2 4 6 8 10 12 14 160

1

2

3

4x 10

−4

[m−

1 ]

0 2 4 6 8 10 12 14 16

4

6

8

10x 10

−3

[deg

]

0 2 4 6 8 10 12 14 16

2

4

6

8

10x 10

−3

[deg

]

0 2 4 6 8 10 12 14 160

0.002

0.004

0.006

0.008

0.01

[s−

1 ]

0 2 4 6 8 10 12 14 16

0.05

0.1

0.15

[deg

/s]

0 2 4 6 8 10 12 14 160

0.05

0.1

0.15

[deg

/s]

RMSE

ϕ

θ

1r

Ω θ

rr

S-turnS-turn

Time [s]Time [s]

σ

Figure 7.11. RMSE (dark) and σ (bright) for the state vector. Scenario A with thetarget performing a single negative coordinated turn.


7.4.2 Scenario B

In Scenario B the target starts to the left of the UAV and travels towards it. Fig-ure 7.12 shows RMSE for rrel and ttg and the probability µj for this scenario.The target is performing an S-turn starting with positive angular velocity, i.e. thetarget turns towards the UAV.

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

0.8

1

RM

SE

0 2 4 6 8 10 12 14 16 180

20

40

60

80

RM

SE

0 2 4 6 8 10 12 14 16 180

20

40

60

80

100

%

rrel = r−rr

ttgS-turn

S-turn

µ

CV

CTpos

CTneg

Time [s]


rand ttg and the probabilities for different models µj .

Scenario B with the target performing a S-turn starting with positive angular velocity.

The filter does not predicts the probabilities for the models as well as for Sce-

nario A and after the maneuver the probability for the CV-model is less than 60%.RMSE for ttg is small in the beginning of the simulation, but increases when thetarget maneuvers. After the target maneuver the probability of the CV-model islow and the filter is not able to estimate ttg well, even though it is observable.RMSE for rrel < 0.3 until the end of the target maneuver when RMSE increasesfast. In the end of the simulation RMSE in rrel decreases but is still large.

Figure 7.13 shows RMSE and σ for all states for Scenario B. In all states bothRMSE and σ increases fast at time 14 s. At this instance of time the target ma-neuver ends and the platform starts to maneuver. In ϕ and θ, both RMSE andσ decrease fast and in Ω and θ, RMSE and σ has decreased to there former lev-els after two seconds. In 1

r , RMSE and σ decrease, but slower than in the otherstates. As stated previous RMSE in ttg was large. RMSE for the fourth state,the negative inverse of ttg, is also large during the entire simulation and exceedsσ during the most part of the simulation.


0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

0.8

1x 10

−3

[m−

1 ]

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

[deg

]

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

[deg

]

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

[s−

1 ]

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

[deg

/s]

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

[deg

/s]

RMSE

ϕ

θ

1r

Ω θ

rr

S-turn

Time [s]Time [s]

σ

Figure 7.13. RMSE for the state vector (dark) and predicted standard deviation(bright). Scenario B with the target performing an S-turn starting with positive an-gular velocity.

When the target is performing a coordinated turn, both range and ttg are un-observable. The measurements do not provide the filter with any information ofrange and ttg and the filter relies only on the models. This means that it is essen-tial for the filter to estimate µj well. Even though µj reflects the true behavior ofthe target, the estimates of the model probabilities are not good enough for thefilter to estimate the range and ttg in an acceptable way.

In Scenario A the target maneuver affected the measurements clearly enoughfor the filter to detect the maneuver. When the target approaches the platformfrom the side, it is hard for the filter to decide whether a change in the measure-ments is due to a target maneuver, or if it should be considered as measurementnoise. When λ was calculated as in Algorithm 4 this resulted in too small valuesof λ and the filter became unstable. λ calculated with (7.1) prevented the filter tobecome unstable, but the estimates of µj are not good enough.


7.4.3 Scenario C

In Scenario C the target is traveling towards the UAV meaning that Ω = 0 andθ = 0 before the UAV maneuvers. As was seen in Chapter 6 the estimates of rangediverge and the initial error in ttg remain in this scenario when neither the targetnor the platform maneuver. Platform maneuvers improved the estimates, but theconvergence time was large in the case where the target was not maneuvering.

Figure 7.14 shows RMSE and µj for Scenario C where the target performs anS-turn. As in Scenario B the model probabilities µj are not estimated well enough.

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

RM

SE

0 2 4 6 8 10 12 14 16 180

20

40

60

80

100

120

RM

SE

0 2 4 6 8 10 12 14 16 180

20

40

60

80

100

%

rrel = r−rr

ttg

µ

CV

CTpos

CTneg

Time [s]


rand ttg and the probabilities for different models

µj . Scenario C with the target performing an S-turn.

RMSE is low for both rrel and ttg during the first half of the target maneuver,when the target turns away from the UAV. During the second half of the maneuverthe estimates worsen. After the target maneuver the probability for the positiveCT-model decreases, but the filter finds the CV-model and the negative CT-modelequally probable. The maneuvering model has too great impact of the estimatesafter the maneuver, which leads to bad estimates.

The same phenomena can be found in Figure 7.15 where RMSE and σ areshown for all states. After the maneuver both RMSE and σ are large in all states.The filter does not find any model that fits the measurements in a good way andtries to combine all models. This yields noisy estimates.


0 2 4 6 8 10 12 14 16 180

1

2

3

4x 10

−3

[m−

1 ]

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

[deg

]

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

[deg

]

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

[s−

1 ]

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

[deg

/s]

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

[deg

/s]

RMSE

ϕ

θ

1r

Ω θ

rr

S-turnS-turn

Time [s]Time [s]

σ

Figure 7.15. RMSE (dark) and σ (bright) for the state vector. Scenario C with thetarget performing a S-turn.

Chapter 8

Conclusions and FutherWork

This chapter summaries the thesis with conclusions from the simulations and ideasof future work.

In angle-only target tracking the aim is to estimate the state of a target withuse of measurement of elevation, θ and azimuth, ϕ. The state consists of therelative position and velocity between the target and the platform. In this thesisthe platform studied has been a UAV. The tracking system is meant to supplya collision avoidance system with information of the target. The fastness of thefilter is essential since the target can not be assumed to stay in the field of visionfor a long period of time. The collision avoidance system must be able to estimatethe collision probability in such a good time that an evasive maneuver can be per-formed if necessary. A fast filter can be obtained if the measurements are accurate.In this case the angle measurements are accurate but they do not provide muchinformation of the range to the target. Under the assumption of both target andplatform traveling with constant velocity, range is not observable, meaning thatit is not possible to eliminate initial errors in range. If the platform maneuversobservability in range can be obtained. The problem of finding an optimal plat-form maneuver has not been treated, since it is not realistic that UAVs have themaneuverability that is required.

Two filters have been implemented and tested on simulated data. The MSC-EKF was designed for non-maneuvering targets. It was based on an EKF wherethe state vector was expressed in MSC. The other filter was an IMM-filter, whichcombined three MSC-EKFs. One of the subfilters used a CV-model and the othertwo used CT-models with angular velocity ±ωmax. The idea was that all targetmaneuvers with angular velocity |ωt| < ωmax could be described as combinationsof the CV-model and the CT-model with ωmax.

83

84 Conclusions and Futher Work

8.1 Conclusions

The performance of the filters highly depends on the geometry in the scenariothat are studied. In the simulations Scenario A, presented in Chapter 5 wasmostly studied and conclusions of the filter performance for other scenarios shouldbe drawn with caution.

8.1.1 MSC-EKF

The filter estimates Time To Go (ttg) best if the fifth and the sixth state, Ω andθ, are large. When both Ω = 0 and θ = 0 ttg is not observable and the initialerror remains. To compensate for this the filter was initializated with Ω = 0 andθ = 0. This is the scenario where it is hardest to get good estimates of the statesand also a scenario where the collision probability is high.

Two initializations of the prediction covariance matrix P were tested. The firstP 1

0|−1 was based on the propagation of uncertainty formula and resulted in goodestimates of the uncertainty in the filter. On the other hand the filter was slow.The convergence time in ttg was large and even though the platform maneuveredthe relative error in range, rrel, did not decrease. The other initialization resultedin a faster filter. The convergence time in ttg was shorter with this initializationand if the platform maneuvered RMSE in rrel decreased. On the other hand theestimated uncertainty was significant larger than RMSE in all states. The fastnessof the filter depends on the Kalman gain K, expressed in Algorithm 3. When Kis large, the measurements have great impact of the estimates and the filter isfast. A smaller value of K results in a slower filter, that are more robust againstmeasurement noise. K depends on the estimated uncertainty in the filter in formof P . When the uncertainty of the estimates is large, K is large and the filterbecomes fast as in the second case. A small uncertainty on the other hand, re-sults in a slower filter as in the first case. The values in P depend both on theinitialization of P and of the chosen values in the covariance matrices for processnoise Q and measurement noise R. With other initializations of P , Q and R, acompromise between the good estimated uncertainty in the first case and the fastfilter in the second case can be obtained, but the main problem remains that afast filter requires large values of K and therefore large values in P .

For the second initialization of P the filter was tested for different platformmaneuvers. A platform maneuver in the form of an S-turn improves the estimatesof range. Range is observable during the maneuver and the larger maneuveringtime the better estimates. A platform maneuver that starts soon after the targetis detected results in decreasing RMSE in rrel, but the convergence time becomeslarge. When a platform maneuver is started later the estimates are bad beforethe maneuver is performed but after the maneuver the estimates converges faster.Whether the maneuver is performed as a left- or right turn affects the estimates.To find rules of when left or right is preferred more testes are needed.

8.1 Conclusions 85

In the case of a non-zero Ω or θ, ttg converges and the error in rrel can beconsidered as constant, in the case of no platform maneuver. If the initial errorin range is small, one might consider the estimates satisfying even if the platformdoes not maneuver. Performing a platform maneuver improves the estimates. Inthe case when the Ω = 0 and θ = 0 a platform maneuver is necessary to get accu-rate estimates. When the platform performs an S-turn, it changes its position insuch a way that Ω will be non-zero after the maneuver and ttg becomes observable.Range is observable during the platform maneuver.

The influence of initial errors in range and speed was also simulated. The filterperforms best if the target is assumed to move faster and be farther away than itreally is or if it is assumed to be closer and move slower.

The MSC-EKF is designed for tracking non-maneuvering target and it wasnot able to track a target that maneuvered, even if the maneuver was small. Fortracking maneuvering targets another filter is needed.

8.1.2 IMM

The IMM filter combines three MSC-EKFs. One subfilter uses a CV-model andthe other two uses CT-models with target angular velocity ±ωmax = 20/s. Thesimulations for Scenario A showed that the filter interpreted target maneuvers withangular velocity ωt < ωmax as a combination of the CV-model and the CT-model.For instance when the target maneuvered with ωt = ωmax/2 the filter estimatedthe probabilities for the CV-model and the positive CT-model to be around 45%each. The estimated probability for the CT-model with negative ωmax was low.The filter was also tested for target maneuvers with |ωt| > ωmax but the filtercould not handle these cases well.

To estimate the model probabilities µj it is essential to have good estimatesof the uncertainty in the filter in form of good estimates of P . The initializationof P used in the MSC-EKF, P 2

0|−1, did not estimate the uncertainty well enough.

On the other hand the first initialization P 10|−1 based on the propagation of uncer-

tainty formula resulted in good estimates of the uncertainty and was used in theIMM filter.

The IMM filter was tested for the case where the target did not maneuverand the IMM filter performed almost as good as the MSC-EKF did, when bothfilter initialized P with P 1

0|−1. When the MSC-EKF used P 20|−1 it provided smaller

RMSE in rrel than the IMM filter. The MSC-EKF could not track a target ma-neuvering with ωt = 1.25/s. The IMM-filter on the other hand was able to tracktargets maneuvering with 0 < |ωt| < ωmax = 20/s for Scenario A. The IMM-filterperformed best for maneuvers with |ωt| close zero or close to ωmax.

The influence of platform maneuvers was studied. Platform maneuvers thatwere performed when the target traveled with constant velocity improved the es-


timates of range. If the platform maneuvered during the time interval where thetarget maneuvered the estimates in range and ttg worsened.

The IMM filter showed acceptable results for Scenario A, but when the filterwas tested on other scenarios it showed an unstable behavior. The subfilterscalculation of the likelihood of the model λ became too small for all models andthe filter could not calculate the model probabilities µj . In Scenario A the targetmaneuver affected the measurements clearly enough for the filter to detect themaneuver. When the target approached the platform from the side, it was hardfor the filter to decide whether a change in the measurements was due to a targetmaneuver, or if it should be considered as measurement noise. An alternative wayto calculate λ was tested, where λ was large when the innovation was small andvice versa. This calculation of λ degraded the filter for Scenario A, but preventthe filter to become unstable in other scenarios. During the target maneuver bothrange and ttg are unobservable. This means that it is essential for the filter to usethe correct model, i.e. estimate the model probabilities µj well. The second wayof calculating λ prevented the filter to become unstable, but the calculations of µj

was not accurate enough for the filter to estimate range and ttg well.

8.2 Future Work

To evaluate the filters more, they should be tested on real flight data to test theassumptions of Gaussian measurement and process noise. If the weather is clearthe measurements are likely to be better than in the case of bad weather. Since theUAV is small, it is likely to be affected by wind, and wind is not well modeled aswhite process noise. The IMM filter has only been tested for CT in the horizontalplane. The filter ought to be tested for other target maneuvers.

To deal with the problem of the limited observability in range the easiest wayis to equip the platform with more sensors. Another alternative is to use multipleplatforms or to have sensors on the ground that also measures the target position.If there are several UAVs in the air they can cooperate. All UAVs can measureangles to the target and the measurements can be fused together.

To improve the performance of the tracking filters the initialization of P needto be investigated more, toghter with the choices of Q and R. When P wasinitialized according to the propagation of uncertainty formula, P 1

0|−1, the filterestimated the uncertainty in the estimates σ well, but the filter became slow. Theother initialization, P 2

0|−1, assumed the initial uncertainty to be significant larger,which resulted in a faster filter, but the estimated uncertainty became significantlarger than RMSE. An initialization of P with values between the first and thesecond initialization could compromise between a fast filter and good estimatesof the uncertainty. Since range is only observable when the platform maneuvers(under the assumption that the target does not maneuver), it is important to havea fast filter during this period of time. One idea is to increase the Kalman gain K

8.2 Future Work 87

in the filter during the platform maneuver, for instance by letting the covariancenoise matrix Q depend on the platform maneuver. When the platform travelswith constant velocity K can be small to get good estimates of the uncertaintyand during the maneuver a greater K would result in a faster filter and hopefullyin better estimates of range. However it is still important that the filter is fastenough to estimate ttg also when the platform does not maneuver.

If the aim is to track maneuvering targets IMM can be studied more. It wouldbe desirable to find a better way of calculating the likelihood λ in the subfilters.The problem remains that it is difficult for the filter to distinguish target maneu-vers from measurement noise. In the Algorithm 4 only one measurement yk isstudied at every time step. If the filter instead used a batch of measurement itmight be easier for the filter to detect maneuvers.

Another interesting question in the IMM design is the choice of subfilters.Instead of the three subfilters presented in this thesis a CV-model can be com-bined with a CA-model, which would result in a less complex filter. This filterwould be able to track targets that maneuvers also in the vertical plane. Anotheralternative is to combine the CV-model with a CT-model with unknown targetangular velocity, in the horizontal plane or in 3D. This is a more complex subfil-ter, but on the other hand the IMM is simplified since there are only two subfilters.

There are a number of other single model filters that can be used to tracknon-maneuvering targets or as subfilters in IMM, instead of the MSC-EKF. TheUnscented Kalman Filter (UKF) is described in [26]. Unlike the EKF that lin-earize the non-linear functions f and h, UKF uses the true functions and approx-imates the distribution of the state vector. In [25] IMM-EKF and IMM-UKF arecompared for an angle-only target tracking problem and the IMM-UKF outper-forms the IMM-EKF. Range Parameterized Extended Kalman Filter (RPEKF)presented in for instance [21] uses a batch of subfilters, where each subfilter usesdifferent hypothesis of the range. The output from the subfilters are then com-bined according to their probability. The approach is similar to IMM. A ParticleFilter (PF), also referred to as sequential Monte Carlo method, approximates thedistribution of the state with a large number of samples. Therefore it is possibleto run multiple hypotheses in parallel. Constraints such as maximum target speedcan easily be incorporated in the filter. The particle filter along with several otherestimation filters are presented in [21] and [26]. Also [4] and [3] present estimationfilters suited for target tracking. An alternative to IMM for tracking maneuveringtargets is the Multiple Model Particle Filter (MMPF) presented in [25].

8.2.1 Target Tracking in a Sense and Avoid Application

Whether the filter performance is good enough depends on its application, in thiscase a collision avoidance system. The requirements that the tracking filter mustfulfill should be analyzed more. To design a filter that works well in all possiblesituations is difficult, but with more work, one of the filters suggested above can


probably be designed to perform well enough in some situations.

Consider Figure 8.1 where three targets are depicted together with the UAV.The first target, tar1, are traveling parallel to the UAV and there is no risk forcollision. The second target, tar2, crosses the path for the UAV, but unless the

UAV

tar1

tar2

tar3

Figure 8.1. Three targets, tar1, tar2 and tr3 are depicted together with the UAV. tar1

and tar2 are not likely to collide with the UAV. Instead of tracking all targets equallywell it might be better to focus on tracking tar3.

target’s velocity is very small the target will not collide with the UAV. The thirdtarget, tar3, on the other hand will cross the path of the UAV and there is a risk forcollision. Instead of designing a filter that tracks all targets equally well, it mightbe better to design a filter that only tracks targets with high collision probabilitieswell. For other targets it is enough if the system can decide that the risk for col-lision is small. In [23] the problem of calculating the collision probability is studied.

Bibliography

[1] V. J. Aidala and S. E. Hammel. Utilization of Modified Polar Coordinatesfor Bearings-Only Tracking. In IEEE Transactionc on Automatic Control,volume AC-28, March 1983.

[2] R. Allen and S. S. Blackman. Implementation of an Angle-only TrackingFilter. In Signal and Data Processing of Small Targets, volume 1481, April1991.

[3] Y. Bar-Shalom and X. Li. Estimation and Tracking: Principles, Techniques,

and Software. Artech House, 1993.

[4] S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems.Artech House, 1999.

[5] S. Blackman and S. Roszkowski. Application of IMM Filtering to PassiveRanging. In SPIE Conference on Signal and Data Processing of Small Targets,volume 3809, July 1999.

[6] W. Blair, G. Watson, and T. Rice. Interacting Multiple Model Filter forTracking Maneuvering Targets in Spherical Coordinates. IEEE, 1991.

[7] G. Blom. Sannolikhetsteori med tillämpningar. Studentlitteratur, Lund, 2edition, 1984. Book A.

[8] G. Blom. Sannolikhetsteori och statistikteori med tillämpningar. Studentlit-teratur, Lund, 4 edition, 1989. Book C.

[9] H. Blom and Y. Bar-Shalom. The Interacting Multiple Model Algorithm forSystems with Markovian Switching Coefficients. In IEEE Transactions on

Automatic Control, volume 33, August 1988.

[10] F. Dufour and M. Mariton. Tracking a 3D Maneuvering Target With Pas-sive Sensors. In IEEE Transactions on Aerospace and Electronic Systems,volume 27, July 1991.

[11] E. Fogel and M. Gavish. Nth-Order Dynamic Target Observability FromAngle Measurements. In IEEE Transactions on Aerospace and Electronic

Systems, volume 24, May 1988.

89

90 Bibliography

[12] T. Glad, S. Gunnarsson, L. Ljung, T. McKelvey, A. Stenman, and J. Löfberg.Digital Styrning, Kurskompendium. Linköping University, 2003.

[13] P. Gurfil and N. J. Kasdin. Two-Step Optimal Estimator for Three Dimen-sional Target Tracking. In IEEE Transactions on Aerospace and Electronic

Systems, volume 41, July 2005.

[14] F. Gustafsson. Adaptive Filtering and Change Detection. John Wiley & Sons,LTD, 2000.

[15] F. Gustafsson, L. Ljung, and M. Millnert. Signalbehandling. Studentlitter-atur, Lund, 2001.

[16] G. Hendeby. Development and Evaluation of an Active Radio FrequencySeeker Model for a Missile with Data-Link Capability. Master’s thesis,Linköping University, 2002. Thesis no.: LiTH-ISY-EX-3309.

[17] A. Holtsberg. A Statical Analysis of Bearing-Only Tracking. PhD thesis,Lund Institute of Technology, 1992. Report 17.

[18] C. Jauffret and D. Pillon. Observability in Passive Target Motion Analysis. InIEEE Transactions on Aerospace and Electronic Systems, volume 32, October1996.

[19] H. J.S. Contact Localization and Motion Analysis in the Ocean Enviroment:a Perspective. In IEEE Journal of Oceanic Engineering 8, 1983.

[20] P. Karlsson. Methods for bearings-only tracking and range estimation. Tech-nical report, Saab Dynamics AB, 1997. IR-OTIS.

[21] R. Karlsson. Simulation Based Methods for Target Tracking. Department ofElectrical Engineering, Linköping University, 2002. Licentiate Thesis no. 930.

[22] J. Le Cadre and O. Trémois. Optimization of the Observer Motion UsingDynamic Programming. Irisa/Cnrs,Campus de Beaulieu, 35042 Rennes cedex,1995.

[23] U. Löfqvist. Kollisionsundvikning för UAV. Master’s thesis, Linköping Uni-versity, 2006. Thesis no.: LiTH-ISY-EX-3867.

[24] E. Pratsini. Informaion System Consulant’s Handbook: System Analysis an

Design, chapter nineteen: Simulation. CRC Press LLC, 1999. ENGnetBASE.

[25] B. Ristic and M. Arulampalam. Tracking a Manoeuvring Target Using Angle-Only Measurements: Algorithms and Performance. In Signal Processing,volume 83, 2003.

[26] B. Ristic, S. Arulampalam, and N. Gordon. Beyond the Kalman Filter, Par-

ticle Filters for Tracking Application. Artech House, 2004.

[27] R. Zhan and J. Wan. Passive Maneuvering Target Tracking Using 3DConstant-Turn Model. In IEEE Conference on Radar, April 2006.

Appendix A

Relation between CartesianCoordinates and MSC

This appendix describes the relations between the Cartesian coordinates and themodified spherical coordinates (MSC). The Jacobians for the transformation equa-tions are also given. The Cartesian coordinates are denoted by xcar and the MSCby zmsc. The state vector are given below:

xcar =(

x1 x2 x3 x4 x5 x6

)T=(

x y z x y z)T

zmsc =(

z1 z2 z3 z4 z5 z6

)T=(

1r ϕ θ r

r Ω θ)T

Ω = ϕ cos θ

A.1 Transformations from zmsc to xcar

z1 =1

r=

1√

x21 + x2

2 + x23

z2 = ϕ = arctan

(x2

x1

)

z3 = θ = arctan

(

−x3√

x21 + x2

2

)

z4 =r

r=

x1x4 + x2x5 + x3x6

x21 + x2

2 + x23

z5 = Ω = ϕ cos θ =x1x5 − x2x4

x21 + x2

2

cos

(

arctan

(

−x3√

x21 + x2

2

))

z6 = θ =−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)√

x21 + x2

2

91

92 Relation between Cartesian Coordinates and MSC

The Jacobian is defined by

Jz(x1, x2, ..., x6) =

∂z1

∂x1

∂z1

∂x2

. . . ∂z1

∂x6

∂z2

∂x1

∂z2

∂x2

. . . ∂z2

∂x6

......

. . ....

∂z6

∂x1

∂z6

∂x2. . . ∂z6

∂x6

=

Jz11 Jz12 . . . Jz16

Jz21 Jz22 . . . Jz26

......

. . ....

Jz61 Jz62 . . . Jz66

where

Jz11 = − x1

(x12 + x2

2 + x32)3/2

Jz12 = − x2

(x12 + x2

2 + x32)3/2

Jz13 = − x3

(x12 + x2

2 + x32)3/2

Jz14 =Jz15 = Jz16 = 0

Jz21 = − x2

x12 + x2

2

Jz22 =x1

x12 + x2

2

Jz23 =Jz24 = Jz25 = Jz26 = 0

Jz31 =x3x1√

x12 + x2

2 (x12 + x2

2 + x32)

Jz32 =x3x2√

x12 + x2

2 (x12 + x2

2 + x32)

Jz33 = −√

x12 + x2

2

x12 + x2

2 + x32

Jz34 =Jz35 = Jz36 = 0

Jz41 =x4

(x2

1 + x22 + x2

3

)− 2x1 (x1x4 + x2x5 + x3x6)

(x21 + x2

2 + x23)

2

Jz42 =x5

(x2

1 + x22 + x2

3

)− 2x2 (x1x4 + x2x5 + x3x6)

(x21 + x2

2 + x23)

2

Jz43 =x6

(x2

1 + x22 + x2

3

)− 2x3 (x1x4 + x2x5 + x3x6)

(x21 + x2

2 + x23)

2

Jz44 =x1

x12 + x2

2 + x32

Jz45 =x2

x12 + x2

2 + x32

Jz46 =x3

x12 + x2

2 + x32

A.1 Transformations from zmsc to xcar 93

Jz51 =x5

(x2

1 + x22

)− 2x1 (x1x5 − x2x4)

(x21 + x2

2)2 cos

(

arctan

(

−x3√

x21 + x2

2

))

−

x1(x1x5 − x2x4)

(x21 + x2

2 + x23)(x

21 + x2

2)3/2

sin

(

arctan

(

−x3√

x21 + x2

2

))

Jz52 =−x4

(x2

1 + x22

)+ 2x2 (x1x5 − x2x4)

(x21 + x2

2)2 cos

(

arctan

(

−x3√

x21 + x2

2

))

−

x2(x1x5 − x2x4)

(x21 + x2

2 + x23)(x

21 + x2

2)3/2

sin

(

arctan

(

−x3√

x21 + x2

2

))

Jz53 =(x1x5 − x2x4)

(x21 + x2

2 + x23)(x

21 + x2

2)3/2

sin

(

arctan

(

−x3√

x21 + x2

2

))

Jz54 =−x2

(x21 + x2

2)cos

(

arctan

(

−x3√

x21 + x2

2

))

Jz55 =x1

(x21 + x2

2)cos

(

arctan

(

−x3√

x21 + x2

2

))

Jz56 =0

Jz61 =−2x1x6 + x3x4

(x21 + x2

2 + x23)√

x21 + x2

2

− 2x1−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)

2√

x21 + x2

2

−

x1−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)(x

21 + x2

2)3/2

Jz62 =−2x2x6 + x3x5

(x21 + x2

2 + x23)√

x21 + x2

2

− 2x2−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)

2√

x21 + x2

2

−

x2−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)(x

21 + x2

2)3/2


Jz63 =x1x4 + x2x5

(x21 + x2

2 + x23)√

x21 + x2

2

− 2x3−x6(x

21 + x2

2) + x3(x1x4 + x2x5)

(x21 + x2

2 + x23)

2√

x21 + x2

2

Jz64 =x3x1

(x12 + x2

2 + x32)√

x12 + x2

2

Jz65 =x3x2

(x12 + x2

2 + x32)√

x12 + x2

2

Jz66 = −√

x12 + x2

2

x12 + x2

2 + x32

A.2 Transformations from xcar to zmsc 95

A.2 Transformations from xcar to zmsc

x1 = x =cos(z2) cos(z3)

z1

x2 = y =sin(z2) cos(z3)

z1

x3 = z = − sin(z3)

z1

x4 = x =z4 cos(z2) cos(z3) − z5 sin(z2) − z6 cos(z2) sin(z3)

z1

x5 = y =z4 sin(z2) cos(z3) + z5 cos(z2) − z6 sin(z2) sin(z3)

z1

x6 = z =−z4 sin(z3) − z6 cos(z3)

z1

The Jacobian is defined by

Jx(z1, z2, ..., z6) =

∂x1

∂z1

∂x1

∂z2

. . . ∂x1

∂z6

∂x2

∂z1

∂x2

∂z2. . . ∂x2

∂z6

......

. . ....

∂x6

∂z1

∂x6

∂z2. . . ∂x6

∂z6

=

Jx11 Jx12 . . . Jx16

Jx21 Jx22 . . . Jx26

......

. . ....

Jx61 Jx62 . . . Jx66

where

Jx11 = −cos(z2) cos(z3)

z12

Jx12 = − sin(z2) cos(z3)

z1

Jx13 = −cos(z2) sin(z3)

z1

Jx14 = Jx15 = Jx16 = 0

Jx21 = − sin(z2) cos(z3)

z12

Jx22 =cos(z2) cos(z3)

z1

Jx23 = − sin(z2) sin(z3)

z1

Jx24 = Jx25 = Jx26 = 0


Jx31 =sin(z3)

z12

Jx32 = 0

Jx33 = −cos(z3)

z1

Jx34 = Jx35 = Jx36 = 0

Jx41 =−z4 cos(z2) cos(z3) + z5 sin(z2) + z6 cos(z2) sin(z3)

z12

Jx42 = −z4 sin(z2) cos(z3) + z5 cos(z2) − z6 sin(z2) sin(z3)

z1

Jx43 = −z4 cos(z2) sin(z3) + z6 cos(z2) cos(z3)

z1

Jx44 =cos(z2) cos(z3)

z1

Jx45 = − sin(z2)

z1

Jx46 = −cos(z2) sin(z3)

z1

Jx51 =−z4 sin(z2) cos(z3) − z5 cos(z2) + z6 sin(z2) sin(z3)

z12

Jx52 =z4 cos(z2) cos(z3) − z5 sin(z2) − z6 cos(z2) sin(z3)

z1

Jx53 = −z4 sin(z2) sin(z3) + z6 sin(z2) cos(z3)

z1

Jx54 =sin(z2) cos(z3)

z1

Jx55 =cos(z2)

z1

Jx56 = − sin(z2) sin(z3)

z1

A.2 Transformations from xcar to zmsc 97

Jx61 =z4 sin(z3) + z6 cos(z3)

z12

Jx62 = 0

Jx63 =−z4 cos(z3) + z6 sin(z3)

z1

Jx64 = − sin(z3)

z1

Jx65 = 0

Jx66 = −cos(z3)

z1

Appendix B

The Coordinated TurnModel in CartesianCoordinates

In this appendix the equations of motion is derived for the case when the platformor the target are performing coordinated turns (CT) in the ξη-plane. First thespecial case where only the platform maneuvers is studied. Thereafter a moregeneral expression is derived where both the platform and the target may or maynot maneuver.

B.0.1 Platform maneuvers

Consider the case where the platform is performing a coordinated turn with an-gular velocity ωu and the target travels along a straight line. Figure B.1 showsthis scenario where the target and the platform are depicted for two instances oftime, tk and tk+1.

First consider the target flying along a straight line trajectory. Let xtk be the

state vector of the target in time tk including position and veloity in Cartesiancoordinates. The equation of motion for the target can be described as

xtk+1 =

1 0 0 T 0 0

0 1 0 0 T 0

0 0 1 0 0 T

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

︸︷︷︸

Acv

xtk

98

99

UAVk

UAVk+1

targetktargetk+1xk

xk+1

ξ

ξ

η

η

Figure B.1. The target and UAV depicted at time tk and tk+1. The target travels alonga straight line and the UAV is performing a coordinated turn. The state vector are alsoshown.

Consider the UAV flying according to a coordinates turn in the ξη-plane andlet xu

k be the state vector in time tk. The equation of motion for the UAV can bedescrided as [4]

xuk+1 =

1 0 0 TSW −TCW 0

0 1 0 TCW TSW 0

0 0 1 0 0 T

0 0 0 cos(wT ) − sin(wT ) 0

0 0 0 sin(wT ) cos(wT ) 0

0 0 0 0 0 1

︸︷︷︸

Act

xuk

where

SW =sin(ωT )

ωT

CW =1 − cos(ωT )

ωT

The state vector in the filter is given by xk = xtk − xu

k . The time update of thestate vector then becomes

xk+1 = xtk+1 − xu

k+1 = Acvxtk − Actx

uk

expressed in the old coordinate system. Since the UAV has turned the coordinatesystem has been rotates. Denoting the state vector expressed in the new coordinatesystem by xn

k+1 givesxn

k+1 = Rctxk+1

100 The Coordinated Turn Model in Cartesian Coordinates

Rct =

cos(wT ) − sin(wT ) 0 0 0 0

sin(wT ) cos(wT ) 0 0 0 0

0 0 1 0 0 0

0 0 0 cos(wT ) − sin(wT ) 0

0 0 0 sin(wT ) cos(wT ) 0

0 0 0 0 0 1

Altoghter this gives

xnk+1 = Rctxk+1 = Rct(Acvxt

k + Actxuk) (B.1)

Evaluating Acvxtk + Actx

uk with time index k surpressed, gives

101

Acvxt + Actxu =

xt1 + Txt

4

xt2 + Txt

5

xt3 + Txt

6

xt4

xt5

xt6

−

xu1 + TSWxu

4 − TCWxu5

xu2 + TCWxu

4 + TSWxu5

xu3 + Txu

6

cos(wT )xu4 − sin(wT )xu

5

sin(wT )xu4 + cos(wT )xu

5

xu6

=

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 T

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 1

x +

Txt4 − TSWxu

4 + TCWxu5

Txt5 − TCWxu

4 − TSWxu5

0

xt4 − cos(wT )xu

4 + sin(wT )xu5

xt5 − sin(wT )xu

4 − cos(wT )xu5

0

=

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 T

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 1

x +

Txt4 − Txu

4 + Txu4 − TSWxu

4 + TCWxu5

Txt5 − Txu

5 + Txu5 − TCWxu

4 − TSWxu5

0

xt4 − xu

4 + xu4 − cos(wT )xu

4 + sin(wT )xu5

xt5 − xu

5 + xu5 − sin(wT )xu

4 − cos(wT )xu5

0

=

1 0 0 T 0 0

0 1 0 0 T 0

0 0 1 0 0 T

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

x +

Txu4 − TSWxu

4 + TCWxu5

Txu5 − TCWxu

4 − TSWxu5

0

xu4 − cos(wT )xu

4 + sin(wT )xu5

xu5 − sin(wT )xu

4 − cos(wT )xu5

0

The UAV is momentarily travelling along the ξ-axis with speed v gives xu4 = v

102 The Coordinated Turn Model in Cartesian Coordinates

and xu5 = xu

6 = 0. Totally this gives the equations of motion as

xnk+1 = Rct

1 0 0 T 0 0

0 1 0 0 T 0

0 0 1 0 0 T

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

︸︷︷︸

Acv

xk+Rct

Tv − TSWv

−TCWv

0

v − cos(wT )v

− sin(wT )v

0

︸︷︷︸

uct

= RctAcvxk+Rctuct

Finally process noise is added giving

xnk+1 = Rct

(Acvxk + uct + Bwwk

)

B.0.2 Platform and Target Maneuver

Now consider the case where both the UAV and the target are allowed to maneuver,according to

xtk+1 = Atxt

k

xuk+1 = Auxu

k

where At and Au can be Acv or Act depending on whether the UAV and targetare maneuvering or not. In the case of both the target and the UAV maneuver,they may have different angular velocities, ωt and ωu respectivly.

This gives according to equation B.1

xnk+1 = R(Atxt

k − Auxuk) = R(Atxt

k − Atxuk + Atxu

k − Auxuk)

R[Atxk + (At − Au)xuk ]

xuk = ( 0 0 0 v 0 0 )T

where R = Rct if the platform maneuvers and R = I if the platform travels withconstant velocity and I denotes the identity matrix. Adding process noise givesthe final expression

xnk+1 = R[Atxk + (At − Au)xu

k + Bwwk]

Appendix C

Initialization of P with thePropagation of UncertaintyFormula

This appendix describe the initialization of the prediction covariance matrix Pwith the use of the propagation of uncertainty formula, also refered to as Gauss’approximation theorem. The variance of the error in the initial state vector de-pends of the scenario studied. To specify a scenario the target’s initial position inazimuth and elevation and the direction of the target’s velocity vector has to beset. Expressions for the initial P will first be derived for the case where the targetstarts ahead of the UAV (θ0 = 0 and ϕ0 = 0), and thereafter the numerical valuesfor Scenario A will be inserted. In Table C.1 a summary of the parameters arepresented together with the values used in Scenario A.

Table C.1. Parameters needed to be set to define a scenario.

Parameter Description Scenario A

ϕ0 Initial azimuth angle 0

θ0 Initial elevation angle 0

α Azimuth for the target’s velocity vector 2

β Elevation for the target’s velocity vector 2

To initialize P the initial covariances are needed to be calculated, i.e the co-variances in the error of the initial state vector x. The initial errors in the statesare assumed to be independent, meaning that P can be initialized as a diagonalmatrix with the state’s variances on the main diagonal.

The angles ϕ and θ is initialized with the first measurements, hence the vari-ances of these states are the measurement noise varϕ and varθ. The expectation

103

104 Initialization of P with the Propagation of Uncertainty Formula

values of the angles are the true initial angles mϕ = ϕ0 and mθ = θ0. α and βare initialized as zero. To use the formula they are considered to be stochasticvariables with mean mα = αinit = 0, mβ = βinit = 0 and variances varα

1 andvarβ . During the Monte Carlo simulations the initial error in range and speedare simulated as Gaussian noise with expectation values mr and mv and variancesvarr and varv respectivly. The approximate variances for all states should beexpressed with these quantities summerazed in Table C.2.

Table C.2. Expectation values and variances used to initialize P .

Parameter Expectation value Varianceϕ mϕ = ϕ0 varϕ = (0.1mrad)2

θ mθ = θ0 varθ = (0.1mrad)2

α mα = 0 varα = (0.04rad)2

β mβ = 0 varβ = (0.04rad)2

r mr = 3km varr = (300m)2

v mv = 100m/s varv = (10m/s)2

Let g be a function of the random variables X1, . . . , Xn with expectation valuesm1, . . . , mn. According to Gauss approximation theorem described in for instance[8] the variance of function g can be approximated by

V ar[g(X1, . . . , Xn)] ≈n∑

i=1

V ar(Xi)

(∂g

∂mi

)2

+ 2∑

i<j

Cov(Xi, Xj)∂g

∂mi

∂g

∂mj

(C.1)

If X1, · · · , Xn are independent the covariances equal zero. In this case (C.1) canbe simplified to

V ar[g(X1, . . . , Xn)] ≈n∑

i=1

V ar(Xi)

(∂g

∂mi

)2

(C.2)

With (C.2) the variance of the first three states can be expressed as

V ar(z1) = V ar

(1

r

)

≈ V ar(r)

(∂z1

∂r

)2

= varr1

m4r

V ar(z2) = V ar(ϕ) = varϕ (C.3)

V ar(z3) = V ar(θ) = varθ

(C.4)

1The error in the initialization of α and β are used to find numerical values of the variance.For Scenario varα = varβ = (0.04rad)2 = (2)2 is used.

105

To find the variances for the states describing the velocity, first express thevelocity vector in it components.

r = v cosα cosβ

ϕ =v sin α cosβ

r(C.5)

θ =v sin β

r

Note that according to the definition of α and β (C.5) is valid only in the case ofθ0 = 0 and ϕ0 = 0.

With (C.2) and (C.5) the variances of the velocity components are approxi-mated by

V ar(r) ≈ V ar(v)

(∂r

∂v

)2

+ V ar(α)

(∂r

∂α

)2

+ V ar(β)

(∂r

∂β

)2

= varv cos2(mα) cos2(mβ) + varαm2v sin2(mα) cos2(mβ)

+ varβm2v cos2(mα) sin2(mβ)

V ar(ϕ) ≈ V ar(v)

(∂ϕ

∂v

)2

+ V ar(r)

(∂ϕ

∂r

)2

+ V ar(α)

(∂ϕ

∂α

)2

+ V ar(β)

(∂ϕ

∂β

)2

= varvsin2(mα) cos2(mβ)

m2r

+ varrm2

v sin2(mα) cos2(mβ)

m4r

+ varαm2

v cos2(mα) cos2(mβ)

m2r

+ varβm2

v sin2(mα) sin2(mβ)

m2r

(C.6)

V ar(θ) ≈ V ar(v)

(

∂θ

∂v

)2

+ V ar(r)

(

∂θ

∂r

)2

+ V ar(β)

(

∂θ

∂β

)2

= varvsin2(mβ)

m2r

+ varrm2

v sin2(mβ)

m4r

+ varβm2

v cos2(mβ)

m2r

By combining (C.2) with (C.6) the variances for the fourth and fifth states can

106 Initialization of P with the Propagation of Uncertainty Formula

be approximated by

V ar(z4) = V ar

(r

r

)

≈ V ar(r)

(∂z4

∂r

)2

+ V ar(r)

(∂z4

∂r

)2

= varvcos2(mα) cos2(mβ)

m2r

+ varαm2

v sin2(mα) cos2(mβ)

m2r

+ varβm2

v cos2(mα) sin2(mβ)

m2r

+ varrm2

v cos2(mα) cos2(mβ)

m4r

(C.7)

V ar(z5) = V ar(Ω) = V ar(ϕ cos θ) ≈ V ar(ϕ)

(∂z5

∂ϕ

)2

+ V ar(θ)

(∂z5

∂θ

)2

= varvsin2(mα) cos2(mβ) cos2(mθ)

m2r

+ varrm2

v sin2(mα) cos2(mβ) cos2(mθ)

m4r

+ varαm2

v cos2(mα) cos2(mβ) cos2(mθ)

m2r

+ varβm2

v sin2(mα) sin2(mβ) cos2(mθ)

m2r

+ varθm2

v sin2(mα) cos2(mβ) sin2(mθ)

m2r

V ar(z6) = V ar(θ) ≈ varvsin2(mβ)

m2r

+ varrm2

v sin2(mβ)

m4r

+ varβm2

v cos2(mβ)

m2r

Finally using the numerical values of Table C.1 and Table C.2 P is initializedfor Scenario A as

P = diag( 1.1 · 10−9 10−8 10−8 2.2 · 10−5 1.8 · 10−6 1.8 · 10−6 )

Appendix D

Simulation Scenarios

This appendix describes the scenarios used in the thesis. Each scenario is describedwith a figure depicting the scenario. In the figures both the target and the UAV aretravelling along to straight line trajectories for simplicity. During the simulationboth target and UAV might maneuver. In Table D.1 the parameters set for thescenarios are presented.

Table D.1. Parameters for the scenarios.

Par A B C Dϕ0 0 30 0 0

θ0 0 0 0 10

α 2 5 0 0

β 2 0 0 −2

Scenario A is used in most simulations. It is also described in Section 5.3.Figure D.1 shows the trajectory for the target and the UAV.

Scenario B is used in the Monte Carlo simulations and in Section 6.2.3 whereright and left S-turns are compared as platform maneuvers. The trajectories forthe scenario can be found in Figure D.2.

In Scenario C the angular velocity is zero meaning that ttg is unobservable. Ifneither the target or the UAV maneuver this scenario will end in a collision. Thescenario is used in the Monte Carlo simulations and in Section 6.2.4. Figure D.3shows the scenario.

In Scenario D the target travels out of sight after only 7.2 seconds. The sce-nario is only used in the Monte Carlo simulations for a non-maneuvering target.Figure D.4 shows the scenario.

107

108 Simulation Scenarios

0 500 1000 1500 2000 2500 3000−80

−60

−40

−20

0

20

ξ

η

UAVTarget

0 500 1000 1500 2000 2500 3000−80

−60

−40

−20

0

20

ξ

ζ

Scenario A

Figure D.1. Trajectories for the target and UAV in Scenario A.

0 500 1000 1500 2000 2500 3000

0

500

1000

1500

ξ

η

UAVTarget

0 500 1000 1500 2000 2500 3000−50

0

50

ξ

ζ

Scenario B

Figure D.2. Trajectories for the target and UAV in Scenario B.

109

0 500 1000 1500 2000 2500 3000−500

0

500

ξ

η

UAVTarget

0 500 1000 1500 2000 2500 3000−50

0

50

ξ

ζ

Scenario C

Figure D.3. Trajectories for the target and UAV in Scenario C.

0 500 1000 1500 2000 2500 3000−300

−200

−100

0

100

200

300

ξ

η

UAVTarget

0 500 1000 1500 2000 2500 3000−600

−500

−400

−300

−200

−100

0

100

ξ

ζ

Scenario D

Figure D.4. Trajectories for the target and UAV in Scenario D.

Appendix E

RMSE Plots Showing theInfluence of ∆r and ∆v

This appendix includes rmse plots from simulations studying the influence of ∆rand ∆v. In Section 6.4 Scenario A were simulated for different values of ∆rwith ∆v = 0. In this appendix plots showing the same scenario is shown with∆v = ±10m/s.

Figure E.1 shows RMSE for rrel and ttg where ∆v = −10m/s and Figure E.2shows RMSE for rrel and ttg where ∆v = 10m/s.

RMSE for rrel is affected of the initial error in velocity, but the influence issmall. The influence of the estimates of ttg on the other hand is large. Since ∆ris positive, RMSE for ttg is smaller for positive ∆v than negative. For the case of∆v = 10m/s RMSE for ttg is smaller if ∆r = 200m than if ∆r = 0m.

110

111

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

8

10

RM

SE

∆ r=0

∆ r=200

∆ r=600

∆ r=1200

0 2 4 6 8 10 12 14 16 180

2

4

6

8

10

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

8

10

RM

SE

time [s]time [s]







Figure E.1. Rmse in rrel and ttg for different values of ∆r. Scenario A with ∆v = −10.Comparison of platform maneuver with ti = 5s (top), ti = 1s (middle) and no platformmaneuver (bottom).

112 RMSE Plots Showing the Influence of ∆r and ∆v

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

0.2

0.4

0.6

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

RM

SE

∆ r=0

∆ r=200

∆ r=600

∆ r=1200

0 2 4 6 8 10 12 14 16 180

2

4

6

RM

SE

0 2 4 6 8 10 12 14 16 180

2

4

6

RM

SE

time [s]time [s]







Figure E.2. Rmse in rrel and ttg for different values of ∆r. Scenario A with ∆v = 10.Comparison of platform maneuver with ti = 5s (top), ti = 1s (middle) and no platformmaneuver (bottom).

Appendix F

Monte Carlo Simulations fora Non-Maneuvering Target

This appendix includes the rmse plots for all states from the Monte Carlo Simula-tions when the target is travelling with constant velocity. RMSE plots for rrel andttg are shown in Chapter 6 Scenario A-D were simulated when the platform wasnon-maneuvering and when the platform maneuvered with ti = 1s and ti = 5srespectivly. For each case 200 simulation has been made. ∆r and ∆v has beensimulated as Gaussian noise with mean zero and standard deviation 300m and10m/s respectivly.

Figure F.1, F.2 and F.3 show rmse for Scenario A. The simulations are ter-minated after 18.3 seconds because the target is not in the field of vision of theUAV.

Figure F.4, F.5 and F.6 show rmse for Scenario B. At time 17.9 seconds thetarget is as closest to the UAV, CPA is reached, thereafter the target is travellingaway from the UAV.

In Scenario C the angular velcity is zero leading to a collision. Figure F.7, F.8and F.9 show rmse for this scenario. In this case it is necessary that the UAVmaneuver to get satisfying estimates.

Figure F.10, F.11 and F.12 show rmse for Scenario D. After only 7.2 secondsthe target has disappeared from the field of vision.

113

114 Monte Carlo Simulations for a Non-Maneuvering Target

0 2 4 6 8 10 12 14 16 180

1

2

3

4

5x 10

−3

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θrr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.1. RMSE (dark) and σ (bright) for the state vector. Scenario A. The platformis not maneuvering.

0 2 4 6 8 10 12 14 16 180

1

2

3

4

5x 10

−3

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.2. RMSE (dark) and σ (bright) for the state vector. Scenario A. The platformis maneuvering with ti = 1s.

115

0 2 4 6 8 10 12 14 16 180

1

2

3

4

5x 10

−3

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.05

0.1

0.15

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θrr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.3. RMSE (dark) and σ (bright) for the state vector. Scenario A. The platformis maneuvering with ti = 5s.

0 5 10 15 200

0.5

1

1.5x 10

−3

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.05

0.1

0.15

0 5 10 15 200

0.01

0.02

0.03

0.04

0.05

0 5 10 15 20 250

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.4. RMSE (dark) and σ (bright) for the state vector. Scenario B. The platformis not maneuvering.


0 5 10 15 200

0.5

1

1.5x 10

−3

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.05

0.1

0.15

0 5 10 15 200

0.01

0.02

0.03

0.04

0.05

0 5 10 15 200

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θrr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.5. RMSE (dark) and σ (bright) for the state vector. Scenario B. The platformis maneuvering with ti = 1s.

0 5 10 15 200

0.5

1

1.5x 10

−3

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.002

0.004

0.006

0.008

0.01

0 5 10 15 200

0.05

0.1

0.15

0 5 10 15 200

0.01

0.02

0.03

0.04

0.05

0 5 10 15 200

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.6. RMSE (dark) and σ (bright) for the state vector. Scenario B. The platformis maneuvering with ti = 5s.

117

0 2 4 6 8 10 12 14 16 180

0.005

0.01

0.015

0.02

0 2 4 6 8 10 12 14 16 180

0.02

0.04

0.06

0.08

0.1

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

0 2 4 6 8 10 12 14 16 180

0.5

1

1.5

2

1r

ϕ

θrr

Ω θ

time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.7. RMSE (dark) and σ (bright) for the state vector. Scenario C. The platformis not maneuvering.

0 2 4 6 8 10 12 14 16 180

0.005

0.01

0.015

0.02

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.8. RMSE (dark) and σ (bright) for the state vector. Scenario C. The platformis maneuvering with ti = 1s.


0 2 4 6 8 10 12 14 16 180

0.005

0.01

0.015

0.02

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.002

0.004

0.006

0.008

0.01

0 2 4 6 8 10 12 14 16 180

0.1

0.2

0.3

0.4

0.5

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

0 2 4 6 8 10 12 14 16 180

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θrr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.9. RMSE (dark) and σ (bright) for the state vector. Scenario C. The platformis maneuvering with ti = 5s.

0 1 2 3 4 5 6 70

0.2

0.4

0.6

0.8

1x 10

−3

0 1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

0 1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

0 1 2 3 4 5 6 70

0.05

0.1

0.15

0 1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

0 1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.10. RMSE (dark) and σ (bright) for the state vector. Scenario D. Theplatform is not maneuvering.

119

0 1 2 3 4 5 6 70

0.2

0.4

0.6

0.8

1x 10

−3

0 1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

0 1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

0 1 2 3 4 5 6 70

0.05

0.1

0.15

0 1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

0 1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θrr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.11. RMSE (dark) and σ (bright) for the state vector. Scenario D. Theplatform is maneuvering with ti = 1s.

1 2 3 4 5 6 70

0.2

0.4

0.6

0.8

1x 10

−3

1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

1 2 3 4 5 6 70

0.002

0.004

0.006

0.008

0.01

1 2 3 4 5 6 70

0.05

0.1

0.15

1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

1 2 3 4 5 6 70

0.01

0.02

0.03

0.04

0.05

1r

ϕ

θ rr

Ω θ

time [s]time [s]

[m−

1]

[rad]

[rad]

[s−

1]

[rad/s]

[rad/s]

Figure F.12. RMSE (dark) and σ (bright) for the state vector. Scenario D. Theplatform is maneuvering with ti = 5s.

Upphovsrätt

Detta dokument hålls tillgängligt på Internet — eller dess framtida ersättare —under 25 år från publiceringsdatum under förutsättning att inga extraordinäraomständigheter uppstår.

Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner,skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för icke-kommersiell forskning och för undervisning. Överföring av upphovsrätten vid ensenare tidpunkt kan inte upphäva detta tillstånd. All annan användning av doku-mentet kräver upphovsmannens medgivande. För att garantera äktheten, säker-heten och tillgängligheten finns det lösningar av teknisk och administrativ art.

Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsmani den omfattning som god sed kräver vid användning av dokumentet på ovan be-skrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan formeller i sådant sammanhang som är kränkande för upphovsmannens litterära ellerkonstnärliga anseende eller egenart.

För ytterligare information om Linköping University Electronic Press se för-lagets hemsida http://www.ep.liu.se/

Copyright

The publishers will keep this document online on the Internet — or its possi-ble replacement — for a period of 25 years from the date of publication barringexceptional circumstances.

The online availability of the document implies a permanent permission foranyone to read, to download, to print out single copies for his/her own use andto use it unchanged for any non-commercial research and educational purpose.Subsequent transfers of copyright cannot revoke this permission. All other uses ofthe document are conditional on the consent of the copyright owner. The publisherhas taken technical and administrative measures to assure authenticity, securityand accessibility.

According to intellectual property law the author has the right to be mentionedwhen his/her work is accessed as described above and to be protected againstinfringement.

For additional information about the Linköping University Electronic Pressand its procedures for publication and for assurance of document integrity, pleaserefer to its www home page: http://www.ep.liu.se/

c© Tina Erlandsson

Passive ranging

Documents

Transcript of Passive ranging