Inertial Systems Based Joint Mapping andPositioning for Pedestrian Navigation

Patrick Robertson†, Michael Angermann†, Bernhard Krach†∗, Mohammed Khider†† German Aerospace Center (DLR), Institute of Communications and Navigation, PO Box 1116, D-82230 Oberpfaffenhofen,

Germany. E-Mail: firstname.lastname@dlr.de∗ EADS Military Air Systems, Manching, Germany

BiographyPatrick Robertsonreceived a Ph.D. from the University ofthe Federal Armed Forces, Munich, in 1995. He is cur-rently with DLR, where his research interests are naviga-tion, sensor based context aware systems, signal process-ing, and novel systems and services in various mobile andubiquitous computing contexts.

Michael Angermannreceived a Ph.D. from the Uni-versity of Ulm in 2004. He is currently with DLR, wherehis research interests are advanced and integrated commu-nication and navigation systems.

Bernhard Krachreceived the Dipl.-Ing. degree inelectrical engineering from University of Erlangen-Nuremberg,Germany, in 2005. From 2005 to 2009 he has been with theInstitute of Communications and Navigation at DLR. Since2009 he is with EADS Military Air Systems.

Mohammed Khiderreceived his master’s degree inelectrical engineering from the University of Ulm, Ger-many. He is currently with DLR and a PhD student at theUniversity of Ulm. His research interests are navigation,multi-sensors fusion, mobility models, signal processingand context aware services.

AbstractWe present results for a new pedestrian localisation tech-nique that builds on the principle of Simultaneous Local-ization and Mapping (SLAM). Our approach is called Foot-SLAM since it is based mainly on the use of shoe-mountedinertial sensors that are used to measure a pedestrian’s stepswhile walking. In contrast to SLAM used in robotics nospecific feature-detection sensors such as cameras or laserscanners are needed in our approach. The work extendsprior work in pedestrian navigation that uses known build-ing plan layouts to constrain a location estimation algo-rithm driven by a stride estimation process. In our ap-proach building plans (maps) can be learnt automatically

while people walk in a building. This can be done eitherdirectly to localise this specific person or in a offline fash-ion in order to provide maps for other people. We havecombined our system with a GPS and have undertaken ex-periments in the important scenario where a person entersa building from outside and walks around within this build-ing without GPS availability. Our experiments were under-taken by recording the raw sensor data and ground truthreference information. Offline procesing and comparisonwith the ground truth reference information allows quanti-tative evaluation of the achieved localisation accuracy.

1 IntroductionRecent work has shown remarkable advances in the area ofpedestrian indoor positioning aided by low cost MEMS in-ertial sensors. At the present time, full autonomous inertialnavigation is still far from the realm of possibilities - dueto sensor error induced drift which causes position errors togrow unbounded within a few seconds. The work of Foxlinon foot mounted Inertial Measurement Units (IMUs) hasshown how zero velocity updates - ZUPTs- during the restphase of a pedestrian’s foot can be used to solve the prob-lem of non-linear error growth over time [1]. This is be-cause the inertial navigation system (INS) is able to accu-rately compute the displacement of the foot during a singlestep before errors would start to grow. The zero updatetells us when the step has been completed (resting phase ofthe foot) and allows us to estimate some of the IMU sensorerror states since we know that the velocity of the sensorarray must be zero in all axes. Nevertheless, errors still ac-crue over time, especially the heading error which is onlyweakly observable from these zero velocity updates (seeFigure 1). Aiding can be performed using a magnetometerbut this sensor also suffers from deviations in the observedmagnetic field, especially indoors. Of course aiding withsatellite navigation systems (e.g. GPS) when available al-

lows for reasonable accuracy outdoors and for short GPSoutages.

Recently, three groups independently showed that footmounted indoor positioning systems work remarkably wellwhen aided by known building layouts, or maps [2, 3, 4].This is because it can reasonably be assumed that pedestri-ans cannot walk through walls and this information shouldbe used by any optimal or close-to-optimal positioning sys-tem. In this work, the researchers used particle filteringalgorithms to incorporate the map information in order toconstrain particle movement to within the areas accessi-ble to a pedestrian. Particle filters (PF), also known assequential importance sampling, are a member of a largefamily of algorithms which are more or less optimal in theBayesian filtering sense. As a result of incorporating maps,long term error stability can be achieved when the map issufficiently accurate and sufficiently constrains the motion- both these criteria are usually met in most indoor envi-ronments such as offices and public buildings. The use ofmaps is also quite natural, since usually any geographic co-ordinate would anyhow have to be placed in the context ofa symbolic location, such as a room number or corridorsection within a building, in order to be used for variouslocation based services such as being directed towards aparticular office. In order for this approach to work, themap information needs to be known and be free of majorinaccuracies.

Figure 1. Plots from two walks around an office environment;see Figure 2. Shown is ZUPT aided inertial navigationbased on a foot mounted IMU.

1.1 Robotic Simultaneous Localization and Mapping(SLAM)

The robotics community has for many years used numer-ous sensors such as laser ranging scanners and cameras toperform high precision positioning of robots in buildings.A difficult problem known as SLAM - Simultaneous Local-ization and Mapping - has been defined as a way of allow-ing robots to navigate in a-priori unknown environments[5]. In SLAM, a moving robot explores its environment

5 meters

Figure 2. Building environment in which the data from Figure1 was recorded; one rectangular corridor circuit androoms on the inside and outside of it.

and uses its sensor information and odometry control in-puts to build a “map” of landmarks or features. Odome-try usually refers to the control signals given to the driv-ing wheels of the robot - and simple integration of theseodometry signals can be seen as a form of dead reckon-ing. There are two main categories of SLAM: EKF-SLAMthat employs an extended Kalman Filter (EKF) to repre-sent the large joint state space of robot pose (position andorientation) and all landmarks identified so far. The ap-proach known as FastSLAM uses a Rao-Blackwellized Par-ticle Filter (RBPF) where each particle effectively repre-sents a pose and set of independent compact EKFs for eachlandmark [6]. The conditioning on a pose allows the land-marks to be estimated independently, thus leading to lowercomplexity. SLAM implementations for robot positioningalways build on sensors and robot odometry, as these arereadily available on robot platforms. The sensors can, forexample, consist of laser rangers or a single or multiplecameras mounted on the robot platform, and the featuresare extracted from the raw sensor data. Simultaneous Lo-calization and Mapping is considered to be a “hard” prob-lem, in contrast to the two easier special cases: Positioningin an environment with known landmarks or building a mapof features given the true pose of the robot.

1.2 SLAM for pedestrian dead-reckoning

This paper will build on both areas of prior work on pedes-trian positioning using foot mounted IMUs as well as theSLAM approach used in robotics which has been describedabove. Our application is human pedestrian positioningbased on SLAM - i.e. the difficult case where no map isavailable a-priori. However, the main difference to roboticSLAM is that no visual sensors are used at all. In fact, theonly sensors used are the foot mounted IMU and option-

ally a magnetometer and / or a satellite navigation receiver.In this paper we show that a pedestrian’s location and thebuilding layout can be jointly estimated by using the pedes-trian’s odometry alone, as measured by the foot mountedIMU. We have confirmed our approach by using real dataobtained from a pedestrian walking in an environment; nosimulations were used - we will present these results in latersections.

There is another major difference between our appli-cation domain and that of the usual applications of roboticSLAM. Our goal is to primarily allow automated gener-ation of maps that can later be used by people wishing tonavigate in that building, say. These maps can be generatedby sensor data collected by people who themselves have noneed for positioning, the data being processed in an offlinefashion. This significantly reduces the computational re-quirements (processing need not be real-time) and our ap-proach still works even in cases where the joint estimationof position and maps would yield a very large position un-certainty during the time when a building is first visited.While our work indicates that an accurate position estimatecan be maintained after (and often prior to) convergence ofthe algorithm upon “loop-closure”, this is not a necessarycondition in our application. Better sensors might, how-ever, make the application of real-time FootSLAM veryworthwhile.

2 Theoretical Basis2.1 Intuitive Discussion of factors governing human

pedestrian motionHuman motion is a complex stochastic process which weneed to model in a sufficiently simple fashion in order todevelop our FootSLAM model and the algorithms whichbuild on it. A person may walk in a random fashion whilsttalking on a mobile phone or they might be following amore or less directed trajectory towards a certain destina-tion. Such phases of motion are governed by the person’sinner mental state and cannot be easily estimated. In [7]a two-state Markov process was used to allow a model ofhuman motion to oscillate between a more random motionand targeted motion.

In order to understand the concept of the kind mapwhich we will use in this work, consider the following sit-uation: An able sighted person is standing in a shoppingcentre facing a wide area in front. The next step(s) whichthis person chooses to take is influenced by two main kindsof governing factors:

• The presence of nearbyphysical constraints, such aswalls, obstacles, other people, etc.

• The presence ofvisual cuesin the environment which

allow the person to orientate themselves and to allowthem to decide which future trajectory they wish tofollow in order to achieve some kind of higher levelgoal, such as reaching a destination (see Figure 3).

Figure 3. Illustration of some of the visual cues that a personmight use to orientate themselves and to plan a trajec-tory. Shown are some obstacles in the direct vicinity aswell as some landmarks (ovals).

In contrast to robotic SLAM we of course have no di-rect access to the visual cues that our subject sees; we do,however, have noisy measurements of the resulting motion,i.e. the steps taken. In a way we can state that we implicitlyobserve these physical constraints and visual features/cuesby observing the pedestrian’s steps - as measured by thefoot mounted IMU. The subject may now wish to enter thewide area in front, or may actually be on the way down tothe level below. Knowledge of previous motion would al-low us to infer more about possible future actions and inprinciple two approaches could be taken: Either interpretthe scene and somehow infer from the overall context whatnext steps are most likely to follow, or observe many previ-ous similar trajectories by the same person or other peoplein this environment, and make a prediction based on sucha learnt Markov process. In our work we will follow thesecond approach and limit the associated Markov processto just a single step (first order). In other words, we willrepresent the possible future next step of the subject basedonly on their current location, and we will learn the proba-bilities of each possible next step through observation.

This would be simple enough if we had perfect knowl-edge of the person’s location at all times - just as roboticmap learning is simple for the case of known pose. In anutshell, we will follow the FastSLAM approach wherebyeach particle assumes a certain pose history and estimatesthe motion probabilities conditioned on its particular as-

sumption. Given a sufficient number of particles we canin principle cover the entire space of possible position his-tories. Particles are weighted by how “compatible” theirmotion is with their previous observations of how the sub-ject had walked when in a certain position. As we shall see,the algorithm converges remarkably quickly as long as theperson revisits locations once or twice during the estima-tion process.

2.2 Our Model as a Dynamic Bayesian NetworkOur work is based on a theoretically well grounded rep-resentation of the Dynamic Bayesian Network (DBN) thatrepresents the pedestrian’s location, her past and presentmotion, the step measurements computed by the lower levelEKF and the “map” (see Figure 4). This approach is usedin all kinds of sequential filtering problems where noisyobservations are used to estimate an evolving sequence ofhidden states. Each node in the DBN represents a randomvariable and carries a time index. Arrows from one statevariable to the next denote causation (in our interpretation),so arrows can never go backwards in time. The arrows canbe read in this way: All incoming arrows originate in statevariables (parent states) that influence the value - in a prob-abilistic sense - of the target (child). In this DBN we have

P

U

Zu E

Int

Vis

Map

Visual information„what the person sees“

Intention„what the person wants to do“

Error states of the odometry

U: Actual step taken (pose change vector)

PoseP

U

Zu E

Int

Vis

P

U

Zu E

Int

Vis

Time k-1 Time k Time k+1

MeasuredStep Zu

“Environment” = Map … constant over time

Figure 4. Dynamic Bayesian Network (DBN) for FootSLAMshowing three time slices and all involved state (ran-dom) variables. The Map can include any featuresand information to let the pedestrian choose theirInt .This DBN is the basis for the formal derivation of theBayesian filtering algorithm.

the following nodes (random variables):

• PosePk: The location and the orientation of the per-son in 2D (with respect to the main body axis).

• Step vectorUk: The change from pose at timek−1to pose at timek. See Figure 5. It is important to bearin mind that the step transition vectorUk has a spe-cial property: given the old posePk−1 and the new

Old pose: location and orientation

New pose: location and orientation

Step vector: translation and orientation change w.r.t. old pose

Measure

‘Know two,compute the third'

Figure 5. Definition of the step and it’s measurement. We use thenotationU to denote similarity to robotic odometry. Inhumans, the true pose changeU is always unknown -it is the actual step taken. In roboticsU is the knowncontrol input to the motors. The pertinent coordinatesystems and error sources are explained in Figure 6.

posePk then this determines the step transitionUk

entirely; just as knowledge of any two of the statesPk−1, Pk andUk determines the unknown one.

• Inertial sensor errorsEk: All the correlated errors ofthe inertial system. For instance angular offsets ordrifts.

• Step measurementZUk : A measurement subject to

correlated errorsEk as well as white noise. See Fig-ure 6 for a definition of the pertinent coordinate sys-tems and step representations. Note thatp(ZU

k |Uk,Ek)encodes the probability distribution of the step mea-surement conditioned on the true step vector and theinertial sensor errors.

• The visual cues which the person sees at timek: Visk.

• The Intention of the person at timek: Int k is memo-ryless in that the resulting intention given a visual in-put is fully encoded in the probability densityp(Int k|Visk).

• The MapM is time invariant and can include any fea-tures and information (such as human-readable signs)to let the pedestrian chooseInt .

Our overall goal is to estimate the states and state his-tories of the DBN given the series of all observationsZU

1:kfrom the foot-mounted IMU (and any additional sensors ifthey are present). The goal in a Bayesian formulation is tocompute the joint posterior [8],

p(P0:kU0:kE0:k,M |ZU1:k) = p({P U E}0:k,M |ZU

1:k) (1)

which following the RBPF particle filtering approach wecan factorize into

p(M |{P U E}0:k,ZU1:k) · p({P U E}0:k|Z

U1:k)

= p(M |P0:k) · p({P U E}0:k|ZU1:k). (2)

It is important to point out now that the additional states ofour pedestrian - encoding vision and intention - are neveractually used; they only serve as importantstructuralcon-straints in the DBN (linkingPk−1 andM as ’parent’ nodesof Uk). The further steps of the formal derivation of theBayesian Filter and the RBPF particle filter are given in[8].

2.3 Definition of pedestrian steps and stepmeasurements

In this section we will show details on how we representthe step transition vector between two steps that a persontakes (see Figure 5) and also [4].

In order to separate the process of updating the in-ertial computer driven by the IMU and the ZUPTs fromthe overall SLAM estimation, we have resorted to a two-tier processing similar to [4] where a low-level extendedKalman filter computes the length and direction change ofindividual steps. This step estimate is then incorporatedinto the upper level particle filter in the form of a measure-ment. It is important to point out that this is a mathematicalmodel that links the measurements received from the lowerlevel EKF to the modelled pedestrian and his / her move-ment, as well as a simple representation of errors that affectthe measured step.

We define a step to be the movement of the shoe thatis equipped with the IMU from one resting phase to thenext. The transition and orientation change of the foot isstrongly coupled to that of the rest of the body: We assumethe position of the pedestrian to be that of the relevant foot,but will follow a different definition of the person’s orienta-tion. The orientation of the pedestrian could be interpretedas where the person is looking (head orientation), in ourcontext, however, it is more useful to interpret the mainbody axis as defining orientation, since it is usually closeto that of the foot. We will introduce an angular devia-tion between this body orientation in space and that of thefoot (IMU). This interpretation is important when we drawon additional body mounted orientation sensors such as amagnetometer. In the complete system there are in totalfour coordinate systems:

• The IMU local reference system with respect to thebeginning of step measurements (i.e. INS calcula-tion) at the lower filtering level.

• A coordinate system aligned to the heading of theIMU at the last step rest phase at the lower filteringlevel (called IMU zero heading.)

• A coordinate system at the higher level filter alignedto the heading of the person’s body at the last steprest phase (called person zero heading.)

• The global navigation coordinate system at the higherlevel filter in which the position estimate and orien-tation are computed (as well as the map).

In Figure 6 we have shown the last three of the above,but have not explicitly represented the angles linking thelast two coordinate systems (they are trivial). We assumethat the step measurement suffers from both additive whitetranslational noise and white noise on the estimated head-ing change of the IMU. In addition, we assume that thereis an additive coloured angular error between the true di-rectional change of the person’s body and that measuredby the INS (which we call IMU heading drift). Lastly, weassume a very slowly changing angular offset between theperson’s body heading and IMU heading - for illustrativepurposes we call this the “duck angle” since such an an-imal would typically experience a large angular deviationwhen equipped with an IMU mounted on its foot. Since weassume that the additive noise components are white, theydo not form a part of the error state of the IMU. We do,however, model the “duck angle” as well as the IMU head-ing drift as random walk processes and they are formallyencoded in the state variableEk. Hereby we allow the IMUheading drift to be unbounded but restrict the ”duck angle”random walk process to+/−20 degrees (essentially lim-ited by human physiology).

2.4 Map Representation in the PracticalImplementation

As stated above, in our model the map is a probabilistic rep-resentation of possible human motion that is based on thesubject’s location in a certain part of a building. It can beinterpreted in this way: a person’s next step will be deter-mined only by his or her current location in the sense thateach future step is drawn from a location dependent prob-ability distribution. This corresponds to the fictive pedes-trian behaviour in which the person looks at a probabilitydistribution posted at each location, and “draws” the nextstep using exactly this governing distribution.

As mentioned earlier we resort to a RBPF that fol-lows a FastSLAM partitioning [6] where each particle rep-resents the pedestrian’s location track and a probabilisticrepresentation of possible motion for each location in atwo-dimensional space. This means that we are represent-ing human motion as a first order Markov process: Thenext step taken by the pedestrian is solely a probabilisticfunction of the current location.

Step measurement: ZUk={Zr

k + nrk ; Zψk + nψk }= {(Zrx

k + nxk , Zry

k + nyk) ; Zψk+ nψk }

U represents the true step vector rk (red); and the true person heading change ψk

φ kε: misalignment between person heading and IMU heading (“duck angle”)

We measure the step ZU = Zrk + noise vector nr

k ; and heading change Zψk + noise nψk

y

x

φk ε

ψk

r xk

r yk

x

IMU zero heading

coordinate system

(i.e. w.r.t. IMU

heading at time k-1)

y

Z ryk Z rx

k Person zero heading

coordinate system (i.e. w.r.t.

person heading at time k-1)

Z ψk

φk ε- γk

rk

k

k-1 Navigation framecoordinate system

Person walked,facing to then facing .

γ k: odometry heading drift γk= Zψk - ψk

k-1

k

Scaled downview of stepin IMU zeroheading coords. (noise free case)

φk ε

k-1

k

ψ k

Zψk

Actual Step U: Uk={rk ; ψk}={(rxk , ry

k) ; ψk} ; with ||rk||= ||Zrk||

Zrk

rk

Pose change (step) in person zero headingcoordinate system

Figure 6. Representation of the important coordinate systems and step vectors (pose change) and angular errors used in our system model.

In order to compute and store the probability distri-bution of human motion as a function of location we havechosen to partition the space (restricted so far to two dimen-sions) into a regular grid of adjacent and uniform hexagonsof a given radius (e.g. 0.5 meters, see Figure 7). Every par-

Six ways out of the hexagon

Figure 7. 2D Hexagon representation for stochastic pedestrianmovement used in this paper. We represent the sixprobabilities for crossing each of the hexagons in a reg-ular grid with adjacent hexagons.

ticle holds (conditional) estimates of the transition proba-bilities across the edges of all visited hexagons and updatesthese based on the motion taken by the particle hypothesis.We assume a uniform prior in our Bayesian estimation ofthese probability distributions. Furthermore, a particle ex-plores possible deviations of the true pedestrian’s path asa result of the sequence of the IMU errorsE0:k (refer toFigure 8). A particle’s weight is updated according to thepreviously estimated knowledge of the state transitions forthe outgoing edges of the hexagon it is currently occupy-ing. As a result, particles will be rewarded by revisitingsimilar transitions in space.

We will now specify the probabilistic map formally,based on transitions across the hexagon grid. We assume atwo-dimensional position domain and populate space withadjacent hexagons of radiusr. We can restrict this spaceto the region visited by any particle and defineH = {H0,

H1, · · · , Hh, · · · ,HNH−1} as the set ofNH hexagons, wherethe indexh uniquely references a hexagon’s position. Fur-thermore we defineMh = {M0

h,M1h, · · · ,M5

h} as the set oftransition probabilities across the edges of theh-th hexagon

k-1

k

Measured stepin IMU zeroheading coords.

k-1

k

ψ/ik

Zψk+ nψk

Zrk + nr

k

r/ik

Lower LevelEKF

RawIMU data

Conversion toIMU zero heading

coordinates (rotation)

Randomly draw particle specificadditive noises;compute Zψ/i

k& Zr/i

k ;Particle index /i

k-1k

Particle i drawn step in IMU zeroheading coords.

Zψ/ik = Zψk+

nψk - nψ/ik

Zr/ik =

Zrk + nr

k - nr/ik

Randomly update particle specific

heading misalignment (“duck angle”) φ k

ε/i; then new step vector r/i

k = rotation of Zr/i

k by φ k

ε/i ; i.e. ||r/ik||= ||Zr/i

k||

φ kε/i

Relative person pose change in person zero heading coords. drawn for particle i

Randomly update particle specific odometry heading driftγ/i

k. Compute new person heading change ψ/ik =Zψ/i

k – γ/ik ;

translate particle i by stepvector r/i

k

Performed per particle i

True, unknown white measurement noise

Particle white noise Particle heading offsetPerformed once at lower layer

Performed per detected step

ψ/ikr/i

k

and drift states

Noises are randomly drawn from zero mean, white Gaussian processesRandom update of φ k

ε/i : follows a bounded first order random walk processRandom update of γ/i

k : follows a first order random walk process

Figure 8. Processing chain from step estimates at the lower filtering level up to the stage where particles are drawn from the proposal density(5). Drawing the two odometry error state angles from their corresponding random walk processes corresponds to drawingEk

i

from p(Ek|Ek−1i). Drawing the two white noise processes fornr/i andnψ/i , and then applying the new angles stored in stateEk,

results in drawing the new step vectorUki from p(Uk|ZU

k ,Eki) - as defined within the person zero heading coordinates.

and

Me(Uk)h(Pk−1) = P(Pk ∈ H j |Pk−1 ∈ Hh)

s.t. H j is reached fromHh by Uk, (3)

and j 6= h; i.e. we moved to a new hexagon, and where 0≤

Edge:

Figure 9. Definition of the hexagon transitionMe(Uk)h(Pk−1)

in (3).

e(Uk) ≤ 5 is the edge of the outgoing hexagon associatedwith Uk, i.e. the edge of the hexagon in whichPk−1 lies andwhich borders the hexagon in whichPk lies - see Figure 9.

Also, we can state that∑eMeh = 1. WhenMe

h is written inbold face we are denoting a random variable. We therebyintroduce the notion thatMe

h, a probability, is unknown tous. We only have estimates ofp(Me

h|P0:k−1) that are theresult of observations of the sequence of positions up tostepk. Our map random variableM is defined as the set:

M = {M0,M1, · · · ,Mh, · · · ,MNH−1}, (4)

whereMh is a random variable vector of length 6 denot-ing the transition probabilities of the hexagon with indexh. In the following we will write h for outgoing hexagonh(Pk−1), ande for the crossed edgee(Uk) for brevity.

2.5 Learning the Transition Map

Learning the map on a particle-by-particle basis is veryeasy and is based on Bayesian learning of multinomial andbinomial distributions. Each time a specific particle withindexi makes a transitionPk−1

i → Pki across hexagon edge

e we count this transition in the local map of hexagonHh

for particlei.

3 Summary of the AlgorithmAs has been described in [4] and [2] it is advantageous toresort to a “Likelihood Particle Filter” [9] since the mea-surementZU

k is very accurate. Weighting with a “sharp”likelihood function p(ZU

k |UkEk) would cause most parti-cles outside the measurement to receive very low weightand effectively be wasted. Thus we sample using the like-lihood function instead of from the state transition model.Specifically, we have chosen the proposal density of theparticle filter to be

q({PUE}k|{P U E}0:k−1,ZU1:k) =

p(Ek|Ek−1) · p(Uk|ZUk ,Ek). (5)

Our RBPF algorithm operates as follows; practicalissues necessary for a real implementation are explainedthereafter:

1. Initialize allNp particles toP0i = (x= 0,y= 0,h= 0)

wherex, y, h denote the pose location and headingin two dimensions; drawE0

i from a suitable initialdistribution for the error states.

2. for each time step incrementk:

(a) Given the latest step measurementZUk : Parti-

cles with indexi are drawn from the proposaldensityp(Ek|Ek−1

i)· p(Uk|ZUk ,Ek

i); see Figure 8.

(b) The posePki is computed by adding the vector

Uki to Pk−1

i ; also updating the heading of thepedestrian according toUk

i .

(c) The particle weight updates are simply

wi α wik−1 ·

{Ne

h+αe

h

Nh +αh

} i

(6)

where the counts are those that are computedup to stepk−1: The termNe

his the number of

times particlei crossed the transition,Nh is thesum of the counts over all edges of the hexagonin this particle’s map counters. The termsαe

handαh = ∑5

e=0 αeh

are the priors of this map seg-ment (in our experiments we choseαe

h= 0.8 for

all edges and hexagons).

(d) Particle weights are normalized to sum to unity.

(e) Recompute{

Neh

}ifor the transition fromhi s.t.

Pk−1i ∈Hhi and the transition ˜ei corresponds to

the step ending atPki .

The counts are kept for each particle and hencestore the entire history of that particle’s path

through the hexagon grid. They are used in (6)the next timeHhi is visited by this particle.

(f) Resampling can be performed if required.

There are a number of implementation issues that needto be addressed in order for the algorithm to work in prac-tice. The first of these is that when computing the countsfor each particle we in fact assume that observing a certaintransition from an outgoing hexagon to an incoming one al-lows us to increment the counts for the outgoing as well asthe incoming hexagon (on the appropriate edge). This is thesame as assuming that a person is likely to walk in eitherdirection and that we should not waste this information.

Next we have assumed so far that an increment of thetime indexk is associated with a step that leads from onehexagon to an adjacent one. In reality a step might keep usin the hexagon or it might lead us over several. To addressthis we simply perform a weight update only when we havestepped out of the last hexagon and apply multiple productsin the weight update (6) for all edges crossed if the step wasa larger one. Similarly, we update the counts of all edgescrossed betweenPk−1

i andPki .

We also incorporated a small correction term in theweight update equation (6) (raising it to a power dependingon the step vector angle within the hexagon grid) to ac-count for the fact that straight tracks with different anglestraversing the grid will yield slightly different total numberof hexagon edge transitions per distance travelled (other-wise particles with some directions are slightly favoured).

4 Results and Conclusions4.1 OverviewOur results based on real data are very promising. We havecollected the data of a pedestrian walking in our office envi-ronment as well as in the adjacent area outside in a numberof runs lasting up to about 12 minutes each. In this paperwe present the quantitative results for the important sce-nario outdoors-indoors-outdoors (Figures 10, 11, 12 and13). This represents the case where a person uses GPSoutdoors and enters a building and leaves it again at somepoint. There are two main applications for this: The trueSLAM scenario where we wish to locate the user in realtime while they are walking, and the map building scenariowhere maps are used later by other people. In order to eval-uate the first case we measured the position accuracy overtime, during the entire walk. To validate the second ap-plication, we show qualitatively the resulting map, createdusing all the data up to the end of the walk (i.e. outdoorsagain). In [8] we have presented results for the indoor-onlycase, where only a foot mounted IMU was used as sensor.In a subset of our evaluations we assumed that we knew

5 meters

Starting point

Finishing point

Rawodometry

Blue building plan: manually rotated, translated and scaled to visually best match the FootSLAM map estimate

Blue hexagons:Maximum a-posteriorihexagon map

Particle cloud

Figure 10. Result of FootSLAM processing for an outdoor-indoor-outdoor walk. Top left: raw odometry - we see how the starting andfinishing point are far apart (in reality, they were very close together). Right: resulting FootSLAM map at the end of the processingrun through the entire data set. Bottom left: particle cloud (note: smaller scale compared to the map window shown on right). Inthis experiment we assumed that we knew the building outer walls.

a-priori the location of the outside building walls to within3 meters of the true wall locations. This helps the Foot-SLAM to converge a little but as we shall show, it is nota requirement. It is realistic, however, for somebody map-ping a new region to roughly manually mark the outer cor-ners of the target building using an online satellite imageservice, for instance. The resulting coordinates can then beused to construct a simple polygon as prior map informa-tion by the particle filter.

In [8] the pedestrian remained indoors and followedno particular course of motion and visited a number of of-fices and rooms as well as the corridor area. In the workpresented here, the pedestrian walked from a point out-side the building, through the front door of the office andround the corridor of the ground floor. During the first walkround, a number of rooms were entered and then left, andthe pedestrian continued to walk the corridor to the nextroom. This pattern was repeated two more times, and thebuilding was left by the same door. In [8] we briefly dis-cussed the sensitivity of the algorithm towards the lengthof time before “loop closure” (i.e. when the pedestrian re-traces previous steps or areas), and the successful indoorexperiments reported in [8] were partly designed to prolongthe time before loop-closure.

4.2 Experiments and processingThe recorded sensor data is collected during the walk andis then processed offline in our RBPF implementation. Inour visualisations, the location estimate (RBPF 2D parti-cle cloud) and the current Maximum A-Posteriori estimateof the probabilistic hexagon map are displayed during theprocessing of the data, as well as the raw step calculationof the lower level EKF (e.g. Figure 10). This allows in-terpretation and explanation of important effects, such asthe the evolving map hypotheses; a collection of our dataprocessing runs were recorded as video and are availableunder [11]. In Figure 10 and Figure 11 we show qualitativeresults for the cases where we assumed and did not assumerough knowledge of the building outline. In Figure 12 weshow the positioning error during the walk - in comparisonto other approaches.

We have also processed the three-dimensional datafrom [10] [11] (data set “March09Measurement03”) assum-ing that this had been a two-dimensional measurement; thiscauses no major degradation as the floor plan corridors aremore or less identical/compatible at different levels of thebuilding. Obviously we ignored the altimeter data but in-cluded the magnetometer. The results are shown in Figure13.

5 m

eter

s

Starting point

Finishing pointRawodometry

Blue building plan: manually rotated, translated and scaled to visually best match the FootSLAM map estimate

Blue hexagons:Maximum a-posteriorihexagon map.

Grey/Black hexagons:Ensemble of mapposterior distribution(evident is a strongcompeting map that isrotated)

Particle cloud

Figure 11. Result of FootSLAM processing for an outdoor-indoor-outdoor walk. Bottom left: raw odometry. Right: resulting FootSLAMmap. Top left: particle cloud (note: smaller scale compared to the map window shown on right). In this experiment we didnotassume that we knew the building outer walls. Observe how two main hypotheses for maps survive - this is to be expected giventhe rotation invariant nature of SLAM.

4.3 Discussion and further workComparison with the true layout of the building - whichwas, of course, not used in the processing and only manu-ally inserted over the resulting FootSLAM maps applyingrotation, scaling and translation chosen to match the Foot-SLAM map - showed a remarkable ability of the RBPF toestimate the reachable areas and errors of the location of thedoors was usually to within +/- 1 meter and never more thanabout 2-3 meters away (based on results in this paper andon those in [8]). All results so far were obtained with justa single track and assume no further processing to mergetracks. In a real system, efforts must be undertaken to re-solve the scale, rotation and translation ambiguities and er-rors which are often inherent in SLAM. In our approachwhere we couple with GPS in the outdoor portion and op-tionally a magnetometer, these ambiguities may not be sopronounced, and may be locally confined to the buildingindoor areas. Future work should address map combina-tion techniques that combine maps from different sourcesunder these considerations.

Inspecting the numerical results we make the follow-ing observations:

• Observing the particle cloud during processing and

also the evolution of the position error it becomesevident that the estimator diverges at first as the areawas being explored, but then begins to converge (atloop closure) closer to the true location and remainsreasonably stable. The cloud naturally spreads asnew areas of the building are being explored for thefirst time, only to converge again as the pedestrianrevisits familiar ground.

• The numerical results indicate that the use of roughknowledge of the outer building walls (building perime-ter) help to improve the error slightly.

• The use of perfect building plan information - notsurprisingly - gives the best performance. This is be-cause the location of the walls is known with sub-meter accuracy. The result is that indoor positioningaccuracy is usually better than outdoors.

• When FootSLAM is used, the accuracy cannot bebetter than the anchor achieved while using GPS be-fore entering the building. This error in our exper-iments was typically around 3-7 meters, so this is abaseline error onto which the FootSLAM relative er-rors are essentially added.

0 100 200 300 400 500 6000

2

4

6

8

10

12

14

16

18

Time [s]

Pos

ition

Err

or [m

]

PF known map; (=no FootSLAM); Ave. Pos. Error=2.1 mEKF; no FootSLAM; no map known; Ave. Pos. Error=5.7 mFootSLAM; no walls known; Ave. Pos. Error=5.9 mFootSLAM; just outer building outline known; Ave. Pos. Error=5.3 m

OUTDOORS

I N D O O R SOUTDOORS

Corridor loop closure pointreached again

Corridor loop closure pointreached

Figure 12. Position error of FootSLAM processing for the outdoor-indoor-outdoor walk of Figures 10 and 11. Comparison with particle filterusing complete building plan information and a simple EKF using no such information. The FootSLAM results were averagedover three RBPF runs over the data set with 55000 particles each. The EKF and PF curves are for a single run.

• The extended Kalman filter (EKF) diverged after sometime, especially in the second data set (divergence isa random process and depends on the random occur-rence of drifts and angular displacement of the strideestimation at the lower level and is a function of theIMU errors).

Since our maps are probabilistic, estimation of pedes-trians’ future paths could also be performed - similar towork for driver intent estimation [12]. Further work shouldalso integrate more sensors, address 3D, as well as collec-tive mapping where users collect data during their dailylives and maps are combined and improved.

4.4 Acknowledgements

This research has received funding from the European Com-munity’s FP7 Programme [FP7/2007-2013] under grant agree-ment no. 215098 of the “Persist” Collaborative Project.

References

[1] E. Foxlin, “Pedestrian tracking with shoe-mounted in-ertial sensors,”IEEE Computer Graphics and Appli-cations, vol. 25, no. 6, pp. 38–46, Nov. 2005.

[2] O. Woodman and R. Harle, “Pedestrian localisationfor indoor environments,” inProc. of the UbiComp2008, Seoul, South Korea, Sep. 2008.

[3] S. Beauregard, Widyawan, and M. Klepal, “IndoorPDR performance enhancement using minimal mapinformation and particle filters,” inProc. of theIEEE/ION PLANS 2008, Monterey, USA, May 2008.

[4] B. Krach and P. Robertson, “Cascaded estimationarchitecture for integration of foot-mounted inertialsensors,” inProc. of the IEEE/ION PLANS 2008,Monterey, USA, May 2008.

[5] R. Smith, M. Self, and P. Cheeseman, “Estimatinguncertain spatial relationships in robotics,” inAu-tonomous robot vehicles, I. J. Cox and G. T. Wilfong,Eds. Springer-Verlag New York, Inc., 1990, pp. 167–193.

[6] M. Montemerlo, S. Thrun, D. Koller, and B. Weg-breit, “FastSLAM: A factored solution to the simul-taneous localization and mapping problem,” inProc.AAAI National Conference on Artificial Intelligence,Edmonton, Canada, 2002.

[7] M. Khider, S. Kaiser, P. Robertson, and M. Anger-mann, “A novel movement model for pedestrians suit-able for personal navigation,” inION NTM 2008, SanDiego, CA, USA, 2008.

[8] P. Robertson, M. Angermann, and B. Krach, “Simul-taneous localization and mapping for pedestrians us-

0 50 100 150 200 250 300 350 400 4500

5

10

15

20

25

30

Time [s]

Pos

ition

Err

or [m

]

FootSLAM; no walls known; Ave. Pos. Error=6.2 mFootSLAM; just outer building outline known; Ave. Pos. Error=4.4 mPF known map; (=no FootSLAM); Ave. Pos. Error=1.7 mEKF; no FootSLAM; no map known; Ave. Pos. Error=16.9 m

OUTDOORS

I N D O O R SOUTDOORS

Corridor loop closure point reached

Figure 13. Position error of FootSLAM processing for the outdoor-indoor-outdoor walk of [10] [11]. Comparison with particle filter usingcomplete building plan information and a simple EKF using no such information. Note: FootSLAM assumes this as beingtwo-dimensional which in this originally 3D data set causes no major degradation as the floor plan corridors are more or lessidentical/compatible at different levels. The FootSLAM results are a single RBPF run each, with 35000 particles. The EKF andPF curves are also for a single run.

ing only foot-mounted inertial sensors,” in Proc. Ubi-Comp 2009, Orlando, Florida, USA.

[9] S. Arulampalam, S. Maskell, N. Gordon, andT. Clapp, “A tutorial on particle filters for onlinenonlinear / non-Gaussian Bayesian tracking,”IEEETransactions on Signal Processing, vol. 50, no. 2, pp.174–188, Feb. 2002.

[10] M. Angermann, A. Friese, M. Khider, B. Krach,K. Krack, and P. Robertson, “A reference measure-ment data set for multisensor pedestrian navigationwith accurate ground truth,” inEuropean NavigationConference ENC-GNSS 2009, Naples, Italy, 2009.

[11] “FootSLAM videos and reference data sets down-load,” http://www.kn-s.dlr.de/indoornav.

[12] J. Krumm, “A markov model for driver turn predic-tion,” in SAE 2008 World Congress, Detroit, MI USA.Springer-Verlag New York, Inc., 2008.