Indoor Positioning Using Opportunistic Multi-Frequency RSS With Foot-Mounted INS

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Indoor Positioning Using Opportunistic

Multi-Frequency RSS With Foot-Mounted INS

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

av Martin Nilsson LiTH-ISY-EX--14/4798--SE

Linköping 2014

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Indoor Positioning Using Opportunistic

Multi-Frequency RSS With Foot-Mounted INS

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan vid Linköpings universitet

av

Martin Nilsson LiTH-ISY-EX--14/4798--SE

Handledare: Jouni Rantakokko

FOI Patrik Eliardsson FOI Joakim Rydell FOI Erika Emilsson FOI Martin Skoglund i s y_{, Linköpings universitet}

Examinator: Gustaf Hendeby

i s y, Linköpings universitet

(4)

(5)

Avdelning, Institution Division, Department

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

URL för elektronisk version

http://www.ep.liu.se

ISBN — ISRN

LiTH-ISY-EX--14/4798--SE

Serietitel och serienummer Title of series, numbering

ISSN —

Titel Title

Inomhuspositionering baserat på opportunistiska signalstyrkemätningar och fotmonterad TNS

Indoor Positioning Using Opportunistic Multi-Frequency RSS With Foot-Mounted INS

Författare Author

Martin Nilsson

Sammanfattning Abstract

Reliable and accurate positioning systems are expected to significantly improve the safety for first responders and enhance their operational efficiency. To be effective, a first responder positioning system must provide room level accuracy during extended time periods of indoor operation.

This thesis presents a system which combines a zero-velocity-update (z u p t) aided inertial navigation system (i n s), using a foot-mounted inertial measurement unit (i m u), with the use of opportunistic multi-frequency received signal strength (r s s) measurements. The sys-tem does not rely on maps or pre-collected data from surveys of the radio-frequency (r f) environment; instead, it builds its own database of collected r s s measurements during the course of the operation. New r s s measurements are continuously compared with the stored values in the database, and when the user returns to a previously visited area this can thus be detected. This enables loop-closures to be detected online, which can be used for error drift correction. The system utilises a distributed particle simultaneous localisation and map-ping (d p - s l a m) algorithm which provides a flexible 2-D navigation platform that can be extended with more sensors. The experimental results presented in this thesis indicates that the developed r s s s l a m algorithm can, in many cases, significantly improve the position-ing performance of a foot-mounted i n s.

The work in this thesis have resulted in a research paper, which has been submitted and accepted to the international conference on indoor positioning and indoor navigation (IPIN 2014) [1].

Nyckelord

Keywords Particle filters, Simultaneous localization and mapping, Radio navigation, Multisensor inte-gration

(6)

(7)

Sammanfattning

Pålitliga och noggranna positioneringssystem tros kraftigt kunna öka säkerheten för uttrycknings- och insatspersonal och samtidigt förbättra deras operativa för-måga. För att vara användbart måste ett system för insatspersonal ha en noggrann-het motsvarande rumsnivå; detta även under långa operationer i inomhusmiljöer. I den här rapporten presenteras ett system som kombinerar ett tröghetsnavigeran-de positioneringssystem (i n s), baserat på en fotmonterad tröghetsmätningsenhet (i m u) och nollhastighetsuppdateringar (z u p ts), med opportunistiska signalstyr-kemätningar (r s s) från olika frekvenser. System förlitar sig inte på kartor eller förinsamlade mätningar från den radiomiljö där det ska användas, istället bygger system upp sin egen databas av insamlade signalstyrkemätningar under själva operationen. Nya mätningar jämförs löpande med tidigare insamlade värden i databasen och när användaren återvänder till en tidigare besökt plats kan det-ta upptäckas. Detdet-ta möjliggör att loopslutning kan detekteras i realtid, vilket i sin tur kan användas för att korrigera drift i det tröghetsnavigerande syste-met. Det implementerade systemet använder en distribuerad partikel simultan lokaliserings- och karteringsalgoritm (d p - s l a m) som utgör en flexibel plattform för 2-D navigering som i framtiden kan utökas med fler sensorer. De experimen-tella resultaten som presenteras i rapporten visar på att det utvecklade systemet i många fall kan förbättra presentandan hos ett fotmonterad och tröghetnavigeran-de positioneringssystem avsevärt.

The här examensarbete har resulterat i ett konferensbidrag som är inskickat och accepterat till den internationalla konferensen för inomhuspositionering och in-omhusnavigering (IPIN 2014) [1].

(8)

(9)

Abstract

Reliable and accurate positioning systems are expected to significantly improve the safety for first responders and enhance their operational efficiency. To be effective, a first responder positioning system must provide room level accuracy during extended time periods of indoor operation.

This thesis presents a system which combines a zero-velocity-update (z u p t) aided inertial navigation system (i n s), using a foot-mounted inertial measure-ment unit (i m u), with the use of opportunistic multi-frequency received signal strength (r s s) measurements. The system does not rely on maps or pre-collected data from surveys of the radio-frequency (r f) environment; instead, it builds its own database of collected r s s measurements during the course of the operation. New r s s measurements are continuously compared with the stored values in the database, and when the user returns to a previously visited area this can thus be detected. This enables loop-closures to be detected online, which can be used for error drift correction. The system utilises a distributed particle simultane-ous localisation and mapping (d p - s l a m) algorithm which provides a flexible 2-D navigation platform that can be extended with more sensors. The experi-mental results presented in this thesis indicates that the developed r s s s l a m algorithm can, in many cases, significantly improve the positioning performance of a foot-mounted i n s.

The work in this thesis have resulted in a research paper, which has been sub-mitted and accepted to the international conference on indoor positioning and indoor navigation (IPIN 2014) [1].

(10)

(11)

Acknowledgments

First I would like to thank my main supervisor at FOI Jouni Rantakokko for all the time and work he has put into helping me with this master’s thesis. I would also like to thank my examiner Gustaf Hendeby and supervisor at the university Martin Skoglund for fruitful discussions and helpful comments. Additionally, I would like to thank my other supervisors at FOI: Patrik Eliardsson, Joakim Rydell and Erika Emilsson for valuable insights and comments throughout the course of the work.

This thesis has been a great collusion to my five years of electrical engineering studies at Linköping University. I would like to express my deepest gratitude to my family, my partner Emma and my friends for their continuous support and engorgement during my academic studies.

Linköping, September 2014 Martin Nilsson

(12)

(13)

Notation

A b b r e v i a t i o n s

Abbreviation Description

DP-SLAM Distributed Particle SLAM

FOI Swedish Defence Research Agency GNSS Global Navigation Satellite System

GP Gaussian Process

GPS Global Positioning System IMU Inertial Measurement Unit

INS Inertial Navigation System MEMS Micro Electro-Mechanical System

PDR Pedestrian Dead Reckoning PF Particle Filter

RF Radio Frequency

RMSE Root Mean Square Error RSS Received Signal Strength

SLAM Simultaneous Localization And Mapping ZUPT Zero Velocity Update

(16)

(17)

1

Introduction

A reliable and accurate positioning system is expected to be able to significantly improve the safety for first responders and to enhance their operational efficiency. To achieve this, a first responder positioning system must be able to provide at least room level accuracy during extended indoor operations and in other global navigation satellite system (g n s s)-challenged environments. There are also other important requirements besides raw performance, such as weight and cost, to consider when designing a personal positioning system. The system should be able to operate in unknown environments without relying on any pre-installed infrastructure [2]. An overview of the challenges associated with reliable indoor positioning for first responder applications, as well as sensor types that could be considered when implementing such systems, is given in [2].

Wearable mobile indoor positioning systems, intended for first responders and other pedestrian applications (such as location-based services, pervasive gaming, and navigation support), have received increased attention the last decade or so. A first approach to the problem is to use inertial sensors. With the rapid progress of sensor miniaturization (micro electro mechanical systems, m e m s) technology, in-ertial measurement units (i m us) have become both small and affordable enough to be used in pedestrian and professional applications. Currently, operational systems employ pedestrian dead-reckoning (p d r) type of algorithms [3, 4]. In these the data from the accelerometers are used to detect when the person takes a step and to estimate the step length, while gyroscopes and magnetometers are used to determine the orientation of the i m u. Motion classification/models can then be applied to decide upon the direction and length of the step.

Lately, many research groups have turned their focus towards foot-mounted i m us, which has the potential to provide improved accuracy and robustness

(18)

2 1 Introduction

[2]. An early example using shoe-mounted inertial and magnetic sensors was presented in [5], and since then several similar systems have been developed [6, 3, 7, 8, 9]. These systems use inertial navigation to estimate the user’s position and rely on a technique called zero-velocity-updates (z u p t) to detect and utilise the stand still phase of the foot during each gait cycle. This technique limits the drift caused by double integrating imperfect measurements, which is performed in an inertial navigation system. However, z u p ts cannot completely alleviate the effects of the drift which results in a degrade in the position estimate over time and/or travelled distance [9]. To obtain long time stable estimates, additional sensor information must be used.

1.1 Background

One approach to improve the long term stability of indoor positioning systems is to incorporate information about beforehand measured received signal strength (r s s) maps or floor plans to support the positioning. In [10] and [11] methods for acquiring r s s measurements to create maps are described and the r s s maps are represented using Gaussian processes (g p, [12]). An early example of utilizing such map information is the RADAR system [13]. These methods have gained attention as they are suitable for implementation on smartphones, which have the required sensors. Relying on pre-collected map information works well where this is possible [14], e.g., in shopping malls or office buildings [15]. However, first responder applications are more demanding as r s s maps cannot be obtained be-forehand, and if they exist, are prone to change. In these situations simultaneous localization and mapping (s l a m) approaches can be employed instead.

There are several s l a m systems relying on r s s measurements, e.g., WiFi Graph-SLAM [16], which is an extension of GraphGraph-SLAM [17], to use r s s measurements from WiFi. In the g p based s l a m solution in [18] and the solution in [19] information from both previously known and unknown WiFi access points are used for positioning. r s s measurements have also been used in conjunction with foot-mounted i n s systems, in the WiSLAM method [20].

WiFi r s s measurements may fail in many first responder scenarios for several reasons, the electricity in the building may be lost or WiFi may not be installed at all. A less explored alternative is to employ multi-frequency r s s measurements of other opportunistic radio sources, e.g., f m radio and t v stations. These signals are more or less available everywhere. The study presented in [21] indicates that a combination of f m and WiFi r s s measurements can be used to distinguish between different rooms with a high level of accuracy. These results were collab-orated by [22], which, however, concludes that the r s s may vary significantly over time. A s l a m system utilizing opportunistic radio signals to navigate a small robot is described in [23], where the r s s map is modelled as a piece-wise linear function. A s l a m system, based on an i m u, magnetometer, and f m r s s measurements and using a p d r, is described and evaluated in [24].

(19)

1.2 Purpose and Scope 3

1.2 Purpose and Scope

In this thesis the problem of integrating opportunistic multi-frequency r s s mea-surements with a z u p t-aided foot-mounted i n s is studied. An experimental foot-mounted i n s is provided by f o i. The main goal of this thesis is to develop a method that (apart from the i n s) only relies on inexpensive radio equipment, and is able to function without any prior knowledge of the environment in which it will be deployed.

1.3 Available Hardware

This thesis will use the FUNCube Dongle Pro+ [25] as its main radio receiver. The FUNCube is a small portable software defined radio which is plugged in to a PC directly via the USB port. The FUNcube is displayed in Figure 1.1a. The

(a)FUNCube Dongle Pro+ (b)Omnidirectional antenna

(c)MicroStrain 3DM-GX3-25

Figure 1.1:Available hardware

frequency coverage is 150 kHz to 240 MHz and 420 MHz to 1.9 GHz and is thus able to operate within the parts of the VHF and UHF bands where FM and TV signals are typically located. The maximum sample rate is 192 kHz, which was used throughout the thesis work.

(20)

4 1 Introduction

Unfortunately, its performance is not specified in any dataset. The same antenna was used in the feasibility study in conducted by [22] and the performance impact of rotating and showing the antenna from different directions is studied there. They conclude that the relatively poor quality of the antenna could potentially have some negative impact on performance if used in an positioning system. However, it was deemed sufficient for use in this feasibility study.

The i m u that is used in this thesis is a MicroStrain 3DM-GX3-25 [26], see Fig-ure 1.1c. It is equipped with three-axis accelerometer and gyroscope as well as a magnetometer. The dynamic range of the accelerometers are 18 g and the dynamic range of the gyroscopes are 1200◦/second.

1.4 System Overview

Figure 1.2 shows the basic layout of the system considered in this thesis.

EKF IMU Foot-mounted INS DP-SLAM Engine Radio Receiver Thesis work

Figure 1.2:Overview of data flow in the system

The inner workings of the foot-mounted i n s, the associated extended Kalman filter (e k f) and the strap down mechanization algorithms are not part of the thesis work. It is however important to understand how they tie in together with the FUNcube radio receiver and the developed s l a m algorithm, in Figure 1.2 referred to as the d p - s l a m Engine. A brief introduction of the mechanics of foot-mounted i n s in general is therefore provided in Section 2.1. The horizontal position estimates from the e k f are directly used as input signals to the d p -s l a mEngine, details of how these are then processed are provided in Chapter 3.

1.5 Thesis Outline

The thesis is organised the following way: after this introduction, the underlying theory for the developed r s s s l a m algorithm is presented in Chapter 2. This is followed by a presentation of the algorithm along with concepts and design choices used in the implementation in Chapter 3. The validation methods, ex-perimental setup and results are all presented in Chapter 4. Finally, concluding remarks as well as suggestions for future work is provided in Chapter 5.

(21)

2

Theory

In this chapter the basic underlying concepts and theory which are used through-out the report are presented.

Information on the basic principles of foot-mounted i n s are provided in Sec-tion 2.1. Theory for Gaussian processes, a generalisaSec-tion of the Gaussian distri-bution, is introduced in Section 2.2. Section 2.3 starts by presenting underlying theory for electromagnetic signals, which eventually leads to the introduction of the r s s fingerprints and how the spatial variations of r s s can be modelled using g p_{s. Finally, Bayesian filtering and the s l a m concept are briefly presented in} Section 2.4.

2.1 Foot-mounted Inertial Navigation Systems

This section provides an introduction to the inner workings of foot-mounted i n s. An implemented algorithm generally consist of the following steps: the i m u’s gyroscope measurements are integrated to get orientation, which is then trans-formed to an earth-bound coordinate system. The orientation of the i m u with respect to the earth is required to be able to correctly compensate for the influ-ence of gravity in the accelerometer measurements. After gravity compensation the accelerometer measurements are also integrated, once to obtain velocity and once more to get position. An outline of the described procedure is provided in Figure 2.1.

m e m s-based i m u sensors are however far from perfect and their measurements are always noisy as well as biased to some degree. Also, orientation errors will result in an imperfect compensation of the gravity effect on the acceleration mea-surements. These effects will cause any system that is relying on integration of

(22)

6 2 Theory Gyro-scope Accel-erometer

∫

Orientation Position Velocity Gravity compensation Initial

velocity positionInitial

Transform to “earth-bound”

coordinate system

Figure 2.1:Dead-reckoning using accelerometers and gyroscopes.

its gyroscope and accelerometer measurements to quickly diverge. There are dif-ferent ways to address this problem and one is a technique called zero-velocity updates (z u p ts) which can be implemented to constrain the error growth in a foot-mounted i n s. Another important aspect is that the i m u should have large dynamic range, this is especially important if it is foot-mounted as the foot is sub-ject to rapid variations in velocity and angular rate even during normal walking.

2.1.1 Zero-Velocity-Updates

When walking or running the human foot is stationary for a short time when it is in contact with the ground. The idea behind zero-velocity updates is to detect this moment when the velocity of the foot is approximately zero and use this information to update the states in whatever positioning filter is being used. At the moment when the user’s foot hits the ground and zero velocity is detected the entire 3D velocity vector, as well as the pitch and roll angles, becomes observable which can then be used to constrain the position error in the system. For a z u p t-aided i n s to work well however there are some general limitations:

• The foot should be on the ground (stand still phase) often. • A robust zero-velocity detection algorithm is needed.

• Performance depends on the user’s movements (but to a lesser degree than for p d r-type systems).

Example 2.1 shows how z u p ts can be integrated into a foot-mounted i n s. 2.1 Example: Zero-velocity-update integration

An i m u, with three-axis accelerometers and gyroscopes, is mounted on the foot of the user. An extended Kalman filter (e k f) is used to estimate the users position, and a zero velocity detection algorithm is introduced, which takes the gyroscope and accelerometer signals as inputs.

(23)

2.2 Gaussian Processes 7 Gyro-scope Accel-erometer Extented Kalman Filter ZUPT-Detector v = 0

Figure 2.2:Foot-mounted ins implementation with zupts.

There are several way of constructing zero velocity detection algorithms but one which relies on both gyroscope and accelerometer data usually performs better than those relying on just one of these sensors, see [9]. Whenever zero velocity is detected this is introduced as a pseudo measurement in the e k f, see Figure 2.2.

2.2 Gaussian Processes

A Gaussian process (g p) is defined by [12] as:

“a collection of random variables, any finite number of which have consistent joint Gaussian distribution”.

They are a natural generalization of the Gaussian distribution which is a distribu-tion over scalars or vectors whereas a Gaussian processes can instead be thought of as a distribution over functions. A g p is fully specified by its mean function M_{(x) and its covariance function K(x, x}0₎1_{. The expression}

F ∼ GP_{(M, K),} _(2.1)

should then be interpreted as: “the function F is distributed as a g p with mean function M and covariance function K”.

Given a set of N (possibly noisy) training samples from F , and a prior in the form of a mean function and covariance function, g ps can be used to infer information about unobserved function values. Let x be a set of N training inputs and let

f := F (xi), i = 1, 2, . . . , N be the set of known training samples. Analogously, let

x∗be a set of M test inputs and let f∗ := F (x

j

∗), j = 1, 2, . . . , M be the unobserved function values corresponding to this set of test inputs. The joint distribution of

(24)

8 2 Theory f and f∗is f f∗ ! = N µ µ∗ ! , Σ Σ∗ Σ∗T Σ∗∗ !! , (2.2)

where µ = M(xi_{), for i = 1, . . . , N , and analogously for µ}

∗. The matrix covariance matrix for all training samples, Σ, is computed by evaluating the covariance func-tion K(x, x0

) with all possible combinations of input points from the training input set x. The matrices Σ∗ and Σ∗∗ are also computed by evaluating the covariance function but in the case of Σ∗instead using all possible combinations of training and test inputs K(x, x0∗), and in the case of Σ∗∗using all possible combinations of test inputs K(x∗, x

0

∗). The conditional distribution of f∗given f then becomes

f∗|f ∼ N µ∗+ ΣT∗Σ −₁ (f − µ), Σ∗∗− ΣT∗Σ −₁ Σ∗ , (2.3)

which is the posterior distribution for specific set of test cases. More generally, the corresponding posterior process given the training data set D (both inputs and observed function values), is

F |D ∼ GP_(M_D_{, K}_D_), M_D_{(q) = M(q) + Σ}_∗_{(q, x}0₎T_Σ−1_{(f − µ),} _(2.4a) KD(q, q 0 ) = Σ∗∗(q, q 0 ) − Σ∗(q, x 0 )TΣ−1Σ∗(q, x 0 ), (2.4b)

where MDand KDdenotes the posterior mean and covariance functions, respec-tively. The input argument q is used to emphasise that the posterior process can be computed for an arbitrary set of test cases. The Σ∗ and Σ∗∗matrices have in-put arguments in (2.4a) and (2.4b) to emphasise that these depend on the test inputs, and if the test inputs are changed the matrices have to be re-computed. The expressions in this section can also be found in e.g., [12].

2.2 Example: 1-D Gaussian process inference

To better illustrate how Gaussian processes work consider the following example: F_{(x) is a 1-D function and its values can be observed with some noise according} to

yobs(x) = F (x) + ,  ∼ N (0, σn2) (2.5)

Lets assume only ten samples of yobs(x) are available and the objective is to

esti-mate more values of the underlying function F (x) in an interval [−5, 5]. Figure 2.3 displays F (x) together with ten noisy measurements.

To attempt to model this function using a g p one first needs to decide on a mean and a covariance function. The mean function M(x) and covariance function K_{(x, x}0_{) are in this example chosen as}

(25)

2.2 Gaussian Processes 9 Example function −5 0 5 −5 0 5

Figure 2.3:True function values (line) and noisy measurements (circles).

M_{(x) ≡ 0} _(2.6) K_{(x, x}0_{) = σ}2 f exp − (x − x0)2 2l2 ! + σ_n2δxx0 (2.7) where θ = (σn, l, σf) are the so called hyper-parameters of the kernel function and δxx0denotes the Kronecker delta. The kernel function which is known as the Gaussian or squared exponential kernel is sometimes used as a standard choice in g ps and one of its main characteristics is that it assumes smooth functions. The mean function is in this case set to zero as there is no information available about the underlying structure of the function F . The kernel hyper-parameters θ can be chosen manually or be estimated from data by maximizing the likelihood of the observations given the parameters [12]. In this example the hyper-parameters were chosen manually as θ = (0.5, 1, 4). Figure 2.4 shows the gp before the obser-vation are taken into account, the mean is zero everywhere and the uncertainty is governed only by σf in the kernel.

Example function with GP prior

−5 0 5

Figure 2.4: True function values (line), noisy measurements (circles), gp mean (dashed line) and 95% percent confidence region.

(26)

10 2 Theory

Figure 2.5 shows the results when the g p is updated with the noisy observations using (2.4). The g p mean closely follows the true function and the uncertainty estimate is significantly lower.

Example function with GP posterior

−5 0 5

Figure 2.5: True function values (line), noisy measurements (circles), gp mean (dashed line) and 95% percent confidence region.

MATLAB® code to reproduce the results of this example is provided in Ap-pendix A.

2.2.1 Hyper-parameter Selection

According to [12] it is possible, given a set of measurement values y, their cor-responding positions x and a kernel function K(x, x0), to make inference about the kernel’s hyper-parameters θ = θ1. . . θk. The log marginal likelihood of the measurement values given their positions and the hyper-parameters is2

L = log p(y |x, θ) = −1 2log |Σ| − 1 2y T_Σ−₁ y −N 2 log(2π) (2.8)

where Σ is the covariance matrix produced using the kernel and the N data points. It is now possible to find the hyper-parameter values that maximises (2.8) based on its partial derivatives which are given by

∂L ∂θi = 1 2tr (Σ−1y)(Σ−1y)T − Σ−1 ∂Σ ∂θi ! (2.9) which can be used together with optimization routines to find good parameter values. For those familiar with the field of system identification it is worth to note that the expression in (2.8) balances model fit and complexity automatically. The first term in (2.8) is the complexity penalty term. The second term is the data-fit measure and it is the only term which depends on the measurement data y. The third term is simply a normalization term which does not depend on the data 2_{Here the mean function is assumed to be M(x) ≡ 0 for simplicity. If the mean function is not} zero, (2.8) and (2.9) must be altered slightly to accommodate this. Also if the mean function has hyper-parameters on its own it is possible to make inference about these as well given the data. See [12] for more details.

(27)

2.3 RSS for Indoor Positioning 11

or the hyper-parameters values in any way which makes it obsolete during an optimum search.

2.2.2 Sparse Approximation Methods

A well known problem with Gaussian processes for online applications is the computational complexity. The main bottleneck is the inversion of the covariance matrix which has computational complexity O(N3) where N is the number of data points used to construct the g p. Fortunately this is a well known problem within the field of online Gaussian processes and one of the more popular solutions is to use a sparse approximation of the full g p. There are many possible ways of constructing these sparse approximations and a number of available methods are compared and analysed in [27]. This thesis picks up on the ideas used in [28] where an assumption is made not about the conditional distribution or the likelihood but instead of the structure of the kernel function itself. Consider the radial basis kernel function (r b f)

K_{(x, x}0_{) = σ}2 f exp − |_{x − x}0|2 2l2 ! , (2.10)

which is simply a higher dimension equivalent of the squared exponential kernel introduced in Example 2.2. This kernel function has local support which means that entries in the covariance matrix K corresponding to measurement locations which are far away from each other will be close to zero. What is meant by far away is determined by the length scale parameter l in (2.10), which determines how quickly the correlation between two points drop off.

If the correlation between two points would instead be exactly zero, and not just close to zero after a certain distance, this would enable efficient implementations of g ps since only measurements from positions close to a prediction position would have to be included when constructing the g p. This can be achieved by instead using kernels with compact support. The r b f kernel presented in (2.10) can be compactified as:

K_{(x, x}0_{) = σ}2 f exp − |_{x − x}0|2 2l2 ! max ( 0, 1 − x − x0 d !ν) (2.11) where d defines the compact region over which the kernel has support and ν is an odd number set to either n or n + 1 where n is the dimension of the input to the g p. By compactifying the kernel in this way it is guaranteed to remain positive definite [29]. By using (2.11) and only data which is inside the compact region the results will be almost as accurate as when also including data outside of the compact region.

2.3 RSS for Indoor Positioning

This section introduces the concept of using signal strength measurements for indoor positioning. Section 2.3.1 introduces basic theory for electromagnetic

(28)

12 2 Theory

signal propagation. Section 2.3.2 explains the mechanics of sampling r s s from bandpass signals, such as f m radio and TV signals. Section 2.3.3 introduces the r s s fingerprint, which revolves around the notion that r s s measurements, especially when collect at many frequencies, can be used as an identifier for specific locations in indoor environments. Finally, Section 2.3.4 presents the idea of using Gaussian processes for modelling the complex spatial variations of r s s in indoor environments.

2.3.1 Electromagnetic Signal Propagation

In wireless communication systems electromagnetic energy is emitted and re-ceived via the use of antennas. The antennas vary in size depending on the frequency of the operation. According to [30, p. 6] the antennas must be longer than ₁₀1 of the signal wavelength to obtain efficient radiation of electromagnetic energy. There are three basic ways that electromagnetic signals, such a radio waves, interact with the environment [31, p. 39]:

• Reflection occurs when a radio wave hits an object which has larger di-mensions than the wavelength of the wave. Any type of object can cause reflections, including the surface of the earth.

• Scattering occurs, in contrast to reflection, when a radio wave travels trough a medium containing a lot of small objects compared to its wavelength. • Diffraction occurs when a radio wave hits an object with irregularities in its

surface, such as rooftops or building corners. Secondary waves are then irra-diated from the surface causing a bending of waves around the obstructing object.

A common problem in wireless communication systems is signal multipath. Sig-nal multipath occurs when a transmitted sigSig-nal interacts with the environment in such a way that multiple, distorted, copies of the signal arrives at the receiving antenna at different time delays. Not only does the delay itself cause problems but there is a chance that signal components arriving via different propagation paths may add destructively at the receiver. This results in a phenomenon called signal fading and it is the very reason that the r s s of a radio signal varies a lot even on short distances in indoor environments, which are inherently multipath-rich.

2.3.2 RSS of Bandpass Signals

According to [30, p. 20] a lowpass or baseband signal is a signal whose spectrum is located around the zero frequency. Examples from every day life would be speech and music, which are both lowpass signals although they have different spectral characteristics and bandwidth. The bandwidth of a real valued lowpass signal is the minimum positive w such that X(f ) = 0 outside [−w, w]. A bandpass signal is a real signal which frequency content is located at some frequency ±f0 which is far from zero. To put it more formally first define the positive and the

(29)

negative spectrum of the signal x(t) as

X+(f ) =            X(f ), f > 0 1 2X(0), f = 0 0, f < 0 X−(f ) =            X(f ), f < 0 1 2X(0), f = 0 0, f > 0 (2.12)

a bandpass signal is a real signal for which there exist positive f0and W such that its positive spectrum X+(f ) is non-zero only inside [f0−w/2, f0+ w/2] and where

w/2 < f0. Every day examples of bandpass signals which are also of interest in this thesis is f m radio and TV signals.

Applying digital signal processing algorithms directly on bandpass signals would in most cases be computationally intractable since the required sampling rate would be much too high. However any bandpass signal x(t) can be represented by a lowpass equivalent xl(t) of the original signal [30, p. 18]. The lowpass

equivalent signal can be expressed as

xl(t) = xi(t) + jxq(t) (2.13)

where xi(t) is called the in-phase component and xq(t) is called the quadrature

component. These components can often be received as outputs from software defined radios. The signal envelope and phase can be expressed as

rx(t) = q x2_i(t) + x2q(t) (2.14) θx(t) = arctan xq(t) xi(t) ! (2.15) and the received signal strength of the radio signal expressed in Decibel-milliwatts (dBm) can be calculated as RSSx(t) = 10 ∗ log₁₀ r 2 x(t) 0.001 ! = 30 + 20 ∗ log₁₀(rx(t)) . (2.16)

2.3.3 RSS Fingerprints

An r s s fingerprint is a set of mean signal strength values collected at different frequencies. Generally a number of samples are recorded at each frequency and the mean values of the recorded sequences are stored in a vector. In short, if a mean r s s value at a specific frequency is Λjand the entire fingerprint vector is denoted by Λ then

Λ=Λ1 Λ2 . . . ΛnT (2.17) where n is the total number of frequencies used in the fingerprint. The reason for using a set of n frequencies instead of just one is that r s s values from one frequency is not very unique in a specific location. However, when a larger set is used, the locations can in many cases be directly associated with a specific

(30)

14 2 Theory

fingerprint with low uncertainty. 2.3 Example: RSS measurements

A small experiment was performed and 200 r s s samples at two different fre-quencies were collected simultaneously during a 40 second walk down an office corridor, the total travel distance was approximately 45 meters. Figure 2.6 shows the recorded r s s values for the two frequencies. It is clear that there are several

Samples S ig n al st re n gh t (d B m ) RSS at 95500 and 97300 kHz 0 50 100 150 200 −90 −80 −70

Figure 2.6:RSS values at two different frequencies

places in the corridor where approximately the same r s s value was recorded, for both frequencies. r s s values at only one frequency is thus not very suitable for identifying specific locations in the corridor. However, the two signals express different behaviour and takes on different values at different times during the walk through the corridor. By combining the r s s values of the two signals in a (fingerprint) vector, this would create a less ambiguous identifier for locations in the corridor than if individual frequencies alone are used.

Adding more frequencies to the set usually makes a better, in the sense of more unique, fingerprint. However, this is not perfectly true, and there are mainly two aspects to consider when choosing which frequencies to include in a fingerprint:

• Frequency separation

The frequency of a radio signal affects how it propagates in different en-vironments, as explained in Section 2.3.1. A good idea might then be to include signals from different frequency bands in the fingerprint to obtain a higher degree of frequency diversity.

• Transmitter location

Some signals present in the environment are transmitted from the same mast and there might then be a strong correlation between the collected r s svalues, provided the frequency separation is small.

Blindly adding more frequencies is thus not guaranteed to improve the fingerprint and some care should be put into the selection process [22].

(31)

2.3.4 Modelling RSS Spatial Variations

There are several ways one can go about to modelling the spatial variations of r s s _{in indoor environments. One possible way is to assume knowledge about} the locations of the transmitters and then model the signal propagation trough the environment in order to calculate the expected r s s at any given location. This approach is however ill suited to model the r s s of f m radio and TV signals since the transmitters are generally relatively far away from the receivers. This means that there is a high risk of the signal interacting with a lot of different objects/obstacles on the way which will impact the r s s in ways that are hard to predict and efficiently model.

Another approach is instead to compute measurement likelihoods by using cali-bration data from the environment in which r s s is going to be use for positioning. This requires a sound way of modelling the, often very complex, likelihoods that the spatial variations of r s s give rise to indoors. Gaussian processes are well suited for this task:

• Calibration data

g p_{s automatically handles noise in the calibration data and does not come} with any requirements on the data being collected in a certain pattern or from specific locations.

• Continuous estimation

g ps does not require a discrete representation of the environment, and can be used to produce predictions of the r s s at arbitrary locations.

• Uncertainty handling

An uncertainty estimate can be supplied for every predicted r s s value. The g pautomatically takes into account the spatial density and distribution of the calibration data as well as noise in the data points.

Example 2.4 demonstrates how the r s s from signals at different frequencies can be modelled using g ps.

(32)

16 2 Theory

2.4 Example: Modelling RSS with GPs

A small experiment was performed and 200 r s s samples at six different fre-quencies were collected simultaneously during a 40 second walk down an office corridor. A part of this data set was used previously; to produce the figures in Example 2.3. The data set was used to train individual g ps on each frequency. The g p mean function was in this example set to M(x) ≡ 0, and r b f kernel was used with hyper-parameters (σn, l, σf) = (2.5, 4, 8). Figure 2.7 presents the measured r s s at each frequency along with the g p posterior.

Samples S ig n al st re n gh t (d B m ) 106900 kHz Samples S ig n al st re n gh t (d B m ) 103200 kHz Samples S ig n al st re n gh t (d B m ) 99800 kHz Samples S ig n al st re n gh t (d B m ) 97300 kHz Samples S ig n al st re n gh t (d B m ) 95500 kHz Samples S ig n al st re n gh t (d B m ) 94400 kHz 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70

Figure 2.7:Calibration data (red line) and gp posterior mean (black line) and 95% percent confidence region.

(33)

2.3 RSS for Indoor Positioning 17 Samples S ig n al st re n gh t (d B m ) 106900 kHz Samples S ig n al st re n gh t (d B m ) 103200 kHz Samples S ig n al st re n gh t (d B m ) 99800 kHz Samples S ig n al st re n gh t (d B m ) 97300 kHz Samples S ig n al st re n gh t (d B m ) 95500 kHz Samples S ig n al st re n gh t (d B m ) 94400 kHz 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 0 50 100 150 200 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70 −95 −90 −85 −80 −75 −70

Figure 2.8:Calibration data (red line) and gp posterior mean (black line) and and 95% percent confidence region.

A second data set was also collected shortly after the first by repeating the walk down the office corridor. This second data set was used for validation and it is presented together with the GP posterior in Figure 2.8. The g ps manages very well to capture the spatial variation of the r s s for most of the included frequencies in this example. For some of the frequencies however the g p does not manage to capture all of the r s s variations fully, see for example Figure 2.8 around sample 25–50 for 106, 9 MHz.

(34)

18 2 Theory

2.4 State Estimation

In Bayesian filtering the goal is to compute the posterior distribution of the state vector given a set of observations. This is done by solving the general recursive Bayesian update equations

p(xk|_y_1:k_{) =} p(yk|x1:k)p(xk|y1:k−1) p(yk|y1:k−1) , (2.18a) p(yk|y1:k−1) = Z Rnx p(yk|xk)p(xk|y1:k−1) dxk, (2.18b) p(xk+1|y1:k) = Z Rnx p(xk+1|xk)p(xk|y1:k) dxk. (2.18c)

Equation (2.18a) corresponds to a measurement update, (2.18c) corresponds to a time update and (2.18b) is a normalisation constant. If the models are all linear and the process and measurements noise Gaussian, then (2.18a)–(2.18c) can be simplified to the Kalman filter update equations [32, p. 217]. However, if the models are nonlinear or if the noise is not Gaussian there is in general no finite dimensional representation similar to that of the Kalman filter.

The particle filter provides a way of numerically approximating the solution to (2.18) by representing the posterior distribution by a set of discrete weighted points called particles. The particle filter is a well established method for solving the Bayesian filtering problem, see [32] or [33] for a general introduction.

2.4.1 Simultaneous Localisation and Mapping

Simultaneous localisation and mapping (s l a m) refers to the problem of deter-mining ones position using observations of features (or landmarks) in the sur-rounding environment, while simultaneously also mapping the positions of the observed features.

In traditional target tracking or localisation applications the particle filter can be used to estimate the entire trajectory x1:k by simulating a set of N possible trajectories, each with an associated probability weight w_k|ki . The particle filter can also be extended to be used in a s l a m context; this means that every particle in the filter no longer only represents an hypothesis about the state vector, but also a map configuration. A general s l a m system can be described by the following equations

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

mk+1= mk, (2.19b)

yk = h(xk, mk) + ek, (2.19c)

where xk is the state, mk the map, uk known or measured input, vk process noise, yk a measurement, and ek measurement noise, all at time k. A presentation of

the chosen motion model (2.19a), dynamic map model (2.19b) and measurement model (2.19c) is provided in Chapter 3.

(35)

3

Method and Implementation

This chapter presents the developed s l a m algorithm. Section 3.1 introduces the basics of distributed particle s l a m which is the foundation of the s l a m method implemented in this thesis. The motion and measurement models are presented and discussed in Sections 3.2 and 3.3, respectively. Finally, an overview of the complete algorithm developed in this thesis is presented in Section 3.4 along with a discussion about important practical issues and design choices.

3.1 Distributed Particle SLAM

Distributed particle simultaneous localisation and mapping (d p - s l a m) [34] is a particle filter based s l a m method which uses an ancestry tree together with an occupancy grid. This way the ancestry of each particle and all its modifications to the grid can be tracked. If a particle is removed after re-sampling, then all its entries into the grid are also removed. When a particle after re-sampling is instead copied, there is no need to copy the entire map over to the new particle. Instead, a new branch will be created in the ancestry tree and the re-sampled particle and its siblings will share all entries made by their ancestors in the occupancy grid made up to that point.

3.1.1 Ancestry Tree

The ancestry is simply a way of letting every particle in the filter keep track of its lineage. Every particle maintains a pointer to its direct parent which allows particles to efficiently share map entries. To put this into a context of slam and particle filters; the tree is a representation of the estimated posterior over the

(36)

20 3 Method and Implementation

entire travelled trajectory up to time k:

p(x1:k|y1:k, m1:k). (3.1)

The trajectory for each individual particle x_1:ki is represented by a branch in the ancestry tree, which in turn always consists of two or more nodes. There are two types of nodes in the ancestry tree:

• Particle nodes are traditional particles in the sense that they contain a state vector but also a pointer to its closest parent. Particle nodes are always at the bottom of the tree (leaf nodes) and can never be parent nodes.

• Ancestor nodes serves as a sort of data storage. Each node contains a section of a particle’s travelled path, as well as information about map entries made along that path. Just as with particle nodes, ancestor nodes contain a pointer to its parent. Ancestor nodes can be both parent and child nodes, but can never leaf nodes. By following a branch in the ancestry tree, from the root node down to the particle node, it is possible to obtain the entire trajectory of the particle corresponding to that particular branch.

Figure 3.1 shows an example of how an ancestry tree might look after a few iterations just using 3 particles.

Figure 3.1:Ancestry tree example. Nodes with black fill are ancestor nodes, nodes without fill are particle nodes.

There are three operations which can be used to maintain and update the ancestry tree during each filter iteration:

• Pruning which consist of removing the particle node as well as all the an-cestor nodes which has no other lineage than the removed particle. The removal of a particle and information about its travelled path effectively means that the posterior in (3.1) shrinks and the filter becomes more certain about a section of the track.

• Trimming is performed directly after the pruning step and its purpose is to constrain the tree size by merging ancestor nodes where it is possible. The criterion for merging two ancestor nodes is that a parent node should only have one ancestor node as its child.

(37)

3.1 Distributed Particle SLAM 21

• Branching is performed when a particle is selected for re-sampling and multiple copies of it is created. The copying of particles in the filter is reflected in the ancestry tree by the creation of new ancestor and particle nodes, one of each for each new particle. The new ancestor node become direct children to the parent node of the particle which was copied. Example 3.1 presents a simplified scenario which demonstrates how the pruning and trimming operations modifies the structure of the tree and how this can be related to different particle trajectories.

3.1 Example: Ancestry tree

Consider the small ancestry tree presented in Figure 3.1, it is reintroduced in Figure 3.2a but with colour coded ancestor nodes. A map is also introduced in Figure 3.2b map where section of the paths for the different particle are also colour coded to their corresponding ancestor nodes in the tree.

(a) (b)

Figure 3.2:Ancestry tree and corresponding paths on map

The purple (or magenta) node in the ancestry tree is selected for removal. Fig-ures 3.3a and 3.3b displays how the removal of the node affects the ancestry tree and Figures 3.3c displays the corresponding changes in the map.

(a) (b) (c)

(38)

After removal the trimming operation starts and it finds that the green ancestor node now has an only child which is also an ancestor node. The green and the yel-low node are thus selected for merging, as display in Figures 3.4a. The trimming operation’s effects on the ancestry tree is display in Figures 3.4b and the changes to the particle trajectories on the map are displayed in Figure 3.4c.

(a) (b) (c)

Figure 3.4:Effects of trimming on ancestry tree and map

After the trimming step one of the two remaining paths would normally branch, so that the particle count is kept constant. This is not illustrated in this example but the effect would be very similar to how the red path slits into a blue and a green path in Figure 3.4c.

More information on exactly how the travelled trajectory can be estimated using the ancestry tree is presented in Section 3.4.5.

3.1.2 Occupancy Grid

The occupancy grid [35] is a spatial representation of the environment. It is a well established method within the field of mobile robotics, which was also the original application of the d p - s l a m algorithm [34]. The general idea is to decompose the high-dimensional mapping problem into a series of one dimensional problems, one for each grid cell, which are considered to be independent of each other. The concept is fairly intuitive in mobile robot applications; the map to be estimated is the physical environment. The robot is usually equipped with some kind of scanning equipment which can provide direct information about its environment, such as a laser range finder or a sonar. The problem is then to determine the probability of whether or not each grid cell is occupied e.g., by a wall or some type of furniture. The posterior distribution for the map at time k given the state vector x and measurements y up to that time k is:

p(mk|x1:k, y1:k), (3.2)

where m denotes the grid map. Due to the grid representation of the map the prob-lem breaks down to determining the probability of occupancy for each individual

(39)

3.2 Motion Model 23

cell, which is here indexed with (i, j), assuming a 2-D grid:

p(mi,j_k |_x_1:k_{, y}_1:k_). _(3.3) The usage of the occupancy grid in the algorithm presented in this thesis differs somewhat from the usage in traditional mobile robot applications. The measure-ment equipmeasure-ment is a radio, which can record r s s fingerprints at the user’s current location. The occupancy grid is not used to map physical objects, but is instead used as a means of representing the r s s field variations throughout the envi-ronment in which the user operates. As the user moves in the envienvi-ronment and collects r s s measurements these are stored in the occupancy grid. Since the grid is combined with a particle filter this means that at every iteration the particle cloud is spread out across a number of cells in the grid. When a measurement update occurs a r s s fingerprint from the user’s true location will be recorded. Every particle will then store the new fingerprint, together with their identity and exact location within the grid cell, into the grid cell which the are currently occupying.

There are several reason for using an occupancy grid in particle filter s l a m. First, the resolution of the grid directly limits the number of measurements which can be stored. This enables constraints to be put on the memory requirements of a s l a m_{algorithm by adjusting the grid resolution. When deciding on a suitable} grid size one must consider the type of measurements which are to be stored in the grid. For example, when dealing with r s s fingerprints there is no point in storing measurements once every hundred meters or once every millimetre. If one wants a system with around meter-level precision, storing r s s measurements in a grid with 0.5–1.0 meter should be sufficient.

Another reason for using an occupancy grid to store measurements is that it is easy to check if a particle has stored measurements in a specific area of the grid. It is not necessary to then search trough the entire measurement history of the particle. Instead, it is possible to look directly in grid cells of interest for measurements stored there by a specific particle. This is an especially useful feature for algorithms which at every iteration needs to fetch measurements which are stored in grid cells near a particles location in the grid.

3.2 Motion Model

The motion model can be described by the following equation

xk+1= f (xk, uk, vk), (3.4)

where xk is the state vector, uk is the input signal and vk is the process noise all

at time k. Equation (3.4) relates back to the general s l a m model presented in Section 2.4.1.

The system has access to an input signal as described in Section 1.4. A position estimate is continuously provided by the foot-mounted i n s and can be used as

(40)

an input to the particle filter. Using the i n s’s position estimates as an input the particle filter enables state such as the user’s velocity to be kept out of the filter’s state vector. This is good since the length of the state vector generally have a large impact on the number of particles needed in the filter. The task of the particle filter is not to directly estimate states related to the user, such as velocity and orientation. Instead, its purpose is to compensate for the long term error drift in the i n s. It is however still necessary to keep user’s position in the state vector since it is required for grid mapping.

The error accumulation model for the i n s is based on the model for z u p t-aided foot-mounted i m us suggested by [36], and similar error accumulation behaviour has also been observed by others, see for example [37]. The model used in this thesis makes the following assumptions:

• All position errors are due to heading drift.1

• The heading error can be modelled with constant growth rate. • The heading error is a function of time, not distance.2

The state vector is given by

xk = (px, py, β, ˙β)T (3.5) and consists of four state: 2D position (px, py), a heading bias β and a heading bias rate ˙β. The motion model uses an input denoted uk which is calculated

using the last and the new position estimates from the Kalman filter presented in Section 1.4. The input consist of a travel distance and a heading angle

uk =       p I N S k+1 −pkI N S ∠_pI N S k+1, pI N Sk      = rk φk ! , (3.6)

where ∠( · , · ) is the quadrant compensated arctangent function. New inputs are only provided by the i n s during z u p ts, practically this means that if the user is walking or running new input are only provided when the foot with the i n s is on the ground. This means that the update time T is variable and depends on the

1_{This is an approximation and technically there is also a small error in the distance between} updates. This simplified model is used as a first step to simplify the evaluation of the developed algorithm, in the future more advanced models could be considered.

2_{The error characteristics depends on the implementation. For instance, some implementations} of foot-mounted i n s do not allow the error to grow during a z u p t. This assumption might thus be ill suited for such implementations, especially in scenarios where the user stands still for extended periods of time. A better model would then be to let the error grow with the travelled distance instead. Furthermore, the type of movement can significantly affect the error statistics.

(41)

3.3 Measurement Model 25

user’s movements.3 The complete motion model is given by

xk+1= xk+             r cos(φ − β) r sin(φ − β) T ˙β 0             +              0 0 T2 2 T              vk, (3.7)

and T is the time difference between updates. The process noise is distributed according to vk ∼ N(0, Q) and it models the errors in the i n s estimate. The

process noise covariance, Q, should reflect typical i n s error behaviour and can be considered a tuning parameter.

3.3 Measurement Model

Following the general s l a m model presented in Section 2.4.1, the measurement model can be described by the following equation

yk = h(xk, mk) + ek. (3.8)

The measurement at time instant k is an rss fingerprint vector, yk =

y_k1 . . . y_knT. Each component, y_kj = Λj_k+ e_kj, contains a mean r s s value for a frequency as de-scribed in Section 2.3.3 and ej_k ∼ N_{(0, σ}_e2_{) is the measurement noise for frequency} component j.

The measurement model uses Gaussian processes as its core component. The g ps are trained using previously recorded measurement and are then directly used to generate the new measurement likelihoods for every particle. The output of a Gaussian processes for any given input is a Gaussian distribution; the predicted measurement for particle i at time instant k is thus distributed according to

h( ˆxi_k|k−1, ˆmi_k|k−1) ∼ N ( ˆy_ki, Ωi_k). (3.9) Here ˆy_ki is the predicted measurement, Ωi_kis the uncertainty and ˆx_k|k−1i and ˆmi_k|k−1

are the predicted state and map for particle i, respectively, given all but the last measurement. Note that the r s s at each individual frequency in the fingerprint is modelled as being independent and individual g ps are used to predict the r s s at each frequency. This means Ωi_k will always be a diagonal covariance matrix.

3.3.1 GP construction

To create the g ps to generate a measurement prediction for a particle, previously recorded r s s fingerprints and their corresponding location coordinates, which 3_{It is possible to extract position estimates from the i n s in between z u p ts, and this would allow T} to be kept constant. During normal walk, when the time between z u p ts is relatively short, this could work well. However, the position estimates can get very uncertain if the user moves in another way where the time between z u p ts is longer, e.g., if the user starts to crawl. In such a scenario there is a risk of r s s measurements getting mapped to inaccurate positions, which would have a negative impact on filter performance.

(42)

together makes up the map landmarks, are fetched from the occupancy grid. The set of map landmarks, which are available to construct a g p for a given particle i and time instant k, is

mi_k = p1 p2 . . . pD Λ₁ Λ₂ . . . Λ_D !

, (3.10)

where D denotes the total number of collected landmarks. In (3.10) p is a vector containing 2D position coordinates and Λ is the fingerprint vector associated with that position. The number of recorded values grows quickly during operation and after a short time it becomes computationally intractable to use all of them to create g ps for every particle and at every new iteration. This problem is addressed by using the compactified radial basis kernel function, presented in Section 2.2.2, with added measurement noise to construct the g p covariance matrix. The kernel function is given by K_{(x, x}0_{) = σ}2 f exp −|x−x 0 |2 2l2 max 0,1 − x−x0 d ν + σn2δxx0, (3.11) where δxx0 is the Kronecker delta function. By using the kernel in (3.11) only a subset of all available landmarks, ˜mi_k ⊂ _mi

k, which are close to the position

coordinates in the predicted state ˆxi_k|k−1needs to be collected. Exactly what is meant by close is defined by the parameter d in (3.11). The hyper-parameters are pre-selected manually, to save computation time and to ensure feasible values. The implication of this manual selection is briefly investigated and discussed in Section 4.3.

The mean function in the g p is chosen as the mean value of the r s s values in ˜mi_k

at each frequency. Note that the covariance function only depends on the position coordinates in ˜mi_k and the g p hyper-parameters. This means that if the same hyper-parameters are used for all frequencies, the g p covariance matrices will be identical for all frequencies in the fingerprint.

3.3.2 Measurement Estimation

When individual g ps have been constructed for each frequency it is then straight-forward to compute the predicted measurement ˆy_ki and the associated uncertainty Ωi_k using (2.4a) and (2.4b), respectively. The likelihood of the recorded measure-ment yk given the predicted state and map for particle i is

p(yk|_{xk|k−1, ˆ}_ˆ _mk|k−1_{) =}exp −1 2(yk−yˆ_ki)T(Ω_ki + R)−1(yk−yˆ_ki) (2π)n/2q Ω i k+ R (3.12)

where n is the number of frequencies included in the fingerprint, and R = I σe2is

(43)

3.4 Algorithm 27

3.4 Algorithm

Algorithm 1 displays the main steps in the developed S L A M algorithm. At the start of every iteration a new input signal and a new RSS measurement is available. Details about important steps in the filter algorithm as well as the design choices are presented here.

Algorithm 1:SLAM algorithm main loop fori = 1, 2, . . . , N do xi₀= x0; /* Initialisation */ wi₀= 1/N ; whileRunning do b N = PN1 i=1(wi)2 ; if bN <2₃N then

Re-sample with replacement; fori = 1, 2, . . . , N do wi_k = 1/N ; fori = 1, 2, . . . , N do xi_k ∼_p(x_k|_x_k−1_{) ;} _{/* Time Update */} wi_k = w_k−1i ps(yk|x_ki, mi_k−1) ; /* Measurement Update */ ck =PN1 wik; fori = 1, 2, . . . , N do wi_k = wk/ck ; /* Normalize Weights */ ˆ

xtraj=PNi=1wikx1:ki ; /* Trajectory update */ fori = 1, 2, . . . , N do

xi_k ←_{Store y}_k _{in grid ;} _{/* Map Update */}

3.4.1 Initialisation

At the initialisation of the algorithm all particle weights are set equal and the initial states for the particles are all set to x0 = (0, 0, 0, 0)T. Another possible approach is to generate the initial state by drawing samples from an initialisation distribution, xi₀ ∼ _px

0, i = 1, 2, . . . , N , to reflect that there is uncertainty in the initial state. This is not done here since the primary purpose of this algorithm is to estimate the heading error of the i n s and the i n s’s start position and heading is manually calibrated in all tests present in Chapter 4. The starting position and heading errors are thus assumed to be negligible.

The heading bias drift rate ˙β, which is not affected by the manual calibration of

starting position and heading, is set to zero initially even though it is technically unknown at start-up. In tests however, it has been observed to be very low initially and experimentation with an initial Gaussian distribution for this state only, ˙β ∼

(44)

3.4.2 Particle Re-sampling

The re-sampling step is costly since it involves pruning old entries from the occu-pancy grid as well as trimming the ancestry tree. To evaluate when it is necessary to re-sample, the number of effective particles is estimated at every iteration as

b

N = _P_N 1 i=1(wi)2

where N is the total number of particles in the filter and wi is the probability weight of particle i. See [32, p. 225] for a motivation. If bN < 2₃N , then

re-sampling will begin and afterwords all particle weights are set equal.

3.4.3 Time Update

In a particle filter the time update is performed by sampling from the so called proposal distribution q(xk+1|xk, yk+1). In this thesis the motion model described

in Section 3.2 will be used as proposal distribution, that is:

q(x_k+1i |_xi

k, yk+1) = p(xik+1|xik, uk) (3.13)

which has the effect that the particle weights are unchanged by the time update:

wi_k+1|k= wi_k|k. This approach is sometimes referred to as prior sampling.

3.4.4 Measurement Update

The prior sampling approach has the following effect on the particle weights during the measurement update step:

wi_k|k= w_k−1|k−1i p(yk|xik, m i

k−1) (3.14)

After the update the probability weights are no longer guaranteed to sum up to one, which is why the weights are normalized after this update step. The mea-surement model p(yk|xki, mik−1) computes the likelihood of the new measurement yk given the new particle state xikand the map from the previous iteration mik−1.

The measurement model was presented in Section 3.3.

There is a practical issue with updating the particle weights this way, which was observed during testing, and it could be traced back to assumptions made in the measurement model. The measurement model uses the approximation that the r s s of every frequency is independent, this can result in the likelihood,

p(yk|xki, mik−1), becoming highly peaked. In the particle filter this results in overly

confident estimates. The problem has also been observed by [11] and it is ad-dressed in a similar way here. Instead of plugging the measurement likelihood

Indoor Positioning Using Opportunistic Multi-Frequency RSS With Foot-Mounted INS

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Indoor Positioning Using Opportunistic

Multi-Frequency RSS With Foot-Mounted INS

Indoor Positioning Using Opportunistic

Multi-Frequency RSS With Foot-Mounted INS

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan vid Linköpings universitet

av

Sammanfattning

Abstract

Acknowledgments

Contents

Notation

1

Introduction

1.1

Background

1.2

Purpose and Scope

1.3

Available Hardware

1.4

System Overview

1.5

Thesis Outline

2

Theory

2.1

Foot-mounted Inertial Navigation Systems

∫

∫

∫

2.1.1

Zero-Velocity-Updates

2.2

Gaussian Processes

2.2.1

Hyper-parameter Selection

2.2.2

Sparse Approximation Methods

2.3

RSS for Indoor Positioning

2.3.1

Electromagnetic Signal Propagation

2.3.2

RSS of Bandpass Signals

2.3.3

RSS Fingerprints

2.3.4

Modelling RSS Spatial Variations

2.4

State Estimation

2.4.1

Simultaneous Localisation and Mapping

3

Method and Implementation

3.1

Distributed Particle SLAM

3.1.1

Ancestry Tree

3.1.2

Occupancy Grid

3.2

Motion Model

3.3

Measurement Model

3.3.1

GP construction

3.3.2

Measurement Estimation

3.4

Algorithm

3.4.1

Initialisation

3.4.2

Particle Re-sampling

3.4.3