Map Matching and Heuristic Elimination of Gyro Drift for Personal

Navigation Systems in GPS-denied Conditions

By

Priyanka Aggarwal*, David Thomas, Lauro Ojeda, and Johann Borenstein

The University of Michigan, 2260 Hayward Street

Ann Arbor, Michigan, 48109, USA

aggarwalpp@gmail.com, davidlt@umich.edu, lojeda@umich.edu, johannb@umich.edu

(*) Corresponding Author

ABSTRACT

This paper introduces a method for the substantial reduction of heading errors in inertialnavigation systems used under GPS-denied conditions. Presumably, the method is applicablefor both vehicle-based and personal navigation systems, but experiments were performed onlywith a personal navigation system called “Personal Dead Reckoning” (PDR). In order to workunder GPS-denied conditions, the PDR system uses a foot-mounted Inertial Measurement Unit(IMU). However, gyro drift in this IMU can cause large heading errors after just a few minutesof walking. To reduce these errors, the Map-matched Heuristic Drift Elimination (MAPHDE)method was developed, which estimates gyro drift errors by comparing IMU-derived headingto the direction of the nearest street segment in a database of street maps. A heuristiccomponent in this method provides tolerance to short deviations from walking along the street,such as when crossing streets or intersections. MAPHDE keeps heading errors almost at zero,and, as a result, position errors are dramatically reduced. In this paper, MAPHDE was used in avariety of outdoor walks, without any use of GPS. This paper explains the MAPHDE method indetails and presents experimental results.

1. Introduction

Pedestrian tracking is the process of determining and maintaining positional informationfor a person traveling on foot. Pedestrian Tracking Systems (PTSs) are useful for trackingmovements of military personnel, finding and rescuing incapacitated first responders, or forlocation-aware computing and personal navigation assistance [1]. For most PTSs, GPS is theprimary source for obtaining position and velocity data. Under ideal conditions, GPS provideslong term accurate and absolute measurements. Outdoors, where there is a clear line of sight tofour or more satellites, GPS provides location with accuracies ranging from tens of meters totens of centimeters, depending on the type of GPS receiver [14]. However, GPS is oftenunavailable in urban areas, where the urban canyon effect may reduce the number of visiblesatellites. Even when GPS is available, it is often less accurate because of multipath effects.Alternative approaches for absolute PTSs require active radiation sources such as infrared light[16], ultrasound [17], magnetic fields [18], Ultrawideband (UWB) [19] or other radio

Published in Journal of Measurement Science and Technology, Vol 22 (2011)

2

frequencies [20]. Common to these methods is that they require initial setup of thecorresponding sources. Many applications exist in which the initial set-up time is not feasible(e.g., firefighting or military) or where the installation of such infrastructure is too costly (e.g.,city blocks).

Another approach is to use light emitting and capturing sources such as cameras orLight Detection and Ranging (LIDAR) systems [21]. LIDAR suffers from disturbances whenthe user walks among other pedestrians, as would be the case in downtown areas of large cities.Another technology that uses cameras and/or LIDAR is Simultaneous Localization AndMapping (SLAM), which is based on building a map within an unknown environment (withouta priori knowledge) or updating a map within a known environment (with a priori knowledgefrom a given map) while at the same time keeping track of the current location. SLAM systemsmay accumulate errors over time and can fail when used on a busy street with pedestrianswalking by. A recent development [22] uses head-mounted laser range and inertial sensors tolocalize a person in indoor environments, without use of any a priori building or floor map data.All LIDAR-based technologies have these limitations in common: have active source ofradiation which might reveal the military users location to the enemy and the equipment pose isaltered when firefighters crawl on hands and knees for rescue mission. A problem common toLIDAR and computer vision systems is that they don’t work well in smoke filled environments,making them unsuitable for firefighters.

There are a few other Radio Frequency (RF) based technologies, such as those usingcellular telephones (exploiting time delays) or triangulation methods based on wirelessnetworks for obtaining navigation parameters. The main limitation of cellular technologies isthat they are not accurate enough for most pedestrian tracking purposes. Wireless Local AreaNetworks (WLANs) can be used for indoor navigation, where the location fix is determined bymeasuring signal strengths from several access points [21]. However, like GPS, this methodsuffers from multipath and fading effects, and often pre-mapping of signal strength is required.Other methods may include active badges or optical tracking systems that employ specificmarkers installed in and around the buildings and can assist in detecting the person’s currentlocation [23].

An entirely different approach is the use of relative sensor modalities, predominantly InertialMeasurement Units (IMUs). IMUs don’t require any external infrastructure and don’t activelyradiate. The problem with IMUs is that position and heading estimates derived from theaccelerometers and gyros of the IMU develop unbounded errors. Moreover and besides theirastronomical cost, very high-end IMUs that develop errors only slowly are typically too largeand heavy to be worn by a person, especially warfighters and first responders who already carrya lot of heavy gear. Smaller and lighter IMUs, typically using Micro-electro-mechanicalsystems (MEMS) technology, develop large position and heading errors quickly. A commonapproach to making MEMS-based IMUs feasible in PTSs applications is therefore to find waysthat allow the bounding of heading and position errors.

The main components of an IMU-based PTS algorithm are footfall detection, step-length along with altitude estimation, and heading estimation [2-6, 20]. Footfall detection maybe accomplished by examining accelerometer or gyroscope variances after pre-processing (such

3

as noise reduction by averaging) [2, 3, 7]. The estimation of step-length can be accomplishedeither by GPS updates [3] (when GPS is at least temporarily available), as a function of stepperiod, acceleration magnitude or acceleration variance [6, 10], by direct inertial sensorintegration [1, 11-12], or by other advanced techniques [15].

One key distinction of IMU-based PTSs is whether the IMU is located on the foot orelsewhere on the body. Location of the IMU on the body makes system integration morepractical, but existing systems all struggle with the problem of estimating distance traveled,especially when users apply different gaits, as well as when walking backward or sideways.Foot-mounted IMUs are more cumbersome to use because there are limited options for placingthe IMU on or in the user’s footwear, and typically a cable connection to a body-worncomputer is required. However, the great advantage of foot-mounted IMUs is that they allowcompensation for accelerometer drift and thus accurate direct step-length measurements [7] aswill be explained in Section 2. In most IMU-based systems, altitude is not measured by theIMU but rather by barometric pressure sensors [20].

In IMU-based PTSs, heading or direction of motion is most commonly obtained fromthe gyroscopes of the IMU. However, in MEMS-based systems bias drift of the gyros issignificant and if left uncorrected can completely mask the navigation solution [1, 11-12].Therefore, alternative equipment or additional data sources are required to achieve reasonableaccuracy. Since heading errors from inertial sensors can grow without bound, reducing headingerrors can dramatically improve position accuracy. Another method for obtaining headinginformation is by using a magnetic compass [2, 10]. A magnetic compass offers absoluteheading information by measuring the earth’s magnetic field. However, these measurementscan be easily distorted by nearby steel structures or electric fields. For these reasons,magnetometers generally don’t work reliably enough inside buildings and work onlymarginally in urban environments [13].

Map matching algorithms [24-25] can also be used for correcting heading errors in PTS.Map matching is the process of comparing personal tracking data with the digital map of streetsto match the pedestrian’s navigation data to the street segment on which the pedestrian iswalking. There are several map matching algorithms such as geometric point-to-point,geometric point-to-curve and geometric curve-to-curve methods. These methods and theirshortcomings will be explained in Section 3.2. This paper proposes a modified geometric point-to-curve method to overcome some of the limitations of these existing map-matchingalgorithms.

The work was motivated by the goal of creating a PTS that is capable of trackingwalking persons in GPS-denied urban environments. The focus was on mitigating headingerrors by means of the so-called “Map-matched Heuristic Drift Elimination” (MAPHDE)method that was developed under this study. This method has two components: (1) obtainingthe direction of the nearest street segment through map-matching and (2) correcting thewalker’s estimated heading based on the inertial sensors and the present best estimate of theheading calculated by previously developed Heuristic Drift Elimination (HDE) technique.

4

It should be noted that the term “drift elimination” as used in this paper is meant to betaken figuratively, not literally. Of course, our method cannot correct gyro drift at the core,namely, inside the gyro. A more precise but stylistically cumbersome name for our methodwould be “Heuristics-based Elimination of the Ill-effects of Drift and of Other Slow-changingError Sources on Heading Estimation.” In the interest of brevity, we chose the shorter term“Heuristic Drift Elimination.”

The remainder of this paper is structured as follows. Section 2 describes the earlier-developed PDR system. Section 3 describes the TIGER database and OpenStreetMaps, wheremap data is freely available, along with previous map-matching techniques. The proposedMAPHDE method is described in Section 4 and results are presented in Section 5.

2. The Personal Dead Reckoning (PDR) System

In earlier work, the Mobile Robotics Laboratory at the University of Michigan developed thePersonal Dead Reckoning system [7-9, 11]. Using only an IMU for all measurements, the PDRsystem has a zero-radiation signature, i.e., it does not emit any signals. This makes the system“invisible” to sensors in hostile environments and immune to interference or jamming.

2.1 Components of the PDR system

The PDR system uses an IMU strapped to the side of the user’s boot or embedded in theheel of the user’s boot, as shown in Figure 1. The Side-mounted IMU can be transferred amongdifferent users while the in-heel version better protects the IMU from damage and cannot bedislocated easily. The PDR system’s computations are performed on a PC-104 computer that islocated inside a belt pack, together with batteries and support electronics. The MEMS-basedIMU used in the PDR system is the nano-IMU (“nIMU” in short), made by Memsense. Somekey specifications for the nIMU are listed in Table I.

(a) (b)

Figure 1: The foot-mounted IMU of the PDR system has two mounting options. (a) Side-mounted IMU (the IMUitself is covered by the beige-colored thermal insulation). (b) In-heel IMU with temperature controlled, shock

resistant housing.

5

Table I: Key Specifications of the Memsense nIMU

Memsense nIMU

Size (mm) 452313

Weight (gram) 15

Bandwidth (Hz) 75

Gyroscope

Range (deg/sec) ±1,200

Angle Random Walk (deg/sqrt-hr) 4.2

Bias Drift (deg/hr) 80

Accelerometer

Range (g) ±10

The computer runs the Linux operating system patched with a real-time extension. AnIMU-based position estimation system combines two functional components: the estimate ofdistance traveled and the estimate of heading. In the PDR system, the accuracy of bothcomponents is predominantly affected by bias drift (“drift,” in short). This is especially true if arelatively low-performance MEMS-based IMU is used. Drift rates for both accelerometers andgyroscopes in a MEMS-based IMU are several orders of magnitude higher than what is foundin high-grade aviation IMUs. Of course, the cost of high-grade IMUs is also one or two ordersof magnitude higher than that of the nIMU in the PDR system. However, the reason for using alow-grade IMU in the PDR system is not just cost. Rather, in order to embed the IMU in theheel of a regular firefighter or military-style boot, the device must be very small. Because ofthis size-limitation, the only suitable IMU technology is that of MEMS. Another limiting factoris the fact that the peak accelerations and rates of turn of the foot, even at normal walkingspeed, are significantly higher than those found on the torso of a person. This limits the choiceof suitable IMUs to less than a handful of models that offer the large dynamic range requiredby that particular IMU location.

A foot-mounted IMU makes wiring more difficult, requires greater dynamic ranges ofthe IMU, and may add difficulties in heading estimation. However, there is a compelling reasonfor choosing this mounting location after all: it is the only way to reset drift of theaccelerometers almost as frequently as once every second and thereby allows the accuratedetermination of step-length, as explained below.

2.2 Estimate of distance traveled

In the PDR system, the estimate of distance traveled is based on the accelerometers ofan IMU. It is well known that measured accelerations must be integrated twice to yield distancetraveled. This double-integration turns even small amounts of drift into large position errors. Inorder to overcome this limitation, a technique known as Zero Velocity Updates (ZUPTs) isused. ZUPT is a well established technique for estimating and counteracting drift in inertialsensors. However, ZUPT is often inconvenient because it can be applied only while the sensorhas zero velocity. A foot-mounted IMU, on the other hand, lends itself perfectly to the

6

application of ZUPT because during walking and many other modes of legged motion, the soleof the instrumented boot has zero velocity during parts of a phase of walking called“midstance.” To exploit this fact, the PDR system examines the incoming data from the IMUand flags the instance of minimal foot motion during the midstance phase. This flaggedinstance of presumed zero velocity is called “footfall” in the context of this paper. The ZUPTmethod is then implemented by comparing the accelerometer-derived velocity of the foot to thevelocity of the foot during footfall. Since at footfall the velocity should be zero, anyaccelerometer-derived non-zero velocity estimate at the instance of footfall is considered to bethe result of drift and is compensated for.

Another helpful aspect of walking is that footfalls of the instrumented foot occur withevery second step, and that is almost as frequently as once per second. This assures thataccelerometer drift is compensated for once per second, thereby not allowing significant driftand position errors to accumulate. As a result, the PDR system estimates distance traveledconsistently with errors of less than 1% of distance traveled, on almost all terrains, except forice, sand, or other non-solid surfaces. The ZUPT method and its application in the PDR systemare explained in more detail in the authors’ earlier papers [7-9, 11].

2.3 Estimate of change in heading

The PDR system estimates changes of heading from the IMU’s gyro data. However,bias drift in the gyros of a MEMS-based IMU is on the order of tens to hundreds of degrees perhour (about 80 °/hour in the case of the nIMU). Unfortunately, a method “similar to the ZUPTapproach” that works so well for the PDR system’s accelerometers cannot be used tocompensate for the gyro drift. The main difficulty is that under realistic walk conditions theshoe heading continues to change during the footfall, therefore it is “non-zero” and a methodsuch as ZUPT would not immediately be applicable.

In earlier work [8] the authors introduced a method, called “Heuristic Drift Reduction”(HDR) that partially compensates for gyro drift by exploiting the fact that pedestrians oftenwalk along approximately in straight lines. This is mainly because pedestrian routes (e.g.,sidewalks in urban centers) are often straight and because straight lines are the shortest way toget from point A to B. The method of [8] reduced average heading errors up to five-fold.

In subsequent work [9], the authors introduced a significantly more effective method,called “Heuristic Drift Elimination” (HDE). This method is applicable only in indoorenvironments in which corridors and walls are straight and intersect at right angles. In practice,this seemingly severe limitation is met by the vast majority of all buildings. In such indoorenvironments, HDE effectively compensate for gyro drift and other slow-changing headingerrors.

Similarly, the key hypothesis or heuristic assumption of the work in this paper is thatwhen pedestrians walk along streets, they do so predominantly in the general direction of thesestreets. Furthermore, the proposed method presumes that these streets are mapped with goodaccuracy and that the maps can be obtained in electronic form. Based on these assumptions, theheuristic method proposed in this paper compares the momentary heading of a walker based on

7

gyro estimates to the known heading of the street segment that the walker is currently walkingon. Any discrepancy is assumed to be due to gyro drift and is compensated for.

However, these heuristic assumptions do not hold true all the time. In reality,pedestrians don’t walk on perfectly straight lines. Pedestrians cross streets at angles, avoidobstacles and pick occasional shortcuts. Nonetheless, this paper argues that despite these localaberrations, streets and walkways channel the motion of pedestrians so that on average, theheading of a street segment is a good predictor for the average heading of the pedestrian alongthat segment. The challenge is to define a practical method that uses the known heading ofstreet segments to correct gyro drift, while being tolerant to these local, temporary deviations.The remainder of this paper introduces one such method.

3. Map Matching

As a source for digital street maps the freely available OpenStreetMap [27] database isused. This database claims a positional accuracy range of 7.6 meters or better.

3.1 The OpenStreetMap Database

The database of roads used in this study was obtained from the OpenStreetMapdatabase, which is mainly populated by the Master Address File/Topologically IntegratedGeographic Encoding and Referencing (MAF/TIGER®) database [26]. This Open Street Map(OSM) database is a single XML file that is hierarchically composed of ways (including streets,paths, rivers, lakes, legal boundaries etc.), covering the entire United States.

The OSM database is collaboratively updated by volunteers and is more inclusive andmore up-to-date than the TIGER database. In the OSM database, straight street segments arerepresented by two points while curved street segments are represented by multiple, ratherdensely spaced points, as shown in Figure 2 and Figure 3. The OSM database contains digitalvector data describing geographic features but it does not include graphic images forvisualization.

Figure 2: Typical frequency of points used in the OSM database to representation ofcurved street segments (overlaid over a conventional map, for visualization).

8

In preparation for this study, the latitude and longitude of street centerline points wereextracted from OSM database and stored in the flash memory of the PDR system. This datawas parsed ahead of time to populate a SQLite database with the required data. In doing so,streets were split into segments of a maximum length of 100 meters, to significantly cut downthe number of viable segments.

3.2 Map-matching techniques and integration methods developed by others

A map matching algorithm identifies the street segment that the user travels along.There are several map matching algorithms and some of them are briefly discussed here [28].

1. Geometric point-to-point matching. In this approach, the closest node in the streetnetwork database to the user is obtained. This is the simplest, easiest to implement, and fastestmethod, but the accuracy of the solution depends on the way the street network is digitized.Streets with a greater number of nodes are more likely to be matched with the user.

2. Geometric point-to-curve method. With this method a street segment is identified that isclosest to the user, rather than the street nodes. From geometry, the minimum distance betweena point and a line is the perpendicular distance, and this distance can be readily computed. Oneproblem with this method is illustrated in Figure 4, where the trajectory is nearest to Street Abut at point U3 it is equally distant to streets A and B. In this situation the above explainedalgorithm fails to select the correct street segment. Hence, this approach does not work wellnear intersections where different streets can be equidistant from the walker. Furthermore, theobtained perpendicular point may be unstable, oscillating back and forth between two closelyspaced parallel streets common in urban environment [28-31]. A single reading may bematched with several street segments, thereby creating an ambiguity in the solution and causesthe user to drift from one parallel street to another.

Figure 3: Typical OSM database representation of straight street segments, using onlytwo points (overlaid over a conventional map, for visualization).

9

Figure 4: Error in geometric point-to-curve method near intersection.

3. Geometric curve-to-curve matching. With this method the previous m positions of the userare matched with the street curve and the nearest street curve is selected as the solution. Thismethod is very sensitive to outliers and there is no good way to compare curves of differentlengths without incorporation of GPS data. On connecting GPS data points, a sequence ofpiece-wise linear curves is formed which is then matched with a set of possible street segments[25]. Generally, the selected street segment will have the smallest norm distance to the GPScurve. However, if GPS data is not accurate, this matching method will produce a wrongmatch. Furthermore, there should be a sufficient number of GPS readings to form a linear curvefor the user’s trajectory.

In this study, a modified geometric point-to-curve method was implemented foridentifying the nearest street segment to the walker. In addition, only the direction of thenearest street was matched to the walker’s estimated heading, since the algorithm does notattempt to correct the walker’s position directly.

There are many possible ways of optimally combining the map information with othersources of position information, such as IMU. One such method utilizes the Maximum-a-Posteriori (MAP) estimator to optimally translate raw position measurements of a walker ontothe street network [32]. A MAP position estimate is obtained by solving equation (1):

x = arg max[p(x)p(y|x)] (1)

where x is the position estimate of the walker with probability p(x), y is the available noisymeasurement and p(y|x) represents the likelihood function.

In cases where the street is a long straight line, it is modeled by the centerline. Curvedstreets are modeled by a sequence of piecewise straight-line segments, where each piece istreated as a separate street and a locally optimum position estimate is determined for that piece.However, it is possible that the solution obtained by the unbiased MAP estimator is not uniqueas the likelihood function may have multiple local maxima and a wrong street may be

Uo

U1

U2

U3

A

BUser trajectory

10

identified. The resolution of this problem requires other sources of position information to beidentified and incorporated into the estimation process.

One such source is to include information contained in the walker’s trajectory such thata probability is associated with each street. This probability represents the likelihood of thewalker being on that particular street. The easily way to accomplish this is by means of aKalman Filter (KF) as it associates a probability with each street such that larger errors producesmaller probabilities of being the selected street segment [3]. However, this approach is notpractical because a KF assumes state variables and noises to have Gaussian distribution, whichis not always true in a real scenario [3].

Other possible methods are to use fuzzy logic-based map matching and probabilisticalgorithms using inertial sensors [32]. In [32], a set of eight rules is defined for obtainingposition using map-matching algorithm, inertial sensors, and GPS data. However, in certaincases error growth can completely mask the true solution. A confidence region in shape of anellipse or rectangle is defined around the navigation sensor output, for the probabilisticapproach. This error or confidence region is then superimposed on a map-database to obtain thecorrect street segment. Even so, additional knowledge may be essential to identify the correctstreet segment from a number of possibilities.

The MAPHDE method uses a modified map matching technique, but in conjunctionwith the aforementioned HDE engine, as will be explained in the following section. The HDEtechnique is capable of fully compensating for all slow-changing errors including gyro drift,thereby providing near-zero heading errors at steady state. In the context of this study, steadystate is typically reached within a few seconds of walking straight along a straight streetsegment. HDE needs only four pieces of information to function: distance traveled per footfall,time elapsed since the last footfall, change in heading since the last footfall, and direction of thenearest street segment [5]. Furthermore, MAPHDE does not need any models or modelparameters, as would be required in the case of Kalman Filters. In its simplest form, MAPHDErequires the tuning of just three parameters, the gain ic, the distance of the walker from thenearest street segment, dmin, and a threshold, ψthres, for the difference between estimatedheading from HDE and the present best estimate of the direction of the nearest street segment.Furthermore, HDE takes just a few lines of C-code to implement. The MAPHDE method isexplained next.

4. Map Matched Heuristic Drift Elimination (MAPHDE) Method

This paper presents a novel method for correcting heading errors in position trackingsystems based on the proposed MAPHDE method. The required real-time input for theproposed method is rate of rotation about the Z-axis or change in heading, step length, timeinterval between subsequent footfalls, and initial starting position and heading. Of course, thissystem also requires its map database to be populated with pertinent map data. Generally, on3-D terrain, or with the foot-mounted sensor of the PDR system, a full 6-DOF (Degree-of-Freedom) IMU is required to estimate heading. Here the notion of a “virtual Z-axis gyro” isintroduced to express the walker’s rate of rotation about the navigation Z-axis. The PDR

11

system uses the Quaternion attitude representation. The output of the virtual Z-axis gyro maybe affected by errors such as offset bias, run-to-run bias, in-run bias, scale factor error,misalignment error, sensor drifts, and random noises etc., which are difficult to identify andmodel or compensate. However, the HDE method treats the combination of all this sources oferror as a single error term which overall behaves as drift and the HDE method estimates andcompensates for it in real-time. This is the main advantage of the HDE technique as it does notrequire individual error sources to be identified and quantified.

When a person equipped with a virtual Z-axis gyro is walking straight forward, theoutput of that gyro should be exactly zero throughout the walk. However, due to a variety ofslow-changing and random errors (such as bias drift, sensitivity to linear accelerations, scalefactor non-linearity, and with integrated white noise) the actual output is off by some smallvalue ε. For simplicity, all of these errors are collectively called “drift” in this paper. The totaltravel distance is divided into smaller intervals i.e., from footfall to footfall, as defined inSection 2.2.

Due to the drift error ε, in each interval the rate of turn computed based on the Z-axisgyro is

ωmeas,i = ωtrue,i + εi (2)

where ωmeas,i is rate of turn around the Z-axis during sampling interval i, ωtrue,i is true butunknown rate of turn around the Z-axis during sampling interval i and εi is the sum of allunknown errors during sampling interval i.

The change of heading in sampling interval i, denoted ψi, is computed by numericallyintegrating ωmeas,i

ψi = ωmeas,i Ti = (ωtrue,i + εi) Ti (3)

where Ti is the duration of time interval i in [sec]. As Ti is the time between footfall i-1 andfootfall i and the new estimated heading is

ψi = ψi-1 + ψi (4)

It is apparent from Equations (2) through (4) that the estimated heading, ψi, representsthe true heading plus the accumulated sum of all heading errors, ψe,i.

4.1 Error correction approaches

A brute force approach to correcting heading errors from known street segmentdirections is to assume that any difference between gyro-derived heading and street segmentdirection is due to gyro-induced errors. Consequently, one can correct the gyro-derived headingby simply replacing it with the appropriate street segment direction. This brute force methodwill fail as soon as the walker actually deviates from the direction of the street, for example, tocross the street or avoid an obstacle.

In order to allow for the walker’s temporary deviations from the direction of the street, amore subtle approach is needed. For example, instead of completely replacing the gyro-derivedheading by the street direction in every sampling interval, one could add a small correction to

12

the gyro-derived heading. This correction could be proportional to the difference between streetand gyro-derived heading. However, this approach would still react too strongly to actualdeviations. For example, when crossing a street the difference between the direction of thestreet and the gyro-derived direction can be as large as 90°.

A more deviation-tolerant approach is to add a repetitive and small, constant-magnitudecorrection to the gyro-derived heading. As long as constant-magnitude heading correctionsaccumulate faster than drift-induced heading errors, this approach can effectively neutralizedrift-induced heading errors. It is also apparent from this discussion that the proposed approachwill not react strongly to temporary deviations, because corrections in response to temporarydeviations cannot accumulate significantly.

The following section formalizes this proposed approach in the form of a feedbackcontrol system. Specifically, it will be shown how the proposed algorithm:

models ψe,i as a disturbance in a feedback control system;

implements a method for accumulating small, constant magnitude corrections by meansof a so-called Binary I-controller;

remains largely insensitive to changes in ψ that have large-amplitudes but short duration.

4.2 The MAPHDE Algorithm

The basic MAPHDE algorithm functions essentially like a feedback control system.This is different from most other measuring systems, where signals pass from the sensor to theinstrument’s output in open-loop fashion. Figure 5 shows a block diagram of the feedbackcontrol system for the MAPHDE algorithm.

Figure 5: The HDE algorithm viewed as a feedback control system. The block labeled “Binary I-controller” isexplained in the narrative.

ωmeas,i = ωtrue,i+εi

1z

z

-

z-1

ψi+ψmap,i-1

BinaryI-controller

Integral

ψi = Δψtrue,i+ Δψe,i

++Ei

+

ψi-1

Z-axis Gyro(adds εi)

ωtrue,i

Ii

ψi-1= ψtrue,i-1+ ψe,i-1

13

The explanation of the feedback control system starts with the signal from the gyro,which is modeled as a disturbance in the block diagram of Figure 5. Suppose the user iswalking straight, along a straight section of a linear map feature (for simplicity, it is assumedthat all linear map features are streets, but the proposed algorithm works equally well with floorplans of tunnels or buildings). Immediately after a footfall, the measured rate of turn isintegrated to yield the change in heading, ψi. ψihas two components, the true but unknownchange of heading, ψtrue,i, and the change of heading error, ψe,i. Next, ψi is added to ψi-1 andto the output of the binary I-controller, which is explained later in this section. Initially, theoutput of the I-controller is zero. The label ‘z-1’ in the feedback loop is the common notation fora pure delay of one sampling interval. After the first iteration, when i>1, the control loop can beclosed by comparing ψi-1 to the direction of the closest map feature, ψmap,i-1. The differencebetween them is the error signal E.

Ei = ψmap,i-1 - ψi-1 (5)

Unlike conventional integral (I) or proportional-integral (PI) controllers, the binary I-controller is designed not to respond at all to the magnitude of E. Rather, it only responds to thesign of E. If E is positive (i.e., the measured direction of walking points to the right of thenearest street segment), then a counter (called “Integrator” or “I”) is incremented by a smallfixed increment, ic , where ic is the gain of the controller and a tunable parameter in MAPHDE.If E is negative, then I is decremented by ic. In this fashion, repeated instances of E having thesame sign will result in repeated increments or decrements of I by ic. The reason for using abinary I-controller is that Ei can differ from zero by tens of degrees, for example, when thewalker is turning. In that case a conventional I-controller would not work well, since it wouldrespond strongly to the large value of E, even though a large E is not an indication for a largeaccumulated heading error. The proposed binary I-controller, on the other hand, is insensitiveto the magnitude of E. Rather, the controller reacts, slowly, to E having the same signpersistently.

As established by equation (5), if ψi-1 > ψmap,i-1 (and thus, E < 0) then ψi-1 isimmediately to the left of ψmap,i-1, and if ψi-1 < ψmap,i-1, then ψi-1 is immediately to the right ofψmap,i-1. During straight-line walking along a street, a heading to the left of ψmap,i-1 suggests thatthe accumulated heading error, ψe,i-1, had a positive value. To counteract this error, the binaryI-controller reduces the content of the Integrator by a small value, ic. Conversely, ifψi-1 < ψmap,i-1, then the Integrator is increased by ic. Now the binary I-controller can beformulated

(6a)

where ic is the fixed increment, also considered the gain of the binary I-controller in units ofdegrees.

0for

0for

0for

1

1

1

EiI

EI

EiI

I

ci

i

ci

i

14

An alternative way of writing Equation (6a) is

ciimapii iψψII )(SIGN 11,1 (6b)

where SIGN() is a programming function that determines the sign of a number. SIGN(x)returns ‘1’ if x is positive, ‘0’ if x = 0, and ‘-1’ if x is negative. The next element in the controlloop adds the controller output to the raw measurement to give the corrected heading.

ψi = ψi-1 + ψi + Ii (7)

4.3 Discussion on the MAPHDE Algorithm

As long as ic is of greater magnitude than typical increments in gyro errors, theintegrator continuously tracks the heading error (but with an opposite sign), just like theintegrator in a conventional I-controller tracks slow-changing disturbances. At steady state, i.e.,when walking straight along a straight street, the content of the Integrator will oscillate aboutthe accumulated heading error, ψe,i, with an amplitude of ic, which is typically smaller than 1°.The strength of the MAPHDE algorithm lies in the fact that it can readily tolerate shortdeviations from walking in the same direction as a street. For example, when dodging otherpedestrians on a busy city street, when crossing a street, when turning at an intersection, orwhen walking along a curving street, short deviations from the nominal direction of the neareststreet segment are incurred. As a result, MAPHDE may increment or decrement the integratorin the wrong direction – but only for the duration of the maneuver. However, once motion isaligned again with the direction of the nearest street segment, it takes just a few steps forMAPHDE to correct the incurred error. The street direction is obtained by the modified map-matching algorithm as explained in the following section.

It is quite possible that other controllers or filters, such as a Finite Impulse Response(FIR) filter or a Kalman Filter (KF) can be designed and tuned to act in a way similar to theBinary I-controller. However, the intention in this study was not to compare the performance ofdifferent methods. Rather, the intention was to demonstrate that heading errors due to gyro driftand other slow-changing errors in a position tracking system can be compensated for andeffectively reduced to near-zero (at steady state) from map data. The proposed MAPHDEcontrol system seems advantageous because it does not require the explicit definition of amodel, nor does it require the controlled process to be linear.

4.4 Modified Map Matching Algorithm

This Map Matching algorithm searches the on-board database for street segments thatmeet two conditions:

1. The segment lies within a pre-defined distance from the walker (dmin).2. The segment has a direction that differs by no more than ψthres from the walker’s

heading.

Once these conditions are met, the perpendicular distance between the selected streetsegments is calculated as follows.

15

=( − + ) +

+ 1(8)

Y= y1 + m( X - x1 ) (9)

= ( − ) + ( − ) (10)

where (x1, y1), (x2, y2) are coordinates of the selected street segment as illustrated by Figure 6;(X,Y) is the perpendicular point coordinate on the street segment; (x3, y3) is walker’s presentposition and m is the slope of the line formed by two ((x1, y1), (x2, y2)) coordinates.

This map matching algorithm selects the street segment closest to the user’s positionand corrects the estimated heading based on the selected street direction. The estimate ofheading errors is adjusted based on a comparison between the estimated heading from the HDEengine and the present best estimate of the direction of the nearest street segment. Thisadjustment occurs only when the estimated heading and the direction of the street agree to agreat extend i.e., when |Ei| < ψthres. ψthres is a tunable parameter for the algorithm, typicallybetween 20° - 45°.

In the modified map matching algorithm, only the walker’s heading is matched to thenearest street direction, without matching the user’s position to the street coordinates. Also, atintersections, all HDE corrections are temporarily suspended by setting ic to zero as theconfidence drops off.

An intersection is detected by the sudden changes in the minimum distance of the userfrom the nearest street. As the user approaches an intersection, the nearest street segmentchanges and so does the distance to the nearest street segment. In Figure 7, the continuous

Figure 6: Coordinates of the street segment, the walker and the perpendicular point

Walker’s currentposition (x3,y3)

Road segmentcoordinate (x1, y1)

Road segmentcoordinate(x2, y2)

Nearest point towalker’s position (X,Y)

16

marked yellow (light-colored) region indicates the intersection where the modified mapmatching algorithm was temporary suspended.

Figure 7: Suspension of the map-matching algorithm near an intersection, indicated by the light, yellow dots.

4.5 Initialization

The proposed method requires that users follow an initialization procedure that has twocomponents: Specification of: (1) an initial position and (2) an initial heading. Initialization is asignificant problem on its own and this paper does not attempt to offer a comprehensive,commercialization-ready solution. Nonetheless, a few possibilities are discussed below.

4.5.1 Initial position

For a commercial position tracking system for police or other first responders, apossible solution is to implement the tracking system in a hand-held device with a touch screen,similar in size and appearance to a handheld GPS device. The user then indicates his/her initialposition by tapping the starting position on the screen that displays a local area map. Of course,it would be advised to start a walk at an easily identifiable landmark, such as the intersection oftwo streets. If a walk started in an area where GPS was available, but then transitioned into aGPS-denied area, then the last GPS-based position could serve as the initial position for theIMU-tracked portion of the walk. Conceivably, a comprehensive tracking system couldtransition from GPS-based to IMU-based tracking automatically, without user intervention.

4.5.2 Initial heading

In a similar approach, initial heading can be indicated by tapping the same touch screen asecond time (the first time was for indicating position), at a different point but on the samestreet. Since a street has only two possible directions, the second tap would unambiguouslyspecify, which of the two directions the user wishes to follow initially. In addition, the userwould have to take the first few steps in that direction along the street.

17

5. Experimental Results

This section presents the experimental results obtained with the MAPHDE algorithm bya walking subject in real-time. In all cases, two trajectories are shown. The light magenta curveshows the output of the PDR system without MAPHDE, while the black curve represents thetrajectory after MAPHDE was applied. These results focus on experiments consisting of a setof long, complex, closed-loop outdoor walks.

A total of eight walks of different durations and lengths were performed on partiallysloped streets, as illustrated by Figure 8. The three tunable parameters were kept the same forall of the walks. The walk data are shown using the GPS Visualizer toolbox [34]. Note thatground truth is not shown in these figures due to the unavailability of accurate GPS data. Analternative ground truth, based on the average distance of the walker from the center line of thestreets (as could be derived from the OpenStreetMap database) was also not feasible, becausethe subjects usually walked on sidewalks, typically several meters away from the streets’ centerlines. Also, the OpenStreetMap data specifies only a few fixed points to express the position ofcurving streets, which is not sufficient to derive ground truth for a curving street (Figure 2). Inthe absence of useable ground truth, graphical results for each of the eight walks are providedin Figure 8. A deliberate effort was made to challenge the MAPHDE method by choosingroutes with long curving segments. For straight streets MAPHDE would have performed evenbetter.

Walk 1 Walk 2

Walk 3 Walk 4

18

Walk 5 Walk 6

Walk 7 Walk 8

Figure 8: Trajectory of eight outdoor walks. Light magenta curve: trajectory from PDR data without MAPHDEcorrection. Black curve: trajectory from PDR data with MAPHDE correction.

\

A particular challenge in Walk 3 was the routing through a downtown area, where therisk of “snapping” to the wrong street was elevated due to the tight grid of streets. It is clearlyvisible that the MAPHDE results didn’t oscillate back and forth between two closely spacedparallel streets common in this dense urban environment.

It is apparent from all of these trajectories that MAPHDE substantially improvedposition estimation. In order to express the performance of MAPHDE qualitatively, a metriccalled “Return Position Error” (RPE) was defined which represents the difference between theend point as estimated by the PDR system and the starting point of the trajectory. Since thesubjects started and stopped each walk at the exact same point, the estimated trajectories shouldalso start and stop at the same point. Relative RPE, which is the percentage of RPE divided bythe total traveled distance, was also calculated. Table II provides quantitative result for theeight walks. One can see from these results that the uncorrected RPEs vary dramatically,whereas corrected RPEs are consistently small. Other differences in the magnitude of theuncorrected errors are due to differences in the duration of the walks.

19

Although this paper focuses exclusively on the correction of heading errors, it shouldbe noted that the results in Figure 8 and Table II reflect heading errors as well as errors in step-length estimation. The latter, however, are very small in the PDR system thanks to the foot-mounted IMU, which allows the application of ZUPT with every footfall (as was explained inSection 2).

Table II: Quantitative results for the eight test walks.

WalksTotal

duration

[minutes]

Totaltravel

distance

[meters]

Returnposition error

(RPE) foruncorrectedtrajectory

[meters]

Relative RPE foruncorrectedtrajectory

as percentage ofdistance traveled

[%]

RPE forcorrectedtrajectory

[meters]

Relative RPEfor corrected

trajectoryas percentage

of distancetraveled [%]

Improvementprovided byMAPHDE[ratio of

uncorrected tocorrected RPE]

Walk 1 25.48 2,197 890.77 40.6 % 16.10 0.7 % 55.3-fold

Walk 2 48.12 4,344 1739.75 40.1 % 20.29 0.5 % 85.7-fold

Walk 3 31.56 1,814 360.77 19.9 % 16.51 0.9 % 21.9-fold

Walk 4 31.93 2,477 133.21 5.4 % 11.73 0.5 % 11.3-fold

Walk 5 24.76 1,724 160.25 9.3 % 16.61 1.0 % 9.6-fold

Walk 6 24.12 1,575 53.46 3.4 % 21.18 1.3 % 2.5-fold

Walk 7 23.63 1,700 43.68 2.6 % 10.20 0.6 % 4.3-fold

Walk 8 26.83 2,276 148.37 6.5 % 10.50 0.5 % 14.1-fold

Average 29.55 2,263 441.28 16.4 % 15.39 0.7 % 25.6-fold

6. Conclusion

This paper introduced a method for using known street maps for correcting headingerrors in an IMU-based personnel tracking system. The proposed MAPHDE method combinesa map-matching technique with a heuristics-based correction algorithm. The strength of theheuristics algorithm is that it is tolerant to short-term deviations from walking along thedirections of streets. Such deviations occur routinely when the walker dodges other pedestrians,crosses streets, or turns.

Because of the difficulty of recording accurate ground truth data (a high-accuracyDifferential GPS system would have to be used), plots of all eight outdoor walks overlaid overa map are provided. The plots show the uncorrected and MAPHDE-corrected trajectories andthereby illustrate the effectiveness of the MAPHDE method. It is also evident from the plotsthat by reducing heading errors to near-zero at steady state, position errors are reducedsubstantially. The quantitative results in Table II show that MAPHDE reduces return positionerrors on average to less than one percent of distance traveled, providing a 25-fold reduction inreturn position errors on average.

Acknowledgements

This research was supported in part by the Ground Robotics Reliability Center (GRRC) at theUniversity of Michigan, with funding from government contract DoD-DoA W56H2V-04-2-0001 through the Joint Center for Robotics.

20

References

[1] Foxlin, E., “Pedestrian Tracking with Shoe-Mounted Inertial Sensors.” IEEE ComputerGraphics and Applications, vol. 25, no. 6, 2005, pp. 38–46.

[2] Mezentsev, O., Collin, J., Kuusniemi, H., and Lachapelle, G., “Accuracy Assessment of aHigh Sensitivity GPS Based Pedestrian Navigation System Aided by Low-Cost Sensors.”11th Saint Petersburg International Conference on Integrated Navigation Systems, 2004.

[3] Basnayake, C., Mezentsev, O., Lachapelle, G. and Cannon, M.E., “ An HSGPS, inertialand map-matching integrated portable vehicular navigation system for uninterrupted real-time vehicular navigation.” International Journal of Vehicle Information andCommunication Systems, vol. 1, no. 1-2, 2005, pp. 131-151.

[4] Kim, J.W., Jang, H.J., Hwang, D.-H., Park, C. “A Step, Stride and Heading Determinationfor the Pedestrian Navigation System.” J. of Global Positioning Systems, vol. 3, no. 2,2004, pp. 273–279.

[5] Ladetto, Q., Gabaglio, V., Meminod, B., Terrier, P., Schutz, Y. “ Human Walking AnalysisAssisted by DGPS.” Global Navigation Satellite System (GNSS), Edinburgh, Scotland2000.

[6] Stirling, R., Collin, J., Fyfe, K., Lachapelle, G. “An Innovative Shoe-Mounted PedestrianNavigation System.” CD-ROM Proceedings of GNSS, the European NavigationConference, 2003, pp. 103–112.

[7]Ojeda, L. and Borenstein, J., "Personal Dead-Reckoning System for GPS-deniedEnvironments." IEEE International Workshop on Safety, Security, and Rescue Robotics(SSRR2007) in Rome, Italy, September 27-29, 2007.

[8] Borenstein, J, Ojeda, L., and Kwanmuang, S, "Heuristic Reduction of Gyro Drift in aPersonal Dead Reckoning System." Journal of Navigation, vol. 62, no. 1, January 2009, pp.41-58.

[9] Borenstein, J. and Ojeda, L., “Heuristic Drift Elimination for Personnel Tracking Systems.”Journal of Navigation, Vol. 63, No. 4, Oct. 2010, pp. 591–606.

[10] Judd, T., “A Personal Dead Reckoning Module.” Presented at Institute of Navigation'sION 97, Kansas City, MO, September 1997.

[11]Ojeda, L., Borenstein, J., "Non-GPS Navigation for Emergency Responders."International Joint Topical Meeting: Sharing Solutions for Emergencies and HazardousEnvironments, February 12-15, Salt Lake City, Utah, USA, 2006.

[12]Brand, T. Philips, R., “Foot-to-Foot Measurement as an Aid to PersonalNavigation.”Institute of Navigation Annual Meeting 59th. Albuquerque, NM, June 23-25,2003.

[13]R. Stirling et al., “An Innovative Shoe-Mounted Pedestrian Navigation System,” Proc.European Navigation Conf. (GNSS), CDROM, Austrian Inst. of Navigation, 2003.

[14]P. Misra, P. Enge, “Global Positioning System, Signals, Measurements, and Performance,”2nd version, Ganga-Jamuna Press, 2006.

[15] S. Moafipoor , A. D. Grejner-Brzezinska, and C.K. Toth. “A Fuzzy Dead ReckoningAlgorithm for a Personal Navigator,” Navigation, ION Journal, vol. 55, no. 4, 2008, pp.241-255.

[16]Butz, A. Baus, J., And Kruger A., “Augmenting buildings with infrared information.”Proceedings of the International Symposium on Augmented Reality, IEEE ComputerSociety Press, pp. 93-96, 2000.

21

[17]Cho, S.Y. and Park C.G.,“MEMS Based Pedestrian Navigation System.” Journal ofNavigation, vol. 59, 2006, pp. 135–153.

[18]Newman, J., Ingram, D., and Hopper A.,“Augmented reality in a wide area sentientenvironment.” Proceedings of the IEEE and ACM International Symposium on AugmentedReality, pp. 77-86, 2001.

[19]Rashid, A. Khatun, S.S., “Performance of Ultra-Wideband Time-of-Arrival EstimationEnhanced with Synchronization Scheme.” ECTI Transactions on Electrical Eng,Electronics, and Communications, vol. 4, no. 1, 2006.

[20]Eggert R. and Raquet, J., “Evaluating the Navigation Potential of the NTSC AnalogTelevision Broadcast Signal,” Proc. ION GNSS-2004, Long Beach, CA, pp. 2436-2446,Sep 2004.

[21]Jarvis, A.M.Y., “Integration of Photogrammetric and LiDAR Data for AccurateReconstruction and Visualization of Urban Environments.” M.Sc. thesis, University ofCalgary, UCGE Report 20282, December 2008.

[22] Cinaz, B. and Kenn, H., “HeadSLAM-Simultaneous Localization and Mapping withHead-Mounted Inertial and Laser Range Sensors.” ISWC, Pittsburg, Pennsylvania, 2008.

[23]Retscher G., “Multi-sensor Systems for Pedestrian Navigation,” ION GNSS 2004Conference, Long Beach, California, USA, September 21-24, 2004.

[24]Pahlavan K., X. Li, J.-P. Makela, “Indoor Geolocation Science and Technolgy,” IEEECommunications Magazine, February 2002, pp. 112-118.

[25]Bernstein, D., Kornhauser, A., “Map matching for personal navigation assistants.”Proceedings of the 77th annual meeting of the Transportation Research Board, 11-15January, Washington D.C., 1998.

[26] U.S. Census Bureau Geography, “TIGER Page: Cartographic Boundary Files,” Jan. 18,2002. [Online]. Available: http://www.census.gov/geo/www/tiger. [Accessed: May 2,2009].

[27] “OpenStreetMap,” [Online]. Available: http://www.openstreetmap.org.[ Accessed: July10, 2009 ].

[28]Greenfeld, J.S., “Matching GPS observations to locations on a digital map.”Proceedings of the 81st Annual Meeting of the Transportation Research Board, January,Washington D.C., 2002.

[29]Kim, W., Jee, G, Lee, J., “Efficient use of digital road map in various positioning for ITS,”IEEE Symposium on Position Location and Navigation, San Deigo, CA, 2000.

[30]Fu, M., Li, Jie, Wang, M.,“A hybrid map matching algorithm based on fuzzycomprehensive Judgment,” IEEE Proceedings on Intelligent Transportation Systems, pp.613-617, 2004.

[31]Quddus, M.A., Ochieng, W.Y., Zhao, L., Noland, R.B., “A general map matchingalgorithm for transport telematics applications.” GPS Solutions, vol. 7, no. 3, 2003, pp.157-167.

[32]Scott, C.A and Drane, C.R., “Increase accuracy of motor vehicle position estimation byutilizing map data: vehicle dynamics and other information sources.” Proc. of VehicleNavigation and Information Systems, pp. 585-590, 1994.

[33]Retscher, G., “An intelligent multi-sensor system for pedestrian navigation.” Journal ofGlobal Positioning Systems, GPS Solutions, vol. 5, no. 1-2, 2006, pp.110-118.

[34]“GPS Visualizer: Do-It-Yourself Mapping” Jan. 1, 2003 [Online]. Available:http://www.gpsvisualizer.com. [Accessed: Dec. 31, 2009].