Performance Enhancement of Bearing Navigation to Known Radio Beacons

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Performance Enhancement of Bearing Navigation to

Known Radio Beacons

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

av

Erik Erkstam & Emil Tjernqvist LiTH-ISY-EX--12/4596--SE

Linköping 2012

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

Performance Enhancement of Bearing Navigation to

Known Radio Beacons

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan vid Linköpings universitet

av

Erik Erkstam & Emil Tjernqvist LiTH-ISY-EX--12/4596--SE

Handledare: Anders Johansson foi

Zoran Sjanic

isy_{, Linköpings universitet} Examinator: Fredrik Gustafsson

isy, Linköpings universitet

Avdelning, Institution Division, Department

Division of automatic control Department of Electrical Engineering SE-581 83 Linköping Datum Date 2012-06-15 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.ep.liu.se

ISBN — ISRN

LiTH-ISY-EX--12/4596--SE Serietitel och serienummer Title of series, numbering

ISSN —

Titel Title

Prestandaförbättring av navigering efter bäring mot kända radiofyrar Performance Enhancement of Bearing Navigation to Known Radio Beacons

Författare Author

Erik Erkstam & Emil Tjernqvist

Sammanfattning Abstract

This master thesis investigates the performance of a car navigation system using lateral ac-celerometers, yaw rate and bearings relative three known radio beacons. Accelerometer, gyroscope and position data has been collected by an imu combined with a gps receiver, where the imu was installed in the approximate motion center of a car. The bearing mea-surements are simulated using gps data and the measurement noise model is derived from an experiment where the direction of arrival to one transmitter was estimated by an antenna array and the signal processing algorithm music.

The measurements are fused in a multi-rate extended Kalman filter which assumes that all measurement noise is Gaussian distributed. This is not the case for the bearing measurement noise which contains outliers and therefore is modelled as a Gaussian uniform noise mixture. Different methods to deal with this have been investigated where the main focus is on the principle to use the Kalman filter’s innovation for each bearing measurement as an indication of its quality and discarding measurements with a quality above a certain threshold.

Nyckelord

Abstract

This master thesis investigates the performance of a car navigation system using lateral accelerometers, yaw rate and bearings relative three known radio beacons. Accelerometer, gyroscope and position data has been collected by an imu com-bined with a gps receiver, where the imu was installed in the approximate mo-tion center of a car. The bearing measurements are simulated using gps data and the measurement noise model is derived from an experiment where the direction of arrival to one transmitter was estimated by an antenna array and the signal processing algorithm music.

The measurements are fused in a multi-rate extended Kalman filter which as-sumes that all measurement noise is Gaussian distributed. This is not the case for the bearing measurement noise which contains outliers and therefore is modelled as a Gaussian uniform noise mixture. Different methods to deal with this have been investigated where the main focus is on the principle to use the Kalman fil-ter’s innovation for each bearing measurement as an indication of its quality and discarding measurements with a quality above a certain threshold.

Acknowledgments

We would like to thank our supervisor Anders Johansson for giving us the opper-tunity to conduct this thesis at foi and for all the help he has given us. Without his input, his effort and his experience this thesis would not have been possible. The same thank you goes to our supervisor Zoran Sjanic at Linköping University and our examinator Fredrik Gustafsson.

We would also like to give a special thank you to Patrik Hedström and Rolf Gustafsson at foi. The help they have given us concerning numerous things have not gone unnoticed.

Linköping, June 2012 Erik Erkstam & Emil Tjernqvist

Contents

Notation ix 1 Introduction 1 1.1 Problem Description . . . 2 1.2 Approach . . . 2 1.3 Background . . . 3 1.4 Goal . . . 3 1.5 Thesis Outline . . . 3 2 Theory 5 2.1 Earth Reference Frames . . . 5

2.1.1 The Ellipsoidal Earth Model WGS84 . . . 6

2.1.2 Conversion to the Planar Local Coordinates RT90 . . . 6

2.2 The Main Coordinate Frames . . . 8

2.3 Brief Introduction to Inertial Navigation . . . 8

2.4 GPS . . . 9

2.4.1 Principles of GPS Positioning . . . 10

2.5 Bearing Sensor . . . 11

2.5.1 DOA Estimation Through the MUSIC Algorithm . . . 11

2.5.2 Regarding MUSIC Eigenvalue Fractions . . . 14

2.6 Nonlinear Filtering . . . 14

2.6.1 Bayes Recursive Filter Equations . . . 15

2.6.2 The Kalman Filter . . . 16

2.6.3 The Extended Kalman Filter . . . 17

3 Modelling 19 3.1 State Vector . . . 19

3.2 Process Model . . . 19

3.2.1 A More Advanced Motion Model . . . 21

3.3 Measurement Model . . . 22

3.3.1 Bearing Measurement Equation . . . 23

3.4 Bearing Measurement Generation . . . 24

3.5 The Bearing Measurement Noise Model . . . 25 vii

viii CONTENTS

4 Implementation 29

4.1 Multirate Extended Kalman Filter . . . 29

4.2 Regarding Settings of Q and R . . . 31

5 Method 33 5.1 MUSIC Eigenvalue Fractions . . . 33

5.2 Bearing Measurements Update Frequency . . . 33

5.3 A More Advanced Motion Model . . . 34

5.4 Outlier Rejection With Fixed Threshold . . . 34

5.5 Outlier Rejection With Dynamic Threshold . . . 34

6 Results 37 6.1 music Eigenvalue Fractions . . . 38

6.2 Bearing Measurements Update Frequency . . . 38

6.3 A More Advanced Motion Model . . . 40

6.4 Outlier Rejection With Fixed Threshold . . . 40

6.5 Outlier Rejection With Dynamic Threshold . . . 43

7 Conclusions and Future Work 53 7.1 Conclusions . . . 53

7.2 Future Work . . . 54

Notation

Variables

Variable Meaning

x state vector

y measurement vector

β relative bearing to a beacon

p Cartesian position for a beacon

State variables

Variable Meaning

x Cartesian position x-coordinate y Cartesian position y-coordinate

ϕ yaw angle

v velocity

a acceleration

ω yaw rate

x Notation Abbreviations

Abbreviation Meaning

bpsk _{binary phase shift keying} cdma _{code division multiple acces}

ctra _{constant turn rate and acceleration} doa _{direction of arrival}

ecef _{earth-centerd, earth-fixed} eci earth-centered inertial ekf extended Kalman filter

foi Swedish Defence Research Agency gps global positioning system

grs80 geodetic reference system 1980 imu _{inertial motion unit}

ins _{inertial navigation system} music _{multiple signal classification}

navstar _{navigation satellite timing and ranging} pdf _{probability density function}

prn _{pseudo random noise}

rmse _{root mean square error}

rt90 Swedish grid 1990

1

Introduction

This thesis concerns car navigation using an integrated navigation system with an inertial motion unit (imu)1and bearings relative known radio beacons. It was conducted in order to investigate if such a navigation system could be a support navigation system to a gps-imu integrated navigation system in gps (global posi-tioning system) denied environments.

Examples of gps denied environments are urban environments, forests and envi-ronments where gps jammers operate. In electronic warfare jamming can be used as an electronic counter measure with the purpose to degrade the performance of the enemy’s navigation system [Schleher, 1986] [Josefsson and Berggren, 1997]. A gps independent navigation system can be a suitable protection against gps jammers.

In thesis [Blomfeldt and Haverstad, 2008] sensor fusion of an imu and a gps receiver for racing car navigation is investigated. The navigation system under investigation is a gps aided inertial navigation system (ins) and it is concluded that it is possible to achieve reasonable estimates of the position and the yaw angle.

Since it is known that a gps-imu integrated navigation system performs well in gps friendly environments and it is of interest to study a gps independent navigation system a study of the performance of an integrated navigation system with an imu and bearings relative known radio beacons is motivated.

This chapter presents the problem description, the problem approach, background information regarding bearings-only navigation, the goal of the the-sis and the thethe-sis outline.

1_{A list of abbreviations can be found on page x.}

2 1 Introduction

1.1

Problem Description

In this thesis the interesting car navigation parameters considered are the posi-tion, the yaw angle and some kinematic parameters of the car velocity, accelera-tion in the moaccelera-tion direcaccelera-tion of the car, acceleraaccelera-tion perpendicular to the moaccelera-tion direction of the car and angular velocity around the motion centre of the car (yaw rate).

An imu mounted in the approximate motion centre of a car measures angular ve-locities and accelerations in three dimensions. However, the car is considered to move on a two dimensional plane and therefore the gyroscope measuring the yaw rate is the only gyroscope used. The accelerometers used are the accelerometers measuring the acceleration in the motion direction of the car and the acceleration perpendicular to the motion direction of the car.

The bearings are simulated and the measurement error model is derived from an experiment where the bearing to a known transmitter were performed using a vertical dipole antenna array with four antenna elements and the array signal processing algorithm multiple signal classification (music).

The navigation parameters are estimated by fusing data collected from an imu and a bearing sensor in an extended Kalman filter and the navigation perfor-mance is evaluated using root mean square error (rmse) between a reference fil-tering and the filtered estimates. The reference filter fuses data collected from all the available sensors gps, imu and bearings in an extended Kalman filter (ekf).

1.2

Approach

The bearing measurements are shown to contain a large amount of outliers and the bearing error distribution can be modelled by a Gaussian uniform mixture distribution with 30% zero mean Gaussian noise with standard deviation of 5◦ and 70% uniformly distributed noise, see Section 3.5. In order to use the bearing measurements in an extended Kalman filter outlier rejection is crucial.

Therefore the navigation performance of an integrated navigation system with an imuand bearings relative known radio beacons using outlier rejection methods for the bearing measurements is studied in this thesis. The navigation perfor-mance is evaluated for different weight and noise variance settings of the Gaus-sian uniform mixture distribution.

An experiment is performed in order to investigate the possibility to preprocess the bearing measurements using additional data from the music algorithm.

1.3 Background 3

1.3

Background

This section presents some additional background information regarding the known difficulties with bearings-only target tracking and the limitations of three object triangulation.

Bearings-only tracking of a target submarine using a single bearings-only sensor on the ownship is investigated in [La Scala and Morelande, 2008]. The bearing measurement error is assumed to have a standard deviation of 1◦and it is stated that bearings-only tracking is known to be difficult due to the high degree of non linearity in the measurement model and the problem with target state observabil-ity.

Bearings-only tracking is analogue to bearings-only navigation, the target to track is the ego vehicle itself. However, the positions of the landmarks are fixed and assumed to be known.

Three object triangulation is the task to determine the position and orientation of a vehicle using bearings-only measurements relative the vehicles orientation to three known objects. In paper [Cohen and Koss, 1992] a robot’s position and ori-entation are estimated using geometric triangulation which is an algorithm built upon trigonometric relations. Given that the measurements provided has rea-sonable small standard deviation, this algorithm gives accurate estimates of the position and the orientation. But this algorithm only works consistently when the robot is located within the triangle that is defined by the three objects. Therefore the navigation performance is investigated in the case when the vehicle is located within an area defined by at least three radio beacons.

1.4

Goal

The goal of this thesis is to investigate the performance of an integrated naviga-tion system with an imu and bearings relative known radio beacons using perfor-mance enhancement methods to deal with the bearing measurement noise.

1.5

Thesis Outline

Chapter 2 explains the theoretical background regarding the navigation frames and it presents a brief introduction to inertial navigation, gps, the bearing sensor and nonlinear filtering. Chapter 3 describes the state space model used in this thesis. It also explains how the bearings are simulated and it presents the bearing measurement noise model.

Chapter 4 explains how the implementation was performed. Chapter 5 presents different methods with potential to improve the inertial and bearing navigation system. Chapter 6 presents the results from the experiments and Chapter 7 presents the conclusions and areas for future work.

2

Theory

This Chapter introduces the main coordinate frames used in this thesis, it gives a brief introduction to inertial navigation, gps, the bearing sensor and non linear filtering.

2.1

Earth Reference Frames

Two basic Earth reference frames are the Earth-centred inertial (eci) frame and the Earth-centred, Earth-fixed (ecef) frame.

The origin of the eci coordinate system is the centre of gravity of Earth. The basis vector ˆxecipoints in the direction of the vernal equinox, the basis vector ˆzeciis

parallel to the rotational axis of Earth pointing at north and the base vector ˆyeci

is orthogonal to the other base vectors such that the base vectors define a right-handed coordinate system. The eci frame is fixed relative fixed stars.

The origin of the ecef frame is the same as the origin of the eci frame and the base vector ˆzecefis the same as ˆzecibut the ecef coordinate system rotates with

Earth.

Coordinates in the ecef frame can also be expressed by polar coordinates using longitude, latitude and altitude. The longitude is measured positive at east and negative at west with respect to the prime meridian at Greenwich, the latitude is measured relative the equatorial plane and the altitude is measured relative the mean sea level.

There are actually several types of latitudes and one of them is the geocentric latitude. It is defined by the angle between a line from the centre of Earth to the reference point and the equatorial plane [Grewal et al., 2011].

6 2 Theory

2.1.1

The Ellipsoidal Earth Model WGS84

wgs84 is an acronym for the world geodetic system and it is an international standard for navigation coordinates, the number 84 refers to a reference Earth model released in 1984. This model approximates the mean sea level of Earth by an ellipsoid with a rotational axis that coincides with the rotational axis of Earth. The ellipsoid rotates with Earth, its centre coincides with the centre of gravity of Earth and the prime meridian is the Greenwich meridian. The semi major axis (corresponds to the equatorial radius) is defined to have the length 6378137 m and the semi minor axis (corresponds to the polar radius) defined to have the length 6356752 m.

The longitude in a geodetic coordinate system is the the same as longitude in ecef coordinates. The geodetic latitude is defined by the angle between the equatorial plane and the normal of the reference ellipsoid surface and can differ up to 12 arc minutes relative the geocentric latitude [Grewal et al., 2011].

2.1.2

Conversion to the Planar Local Coordinates RT90

The ellipsoidal model wgs84can deviate several hundred meters from a geocen-tric model, locally. Therefore, locally adapted coordinate systems are preferred. A planar locally adapted coordinate system for Sweden is rt90 (Swedish grid 1990).

To translate wgs84coordinates to rt90coordinates a map projection called Gauss conformal projection is done. Gauss conformal projection maps an ellipsoid to a cylinder using the Krüger formulas [Lantmäteriet, b]. In this conversion the ref-erence ellipsoid grs80(geodetic reference system 1980) is used with parameters

a = 6378137 m, f = 298.257222101, k0 = 1.00000561024, FE = 1500064.274 m

and FN = −667.711 m. [Lantmäteriet, a]. The following variables are defined.

a = semi major axis of the ellipsoid f = flattening of the ellipsoid e2= first eccentricity squared

φ = geodetic latitude, positive north

˜

λ = geodetic longitude, positive east

x= grid coordinate, positive north y= grid coordinate, positive east ˜

λ0= longitude of the central meridian

k0= scale factor along the central meridian

δ ˜λ = ˜λ − ˜λ0

FN = false northing FE = false easting

2.1 Earth Reference Frames 7

From a and f the variables e2, n and ˆa are calculated, (2.1)-(2.3).

e2 = f (2 − f ) (2.1) n = f 2 − f (2.2) ˆa = a 1 + n(1 + 1 4n 2₊ 1 64n 4_{+ . . .) (2.3)}

The computation of the conformal latitude is done according to (2.4).

φ∗= φ − sin φ cos φ(A + B sin2φ + C sin4φ + D sin6φ + . . .) (2.4)

Calculation of the coefficients A, B, C, D, ξ0

and η0 is done according to (2.5)-(2.10). A= e2 (2.5) B= 1 6(5e 4_−e6_{) (2.6)} C= 1 120(104e 6_−45e8_{+ . . .) (2.7)} D= 1 1260(1237e 8_{+ . . .) (2.8)} ξ0= arctantan φ ∗ cos δ ˜λ (2.9)

η0= arctanh cos φ∗sin δ ˜λ (2.10)

Finally the position can be calculated according to (2.11) and (2.12), where the coefficients b1, b2, b3and b4are calculated according to (2.13)-(2.16).

x= k0ˆa(ξ 0 + b1sin 2ξ 0 cosh 2η0+ b2sin 4ξ 0 cosh 4η0 + b3cos 6ξ 0 sinh 6η0+ b3cos 8ξ 0 sin 8η0+ . . .) + FN (2.11) y= k0ˆa(η 0 + b1cos 2ξ 0 sinh 2η0+ b2cos 4η 0 sinh 4η0 + b3cos 6ξ 0 sinh 6η0+ b3cos 8ξ 0 sin 8η0+ . . .) + FE (2.12)

8 2 Theory b₁= 1 2n − 2 3n 2₊ 5 16n 3 41 180n 4_{+ . . . (2.13)} b₂= 13 48n 2₋3 5n 3₊ 557 1440n 4_{+ . . . (2.14)} b₃= 61 240n 3₋103 140n 4_{+ . . . (2.15)} b₄= 49561 161280n 4_{+ . . . (2.16)}

2.2

The Main Coordinate Frames

The main coordinate frames used in this thesis are the world frame (W), the navi-gation frame (N) and the body frame (B).

The world frame is the same as the rt90coordinate frame. The base vector ˆx_W points at north, ˆyWpoints at east and ˆzWpoints downward.

The navigation frame has its origin in the motion centre of the vehicle but it does not rotate with the vehicle. The base vectors ˆxN, ˆyN and ˆzN point in the same

direction as the world frame.

The body frame is fixed in the vehicle and follows the vehicle’s rotation. The base vector ˆxB point in the vehicle’s forward direction, ˆzB points downward and ˆyB

is orthogonal to the other base vectors such that the base vectors define a right-handed coordinate system.

The relationship between these coordinate frames are further explained in Sec-tion 3.3.1.

2.3

Brief Introduction to Inertial Navigation

The point of departure in inertial navigation is Newton’s first law (or law of mo-tion). The first law states that inertia is the propensity of bodies to maintain con-stant translational and rotational velocity, unless disturbed by forces or torques, respectively.

A coordinate frame where Newton’s laws of motion hold is defined as an inertial reference frame. An inertial reference frame is neither accelerating nor rotating and is not necessarily the same as navigation coordinates. A typical navigation frame accelerates (to counter gravity) and rotates with Earth, therefore the gravi-tational force and the Coriolis effect must be taken into account when implement-ing inertial navigation systems.

Consider a body with no applied force, from Newton’s first law it is known that the body maintains constant velocity in a non rotating coordinate frame. How-ever, in rotating coordinate frames the body experiences an additional apparent acceleration called the Coriolis effect. The Coriolis effect can be written as (2.17),

2.4 GPS 9

where ω is the rotation rate vector and vrotating is the velocity of the body

mea-sured in rotating coordinates.

acoriolis= −2ω × vrotating (2.17)

Newton’s second law states that a body’s acceleration (a) is proportional to the specific force (F ) applied to it and inversely proportional to its mass (m), F = ma. The acceleration can be measured by accelerometers using this principle.

A gyroscope (or gyro) is a sensor for measuring the rotation. A displacement gyro measures the accumulated rotation angle and a rate gyro measures the rotation rate.

With knowledge of the initial navigation parameters position, velocity, acceler-ation, heading and heading rate it is possible to estimate the current value of the navigation parameters using measured values of the acceleration and the heading rate. This technique is called dead-reckoning and it is popular in mil-itary applications because it is autonomous and does not depend on outer signals for navigation. On the other hand the position error increases unbounded over time [Grewal et al., 2011].

An electronic inertial measurement unit contains at least accelerometers, gyro-scopes, AD-converters, a processor and a flash memory. The sensors are com-monly mounted orthogonal to each other in three dimensions and sealed in some box.

2.4

GPS

gpsis a navigation system developed by the United States Department of Defence under its navstar (navigation satellite timing and ranging) satellite program and includes at least 24 operational satellites. The satellites are spread out on six non geostationary orbits each at an altitude of approximately 26560 km and has an orbital period of approximately 11.967 hours. The orbits have an angle of inclination of 55◦relative to the equator and they are separated by multiples of 60◦and contains at least four satellites each.

Each satellite transmits two spread spectrum signals L1 and L2using the

corre-sponding carrier frequencies f1 = 1575.42 MHz and f2 = 1227.6 MHz. Accurate

caesium and/or rubidium clocks provide timing information for the transmitted signals. The L1and L2signals will have different propagation delay and the

mea-sured difference in propagation delay can be used to compensate for the delay in each carrier.

The L1 signal is first modulated by binary phase shift keying (bpsk) and then

modulated in phase quadrature by two pseudo random noise (prn) codes, C/A-code (coarse acquisition C/A-code) and P-C/A-code (precision C/A-code). The L2signal is

10 2 Theory

applications.

The P-code is a relatively long fine-grained code with chip rate of 10.23 MHz and provides more accurate timing information to the transmitted signal. The C/A-code is a shorter coarser grained C/A-code with chip rate of 1.023 MHz and provides coarser timing information to the transmitted signal.

The satellites use a multiple access technique called code division multiplexing (cdma). Each satellite uses unique prn codes which are known by the receiver which makes it possible for the receiver to detect signals from several satellites on the same carrier frequency [Grewal et al., 2011].

2.4.1

Principles of GPS Positioning

The timing information sent from a satellite is used to calculate the pseudorange (range) to a receiver, ρr, via the speed of propagation of an electromagnetic wave.

The calculated pseudorange contains errors due to propagation delay, clock er-rors, receiver errors etc.

The position of a gps satellite in an ecef coordinate system (xecef, yecef, zecef)

is known by the gps receiver, and the goal is to calculate the position of a gps receiver in an ecef coordinate system (Xecef, Yecef, Zecef) using pseudoranges

from at least four satellites.

The point of departure is the equation for the pseudorange (2.18). If both sides of (2.18) are squared then the equation is rewritten according to (2.20), where r denotes the radius of Earth and Cbiasdenotes clock bias correction.

With pseudoranges to four satellites it is possible to set up a linear equation sys-tem and solve for Xecef, Yecef, Zecef and C_bias. If more than four satellites are

available the solution to the equation system is given by a least squares solu-tion [Grewal et al., 2011].

ρr =

q

(xecef− Xecef)2+ (yecef− Yecef)2+ (zecef− Zecef)2 (2.18)

ρ2r = (xecef− Xecef)2+ (yecef− Yecef)2+ (zecef− Zecef)2 (2.19)

= X2ecef+ Y2ecef+ Z2ecef

| {z }

r2_+Cbias

+x2ecef+ y2ecef+ z2ecef

−_2Xecefxecef−_2Yecefyecef−_2Zecefzecef

ρ2_r −_(x2_{ecef+ y}2_{ecef+ z}2_{ecef) − r}2_{= Cbias}−_2Xecefxecef (2.20)

2.5 Bearing Sensor 11

2.5

Bearing Sensor

The bearing sensor is defined by an antenna array in combination with the array signal processing algorithm music. The antenna array used during the field test was a vertical dipole antenna array with four antenna elements L = 4 (see Fig-ure 2.1) and the bearing was estimated to one transmitter M = 1. In the general description of the music algorithm it is possible to detect up to L − 1 bearings. It is assumed that the bearings can be calculated at a rate in the interval [1, 1000] Hz.

Figure 2.1: The antenna array used for collecting bearing measurements. Courtesy of foi.

2.5.1

DOA Estimation Through the MUSIC Algorithm

This section describes the general music algorithm and follows the description given in [Krim and Viberg, 1996].

Consider a narrowband transmitted electro-magnetic waveform, with a field com-ponent (E) in the origin given by (2.21). It is assumed that the signal s(t) ∈ C is slowly time-varying compared to the carrier ejωt_{, where ω denotes the carrier}

angular frequency and t denotes time.

12 2 Theory

Assume that |r|  c/B, where r denotes the radius vector, c is the speed of propa-gation and B is the bandwidth of s(t). Then the electro-magnetic field component at r can be written as (2.22), where k is the wave-vector which point in the di-rection of propagation, and its magnitude is the wave-number |k| = k = 2π_λ (here

λ denotes the wave-length). The field component (2.22) assumes far-field

condi-tions, i.e. the transmitter is located far away where the field is well described by plane waves.

E(r, t) = s(t)ej(ωt−rTk) (2.22)

If the horizontal plane is considered the wave-vector is given by (2.23), where θ is the direction of propagation, defined counter-clockwise relative the x-axis.

k =k(cos θ, sin θ)T (2.23) The field at a receiver l at position rl = (xl, yl)T due to a transmitter at azimuthal

θ is given by (2.24).

E(rl, t) = s(t)ej[ωt−k(xlcos θ+ylsin θ)] (2.24)

If the sensor is assumed to have a flat frequency response gl(θ) over the signal

bandwidth, the sensor output will be proportional to the field at rl. If the carrier

term is dropped the noise free output is modelled by (2.25). xl(t) = gl(θ)e

−jk(x_lcos θ+y_lsin θ)_{s(t) = a}

l(θ)s(t) (2.25)

The noise free array output vector of an L-element antenna array with arbitrary geometry is expressed by (2.26), where a(θ) is called the steering-vector (2.27).

x(t) = a(θ)s(t) (2.26)

a(θ) = [a1(θ), . . . , aL(θ)]T (2.27)

If the medium is assumed to be linear the superposition principle holds, and if

M signals are received by an antenna array at distinct doa:s (θ1, . . . θM) the noise

free array output vector is given by (2.28), where sm(t) are baseband signals.

x(t) =

M

X

m=1

a(θm)sm(t) (2.28)

By defining a steering matrix (2.29) and a vector of baseband signals (2.30) and also taking additive spatially white noise into account the array output vector can be expressed as (2.31). The music algorithm requires that M < L and it is therefore assumed throughout this section.

A(θ) = [a(θ1) . . . a(θM)] (2.29)

2.5 Bearing Sensor 13

x(t) = A(θ)s(t) + n(t) (2.31) A signal parameter of interest is the spatial covariance matrix given by (2.32), where the source covariance matrix is given by (2.33) and the noise covariance matrix is given by (2.34).

R =E[x(t)x(t)H] = AE[s(t)sH(t)]AH+ E[n(t)nH(t)] (2.32)

E[s(t)sH(t)] = P (2.33)

E[n(t)nH(t)] = σ2I (2.34) The music algorithm is a subspace based algorithm and therefore the factorisa-tion of R is of central importance (2.35). The matrix Λ is a diagonal matrix con-taining eigenvalues Λ = diag (λ1, . . . , λL), where the eigenvalues are ordered such

that λ1 ≥. . . ≥ λL > 0. The matrix U is unitary and contains the corresponding

eigenvectors.

R = AP AH+ σ2I = U ΛUH (2.35) The eigenvalues and the corresponding eigenvectors can be partioned into sig-nal eigenvectors and noise eigenvectors, respectively. Assuming AP AHto be full rank, the M largest eigenvalues belong to the signal subspace, where the corre-sponding eigenvectors span the signal subspace. The rest of the L − M eigenval-ues belong to the noise subspace, where the corresponding eigenvectors span the noise subspace. The spatial covariance matrix can also be expressed as (2.36), where Us is a matrix containing signal eigenvectors, Un is a matrix containing

noise eigenvectors, Λsis a diagonal matrix containing signal eigenvalues and Λn

is a diagonal matrix containing the noise eigenvalues. Throughout this section the number of received signals is assumed to be known.

R = UsΛsUs+ UnΛnUn (2.36)

In practice the array output vector is sampled at some time instances t = 1, 2, . . . , N and estimates of the spatial covariance matrix can be obtained (2.37) and fac-torised (2.38). ˆ R = 1 N N X t=1 x(t)xH(t) (2.37) ˆ R = ˆUsΛˆsUˆs+ ˆUnΛˆnUˆn (2.38)

The noise eigenvectors Unare orthogonal to A, therefore (2.39) holds. This

prop-erty is used in the music spatial spectrum.

UHna(θ) = 0, θ ∈ {θ1, . . . θM} (2.39)

The antenna array is usually assumed to be unambiguous to allow for unique doaestimates. It means that any collection of L steering vectors form a linearly

14 2 Theory

independent set {a(η1), . . . a(ηL)}, given distinct doa:s ηk. If the steering vector a

satisfies these conditions and if P has full rank, then AP AH has full rank too. It follows that the doa:s {θ1. . . θM}are the unique solutions to (2.39).

The spatial spectrum of the music algorithm is given by (2.41), where ˆΠ⊥is an orthogonal projector onto the noise subspace (2.40). As suggested by (2.39) the spatial spectrum will exhibit peaks close to the true doa:s. The angles corre-sponding to the L observed peaks define the estimated doa:s.

ˆ Π⊥= ˆUnUˆHn (2.40) Pmusic= aH(θ)a(θ) aH_{(θ) ˆΠ}⊥ a(θ) (2.41)

2.5.2

Regarding MUSIC Eigenvalue Fractions

Consider the case with one transmitter M = 1 and an antenna array with four elements L = 4. Study the factorisation of the spatial covariance matrix R given in (2.35). It can be observed that any eigenvector orthogonal to the steering matrix

A is an eigenvector of R with an eigenvalue equal toσ2(the noise power). There are L − M = 4 − 1 = 3 such vectors which are linearly independent, they define the noise eigenvectors. The is one signal eigenvector and its eigenvalue is larger than σ2[Krim and Viberg, 1996].

It is assumed that the signal eigenvalue λ1 is proportional to the signal power.

Then it follows that the eigenvalue fractions; λ1

λ2

λ1

λ3 and

λ1

λ4 are proportional to the signal-to-noise ratio.

2.6

Nonlinear Filtering

This section gives a brief introduction to nonlinear filtering via Bayes filter equa-tions, the Kalman filter and finally the extended Kalman filter. The point of departure is a nonlinear discrete time state space model, given by the process model (2.42) and the measurement model (2.43).

xk = fk(xk−1, vk−1) (2.42)

y_k = hk(xk, ek) (2.43)

The state vector xk contains information that describes the system under

consid-eration, the process model given by a nonlinear system of difference equations describes how the state vector evolves with time and the measurement model given by a nonlinear equation system describes how noisy measurements relate to the state vector. The vector vk−1denotes the process noise and the vector ek

2.6 Nonlinear Filtering 15

The state vector is real, xk ∈ Rnx, where nx denotes the dimension of the state

vector. The time index k is derived from a continuous time instant tk and the

sampling time Tk = tk−tk−1may be time dependent.

Nonlinear filtering is the problem to sequentially estimate the state of a dynamic system using a noisy measurement sequence made on the system. The problem can be divided into a prediction step and a update step. The prediction step uses the dynamic description of the system and the update step uses the measurement model. The prediction step typically results in an increase in the covariance of a state estimate and when the update step occurs the covariance of the state esti-mate typically decreases [Ristic et al., 2004].

2.6.1

Bayes Recursive Filter Equations

The objective of Bayes filter equations is to recursively calculate the posterior pdfp(x_k|Y_k) for the state x_kat time k given the measurements up to time k , Y_k, where Yk = (yk, yk−1, . . .).

It is assumed that the state space model is available in a probabilistic form. It is also assumed that the posterior pdf at time k, p(xk|Yk), and the initial state pdf

p(x0) are available and that the process model is described by a Markov process

of order one. The Markov property yields p(xk|xk−1, xk−2, . . . , x0) = p(xk|xk−1).

Then the process model is described by a transitional pdf p(xk+1|xk) and the

mea-surement model is described by a likelihood function p(y_k|xk).

To obtain the prediction pdf at the next time instant k +1 the Chapman-Kolmogorov equation (2.44) is used. The second equality of (2.44) holds because the process model describes a Markov process of order one. The posterior pdf at time k can be rewritten according to (2.45) using Bayes’ rule, where the second equality holds beacuse the process model describes a Markov process of order one. The de-nominator of (2.45) can be rewritten according to (2.46) [Ristic et al., 2004] [Lundquist, 2011]. p(xk+1|Yk) = Z p(xk+1|xk, Yk)p(xk|Yk)dxk = Z p(xk+1|xk)p(xk|Yk)dxk (2.44) p(xk|Yk) = p(xk|yk, Yk−1) = p(yk|xk, Yk−1)p(xk|Yk−1) p(y_k|Yk−1) = p(yk|xk)p(xk|Yk−1) p(y_k|Yk−1) (2.45) p(y_k|Yk−1) = Z p(y_k|xk)p(xk|Yk−1)dxk (2.46)

16 2 Theory

2.6.2

The Kalman Filter

If the state space model is linear with process model (2.47) and measurement model (2.48) and if the pdf:s p(y_k|xk) p(xk+1|xk) and p(x0) are Gaussian then

(2.44) and (2.45) reduce to the Kalman filter which is divided into a prediction step and a update step [Gustafsson et al., 2008].

xk+1= Fkxk+ wk, wk ∼N (0, Qk) (2.47)

y_k = Hkxk+ ek, ek∼N (0, Rk) (2.48)

Algorithm 1Kalman filter Require: ˆx0|0and ˆP0|0 Time update ˆxk|k−1= Fk−1ˆxk−1|k−1 Pk|k−1= Fk−1Pk−1|k−1F_k−1T + Qk−1 Measurement update Kk = Pk|k−1H_kT(HkPk|k−1H_kT + Rk)−1 ˆxk|k = ˆxk|k−1+ Kk(yk−Hkˆxk|k−1) Pk|k= (I − KkHk)Pk|k−1

2.6 Nonlinear Filtering 17

2.6.3

The Extended Kalman Filter

Consider a nonlinear system with additive noise given by (2.49) and (2.50), where the noise sequences vk−1 and ek are assumed to be mutually independent

zero-mean white Gaussian with the corresponding covariances Qk−1 and Rk. An

ex-tended Kalman filter uses the first order Taylor expansion of the nonlinear func-tions (2.49) and (2.50) around the current estimates and then applies the Kalman filter equations [Gustafsson, 2010] [Lundquist, 2011].

xk = fk−1(xk−1) + vk−1 (2.49)

y_k = hk(xk) + ek (2.50)

Algorithm 2Extended Kalman filter Require: ˆx0|0and ˆP0|0 Time update ˆxk|k−1= fk−1( ˆxk−1|k−1) Pk|k−1= Fk−1Pk−1|k−1F_k−1T + Qk−1{where Fk= ∂f_∂xk(x_kk)    _x k= ˆxk|k } Measurement update Kk = Pk|k−1HkT(HkPk|k−1HkT + Rk)−1 ˆxk|k= ˆxk|k−1+ Kk(yk−hk( ˆxk|k−1)) Pk|k = (I − KkHk)Pk|k−1{where Hk =∂h_∂xk(x_kk)    _x k= ˆxk|k−1 }

3

Modelling

This Chapter describes the state space models that are used in the extended Kalman filter framework. First a brief introduction to the state vector is given then the process models and the measurement models are described. This Chap-ter also includes; how the bearing measurements are modelled, the bearing mea-surement equation in more details and the bearing meamea-surement error model.

3.1

State Vector

The point of departure in the state space model is the state vector x which con-tains variables that describe the navigation system. The specific state vector used in this thesis is x = (x, y, ϕ, v, a, ω)T, where x and y are position coordinates in the rt90coordinate system, ϕ is the vehicle’s heading angle, v is the vehicle’s speed, a is the vehicle’s acceleration in the vehicle’s motion direction and ω is the angular velocity around the vehicle’s motion centre.

3.2

Process Model

The process model describing the dynamics of the car is a time continuous co-ordinated turn model in two dimensions and the foundation is given in (3.2) [Gustafsson, 2010]. The acceleration and the angular velocity are assumed driven by additive zero mean white Gaussian noise, wa∼ N(0, Qa) and wω ∼ N(0, Qω)

i.e. w = (wa wω)T ∼ N(0, Q) where Q is given by (3.1).

Q = Qa 0

0 Qω

!

(3.1) 19

20 3 Modelling                      ˙x ˙y ˙ ϕ ˙v ˙a ˙ ω                      =                      v cos ϕ v sin ϕ ω a 0 0                      +                      0 0 0 0 0 0 0 0 1 0 0 1                      | {z } B wa wω ! |{z} w (3.2)

Since the measurements are provided at discrete time instances it is necessary to discretise the time continuous process model in order to use it in the extended Kalman filter framework. The process model is discretised using the sampling formula (3.3) and it can be solved exactly (T denotes the sampling time). The dis-cretised process model is given by (3.4), where wk ∼ N(0, Qk) and the dimension

of wkis 6. x(t + T ) = x(t) + t+T Z t a(x(τ))dτ (3.3)                      xk+1 yk+1 ϕk+1 vk+1 ak+1 ωk+1                      =                        x_k+ 2vk ωk sin ( ωkT 2 ) cos (ϕk+ω2kT) y_k−2vk ωk sin ( ωkT 2 ) sin (ϕk+ ωk2T) ϕk+ T ωk vk+ T ak ak ωk                        + wk (3.4)

It is possible to discretise the process noise analytically via (3.5), where A denotes the time continuous Jacobian (3.6) [Gustafsson, 2010]. In this thesis the time discrete process noise Qkis considered to be a parameter that can be used to tune

the extended Kalman filter and the exact analytic expression of Qk is not needed.

Nevertheless, the structure can be useful.

Qk = T Z 0 eAτBQBTe(A)Tτdτ (3.5) A =                      0 0 −_{v sin ϕ cos ϕ 0 0} 0 0 v cos ϕ sin ϕ 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0                      (3.6)

3.2 Process Model 21

The process model (3.4) holds for ωk , 0 but in the limitsin (

ωk T

2 )

ωk T

2

tends to 1 as ωk

tends to 0. Then (3.4) can be rewritten according to (3.7).                      x_k+1 y_k+1 ϕk+1 vk+1 ak+1 ωk+1                      =                      x_k+ vkT sin ϕk y_k+ vkT cos ϕk ϕk+ T ωk vk+ T ak ak ωk                      + wk (3.7)

3.2.1

A More Advanced Motion Model

The motion model described in Chapter 3.2 is a coordinated turn model which assumes that the vehicle has a circular movement. A more advanced model is put forth by [Schubert et al., 2008] called ctra (constant turn rate and acceleration). It shares the same state vector and the characteristics of assuming a constant acceleration and yaw rate but differs by modelling the motion as a clothoid move-ment instead of a circular one. The difference being that the clothoid motion model allows a linear change in the turn radius whereas the circular movement is modelled with a fixed radius.

The discrete time motion model is described in (3.8).

                     xk+1 yk+1 ϕk+1 vk+1 ak+1 ωk+1                      =                      xk+ ∆xk yk− ∆yk ϕk+ T ωk vk+ T ak ak ωk                      + wk (3.8) where wk ∼ N(0, Qk), ∆x_k = 1 ω_k2[(vkωk+ akωkT ) sin (ϕk+ ωkT )+

+ akcos (ϕk+ ωkT ) − vkωksin ϕk−akcos ϕk] (3.9)

and

∆y_k = 1

ω2_k[(−vkωk

−_{akωkT ) cos (ϕk+ ωkT )+}

22 3 Modelling

The case of ωk = 0 needs extra precaution and since

lim ω→0xk+1= xk+ (vkT + ak 2T 2_{) cos ϕ} k (3.11) and lim ω→0yk+1= yk+ (vkT + ak 2T 2_{) sin ϕ} k (3.12)

holds, a new motion model is given according to (3.13).

                     x_k+1 y_k+1 ϕk+1 vk+1 ak+1 ωk+1                      =                      x_k+ (vkT +a2kT2) cos ϕk y_k+ (vkT +a2kT2) sin ϕk ϕk+ T ωk vk+ T ak ak ωk                      + wk (3.13)

3.3

Measurement Model

The measurement model describes how the measurements relate to the state vec-tor. In this thesis there are basically two building blocks of measurement models, one measurement model for each sensor; imu (3.14) and bearing (3.16) (here M denotes the number of beacons). For reference filtering a third measurement model for gps is used (3.15).

yimu k =         ak vkωk ωk         + eimu k (3.14) ygps k = x_k y_k ! + egps k (3.15) yBearing_k =                β_k1(xk, yk, ϕk, p1) β_k2(xk, yk, ϕk, p2) .. . βM_k (xk, yk, ϕk, pM)                + eBearing_k (3.16)

The measurement error is modelled as zero mean additive white Gaussian noise for the imu, eimu

k ∼ N(0, R imu k ) and the gps, e gps k ∼ N(0, R gps k ).

The bearing measurements to different beacons are modelled as independent pro-cesses where each component of the vector eBearing_k is modeled as a truncated zero mean white Gaussian and uniform mixture distribution. This is explained in

3.3 Measurement Model 23

more details in Section 3.5. However, in the extended Kalman filter it is assumed that the bearing measurement error is zero mean white Gaussian according to

eBearing_k ∼ N_{(0, R}Bearing

k ) . (3.17)

The full expression of how the bearing to beacon i, βi_k, relates to the state, xk, is

given in Section 3.3.1.

3.3.1

Bearing Measurement Equation

The bearing measurement equation is more complex than the other measurement equations and deserves a detailed explanation. The bearing β to a beacon p is defined as β = arctan2 (pB

x, pBy), where pBx and pyBis the position of beacon p in

the vehicle fix coordinate system (body fixed (B)). To express this position in the world frame (W) a transformation is done accordingly.

The relationship between a coordinate in the world frame (W) and the navigation frame (N) is given by translation, where rN/Wdenotes the vector to the navigation

coordinate system relative the origin of the world coordinate system i.e. it is a position vector for the vehicle (x, y)T.

x y ! = rN/W+ xN yN ! (3.18)

The relationship between a coordinate in the vehicle fixed coordinate system (B) and the navigation coordinate system (N) is given by the rotation

xN yN ! = cos ϕ − sin ϕ sin ϕ cos ϕ ! xB yB ! (3.19)

Matrix inversion yields the relationship between N and B, were the rotation angle

ϕ defines the heading angle.

xB y_B ! = cos ϕ₋ sin ϕ sin ϕ cos ϕ ! xN y_N ! (3.20)

Using the translation between N and W yields x y ! − rN/W= x_N yN ! . (3.21)

Combining (3.20) and (3.21) yields (3.22). x_B y_B ! = cos ϕ − sin ϕ sin ϕ cos ϕ ! x y ! − rN/W ! (3.22)

writ-24 3 Modelling

ten as (3.23).

β = arctan2 (cos(ϕ)(px− x) − sin(ϕ)(py− y),

sin(ϕ)(px− x) + cos(ϕ)(py− y))) (3.23)

3.4

Bearing Measurement Generation

Collected position data is used to simulate the relative bearing βk to a beacon p

at time instance k. This relationship is defined according to Figure 3.1.

p

yB

x

B

k

x

_k

x

_k1

yW

x

W

zW

r

p/xk

r

_{xk/ xk} 1

r

p/xk1

Figure 3.1: Explanatory drawing describing how the true bearing is calcu-lated.

rp/xk denotes the vector to beacon p relative the vehicle position given by the

state xk, rxk+1/xk denotes the vector to the vehicle position given by the state xk+1

relative the vehicle position given by the state xk and rp/xk+1 denotes the vector

to beacon p relative the vehicle position given by the state xk+1. The bearing is

calculated using the law of cosine (3.24).

cos(βk) = b2_{+ c}2_−a2 2bc (3.24) a = krp/xk+1k= px py ! − xk+1 yk+1 ! = q (px− xk+1)2+ (py− yk+1)2 (3.25) b = krxk+1/xkk= xk+1 yk+1 ! − xk yk ! = q (xk+1− xk)2+ (yk+1− yk)2 (3.26) c = krp/xkk = px py ! − xk yk ! = q (px− xk)2+ (py− yk)2 (3.27)

3.5 The Bearing Measurement Noise Model 25

The reason that the measurement equation (3.23) is not used to simulate the bear-ings is that the yaw angle ϕ is not directly measurable. However, the yaw angle can be estimated by an extended Kalman filter. On the other hand the position is measured directly via gps and a linearly interpolated curve based on gps mea-surements defines a smooth position trajectory that can be used to simulate the bearing measurements.

The bearing is defined as positive for positive ˆyBand negative vice versa. This is

determined by investigation of the sign of the cross product rxk+1/xk× rp/xk.

3.5

The Bearing Measurement Noise Model

During a field test in urban environment in the city of Linköping in Sweden bearing measurements to a known transmitter were performed (data collection A). The bearings were estimated using a vertical dipole antenna array with four antenna elements attached to a car and using the array signal processing algo-rithm music. The true bearings were estimated using position data from a gps attached to the car and the position of the known transmitter. With means of 384 bearing estimates a histogram of the measurement error could be calculated, see Figure 3.2.

The histogram is shown to have heavy tails and the reason is that the received radio signal experiences multi path propagation resulting in erroneous bearing measurements from the music algorithm. The probability density function corre-sponding to the histogram, Figure 3.2, can be modelled as a truncated Gaussian and uniform mixture, where the uniform part represents the outliers and the Gaussian part represents the measurements. The bearing error sample space is bounded by [−π, π] rad and it is therefore reasonable to use the truncated Gaus-sian density function in the mixture [Robert, 1995]. Numerous literature can be found concerning the Gaussian and the uniform distribution, for example see [Blom et al., 2005].

1 Definition. Let Y be a truncated normal random variable, T N (µ, σ2, a, b). Then the

probability density function for Y can be expressed as (3.28), where I_[a,b](y) is the indica-tor function. I_[a,b](y) = 1 , if y ∈ [a, b] and zero elsewhere. Φ is the cumulative distribution function of a N (0, 1) variable. fY(y) = √ 1 2πσ2 exp(−(y−µ)_2σ22) Φ(b−µ_σ ) − Φ(a−µ_σ ) I_[a,b](y) (3.28)

By experiments the bearing measurement noise can be modelled to be a mixture of 30% truncated normal distribution with zero mean and standard deviation of 5◦(₁₈₀5π rad), T N (0, (₁₈₀5π)2, −π, π), and 70% uniform distribution on the interval

[−π, π] rad, U (−π, π). The stochastic variable E describing the bearing error dis-tribution is given in (3.29), where the weight α = 0.3 and the corresponding pdf

26 3 Modelling

is shown in Figure 3.4, the pdf sums to 1 in Matlab.

E ∼ αT N (0, (5π

180)

2

, −π, π) + (1 − α)U (−π, π) (3.29) The measurement error can be simulated using the following approach. Generate a random vector containing realisations (samples) from the uniform distribution U_{(−π, π) and a second random vector with realisations from N (0, (}5π

180) 2

). Draw 70% of the samples from the uniform distribution and 30% of the samples from the Gaussian distribution in order to emulate the bearing measurement noise. A histogram generated by 384 samples with this approach is shown in Figure 3.3. The settings standard deviation σ and weight α of the error distribution is likely to change depending on where the car is located relative the radio beacons and the obstacles in the surroundings. This model is simple and does not take that into account.

Figure 3.2:Bearing measurement error histogram from the experiment.

3.5 The Bearing Measurement Noise Model 27

4

Implementation

The implementation of the extended Kalman filter has been performed in Matlab.

4.1

Multirate Extended Kalman Filter

The bearing measurements and the imu measurements have different sampling frequency. The imu samples acceleration and the yaw rate at a frequency of 100 Hz and the bearings can be estimated with a frequency in the interval [1, 1000] Hz. Therefore it is preferable to implement a multirate extended Kalman filter. The main idea of a multirate extended Kalman filter is that measurement updates are performed when the measurements become available.

However, the multi rate extended Kalman filter is in general well described by algorithm 2. In the implementation the time index k is related to the time instant

t = kT , where T = 0.01 s is the imu sampling time.

Since the implementation is done in discrete time the time discrete Jacobian of the process model is used. The time discrete Jacobian can be calculated using a matrix exponential of the time continous Jacobian A and the imu sampling time

T . At every time instance where the prediction step is performed the time

conti-nous Jacobian is first evaluated and then the time discrete Jacobian is calculated according to (4.1).

F = eAT (4.1)

At every time step k, measurements from the imu are available and if no bearing measurements occur at the same time instant the measurement model is the same

30 4 Implementation

as in (3.14). When the bearing measurements become available the measurement model is expanded by (3.16) and can be rewritten according to (4.3), where the measurement noise is assumed to be zero mean white Gaussian with noise covari-ance according to (4.2). Bearings to several beacons are assumed to be measured simultaneously. RI MU ,Bearing_k = R I MU k 0 0 RBearing_k ! (4.2) yI MU ,Bearing_k =                             ax k ay_k ωk β1_k β2_k .. . β_kM                              =                             ak vkωk ωk β1_k(xk, yk, ϕk, p1) β2_k(xk, yk, ϕk, p2) .. . β_kM(xk, yk, ϕk, pM)                             + eI MU ,Bearing_k (4.3)

The reference filter used in this thesis is a multi rate extended Kalman filter that uses all the available sensors. The sampling frequency of the gps and the bearings is set to 4 Hz and it is assumed that the gps and the bearings are synchronised. Between measurement updates from the gps and the bearings the measurement model is given by (3.14). When the measurements from the gps and the bearings become available the measurement model is expanded according to (4.5), where the measurement noise is assumed to be zero mean white Gaussian with noise covariance according to (4.4). Rgps_k ,imu,Bearing =          Rgps k 0 0 0 Rimu k 0 0 0 RBearing_k          (4.4) ygps_k ,imu,Bearing=                                      x_k y_k ax k ay_k ωk β1_k β2_k .. . β_kM                                      =                                      xk yk ak vkωk ωk β1_k(xk, yk, ϕk, p1) β2_k(xk, yk, ϕk, p2) .. . β_kM(xk, yk, ϕk, pM)                                      + egps_k ,imu,Bearing (4.5)

4.2 Regarding Settings of Q and R 31

4.2

Regarding Settings of Q and R

The process noise covariance Q and measurement noise covariance R are tuning parameters for the ekf.

The common tuning approach is to fix the measurement noise covariance matrix according to sensor specifications or measured sensor covariances and tune the process covariance matrix.

5

Method

This chapter presents the different methods with potential to improve the inertial and bearing navigation system.

5.1

MUSIC Eigenvalue Fractions

The algorithm music is a subspace based algorithm for bearing estimation. Dur-ing the field test bearDur-ings were estimated to one transmitter usDur-ing an antenna array with four elements. Therefore the signal subspace has dimension 1 and the noise subspace has dimension 3.

According to Section 2.5.2 it can be shown that the the eigenvalue fractions λ1

λ2,

λ1

λ3 and

λ1

λ4 are proportional to the signal-to-noise ratio. Therefore they could be used as a measure of quality of the received radio signal.

The eigenvalue fractions were plotted versus the bearing measurement error in order to investigate if there were any relationship that could be used to distin-guish good measurements from outliers. Depending on the outcome this could be a method for preprocessing the bearing measurements.

5.2

Bearing Measurements Update Frequency

In practical application it is realistic to achieve a bearing update frequency of up to 1 kHz. To determine whether or not it would give a substantial improvement, different update rates of the bearing measurements are investigated according to Table 5.1.

34 5 Method

imu Bearing

100 Hz 1 Hz 100 Hz 4 Hz 100 Hz 100 Hz

Table 5.1:The different update frequencies used for evaluation

5.3

A More Advanced Motion Model

Two different motion models were described in Section 3.2; the simple and the ctramotion model. Since the ctra motion model is more advanced it could give a better prediction of the movement and therefore a better filtering.

5.4

Outlier Rejection With Fixed Threshold

The bearing measurement noise is modelled as a Gaussian and uniform noise mixture. The measurements with uniform noise, referred to as outliers, must be discarded in order to get a good estimation. This is done by comparing the innovation of a bearing measurement i to a fixed threshold c and discarding it if

|_yi

k−hik( ˆxk|k−1)| > c (5.1)

The idea behind this is the assumption that the measurement prediction hi_k( ˆxk|k−1)

is of high quality and therefore should not differ too much from the actual mea-surement update.

The enhanced ekf with this outlier rejection scheme is presented in algorithm 3.

5.5

Outlier Rejection With Dynamic Threshold

The problem with using the innovation as an indication of the quality of a mea-surement is that it becomes large if the vehicle deviates too much from its pre-dicted path, i.e. if the vehicle is turning.

A method were the fixed threshold c in Algorithm 3 is replaced with a dynamic threshold d(ωk) and discarding a measurement i if

|_yi

k−hik( ˆxk|k−1)| > d(ωk) (5.2)

5.5 Outlier Rejection With Dynamic Threshold 35

Algorithm 3 ekfwith outlier rejection Require: ˆx0|0and ˆP0|0 Time update ˆ xk|k−1= fk−1( ˆxk−1|k−1) Pk|k−1= Fk−1Pk−1|k−1F_k−1T + Qk−1{where Fk= ∂f_∂xk(x_kk)_x k= ˆxk|k} Kk = Pk|k−1H_kT(HkPk|k−1H_kT + Rk)−1 Measurement update

for allbearing measurements i do if|y_ki −_hi

k( ˆxk|k−1)| > c then

Discard bearing measurement i end if end for ˆ xk|k= ˆxk|k−1+ Kk(yk−hk( ˆxk|k−1)) Pk|k = (I − KkHk)Pk|k−1{where Hk =∂h_∂xk(x_kk)_x k= ˆxk|k }

6

Results

This chapter presents the results of the possible performance enhancement meth-ods presented in Chapter 5. The quality of each method is evaluated through the rmsedefined as xrmse= v u t 1 N N X k=1 (xk,ref erence−xk)2 (6.1)

where x is one of the states x, y or ϕ and N are all time instances of a filtering. The reference used to calculate the rmse is a gps and imu aided filtering with noise free bearing measurements and update frequencies according to Table 6.1. The reference trajectory with position of the beacons can be seen in Figure 6.1.

imu[Hz] gps[Hz] Bearings [Hz]

100 4 4

Table 6.1:Update frequencies for the reference filtering.

The matrices Q and R have been chosen ad hoc in order to achieve a filtering that is perceived as good. The bearing measurements are trusted heavily upon and the same values have been used for all cases, including the reference filtering. Be-cause of this the reference trajectory has a lower quality outside the area defined by the three radio beacons [Cohen and Koss, 1992].

As described in Section 3.5 the most realistic noise for the bearing measurements is a 30% Gaussian and 70% uniform noise mixture with 5◦

standard deviation for the Gaussian noise. The different methods, except the one described in Sec-tion 6.1, are tested with different Gaussian uniform noise mixtures and different standard deviations of the Gaussian noise for the bearing measurements to deter-mine how well they perform.

38 6 Results

Figure 6.1:Reference trajectory with beacons.

6.1

MUSIC

Eigenvalue Fractions

The eigenvalue fractions, λ1

λ2,

λ1

λ3 and

λ1

λ4, are plotted in Figure 6.2 Figure 6.3 and Figure 6.4.

By visual observation of Figure 6.2, Figure 6.3 and Figure 6.4 no obvious maxi-mum can be observed for small measurement errors, i.e. a small measurement error does not necessarily coincide with a large eigenvalue fraction. Therefore this method can not be used to distinguish good measurements from outliers. The explanation behind this observed behaviour is that music can not distin-guish a direct radio wave from a multipath component of a radio wave.

6.2

Bearing Measurements Update Frequency

Three different filterings with different update frequencies were performed. The rmsefor the three estimated states is given in Table 6.2. The noise used for the bearing measurement is Gaussian noise with a standard deviation of 0.5◦. Each rmsevalue is an average value from five simulations.

6.2 Bearing Measurements Update Frequency 39

Figure 6.2: λ1

λ2 versus bearing measurement noise.

Figure 6.3: λ1

λ3 versus bearing measurement noise.

Figure 6.4: λ1