Indoor Navigation System UsingFoot-Mounted IMUs

Micha l Meina, Krzysztof Rykaczewski, Bartosz Celmer

Faculty of Mathematics, Informatics and Mechanics, University of WarsawSection of Computer Science, The Main School of Fire Service, Warsaw

Introduction

Multi IMU

Polulu Altimu-10

Indoor Navigation for Fire Fighters:I Non-GPSI Infrastructure-free

Dead Reckoning navigation usingfoot-mounted Micro-ElectroMechanical Systems that can yieldaccelerations, angular rates andmagnetic field strength in three axis.

ZUPT-based INS

Direct integration of the sensor readings outputs unusable results due to thesensor noise and numerical instability. Zero-velocity Update is a techniquefor inertial-based INS in which simple model of the step (namely stance phaseor zero-velocity phase) is used to correct the integration.

0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0−30

−20

−10

0

10

20

30

a[m/s

2]

0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

t[s]

−2

−1

0

1

2

3

v[m/s

]

Kalman Filter for INS

Foxlin [2] proposed a ZUPT-based technique implemented by the means ofthe Kalman Filter in which zero-velocity states are treated aspseudo-observations. Error in sensor (or foot) orientation is computed by“alignment transfer” (using Caley’s Formula) — a technique that correlateserrors in velocity with orientation estimation errors.Our approach (described below) uses model fusion: orientation is computedcontinuously by the state-of-the-art orientation filter and then utilized insimplified form of Foxlin technique.

PREDICTION

A priori state x̂k can be derived fromtime-controlled recursive equation

x̂k :=

[pk

vk

]:=

[pk−1 + vk−1∆tvk−1 + aN

k ∆t

],

(1)where conversion from sensor frame tonavigational frame is given by

aNk := qest,k⊗ aS

k ⊗ q−1est,k− g0, (2)

and error covariance

P̂k := AkPk−1ATk + Q, (3)

with state transition matrix

A :=

[I3x3 0

∆tI3x3 I3x3

]. (4)

UPDATE

When the zero-velocity test is triggeredKalman Gain matrix is computed:

Kk := PkHT(HPkHT + R)−1, (5)

and according to our initial hypothesiswe can compute a posteriori state:

xk := Kk(01x3 − Hx̂k), (6)

and a posteriori covariance error:

Pk := (I3x3− KkH)P̂k−1, (7)

since our initial assumption concervelocity, observation matrix is:

H :=[03x3 I3x3

]. (8)

MODEL FUSION

Following iterative schema is used to estimate orientation:

qest,k := qest,k−1 + (q̇ω,k − βq̇ε,k)∆t. (9)

In the fusion step estimated sensor orientation qest,m is computed usingprevious estimate and quaternion calculated from gyroscope angularrates q̇ω,m. Simultaneously, the direction of estimated error q̇ε,m := ∇f

‖∇f‖ is

discarded, where ∇f := ∇qf(q̇ω,m, aE, aS) is the objective functiongradient with respect to q Objective function is defined as:

f(q, aE, aS) = q−1 ⊗ aE ⊗ q− aS. (10)

We refer to [3] for details of the Madgwick’s algorithm.

Main challenges

I Filter is not optimal (model mismatch),I Sensor calibration and biases,I Gyroscope drift,I Fast movements are not recorded by off-the-shelf circuits

Experimental Results

Experiments were conducted by our prototype implementation and also usingreference dataset [1] with ground-truth data obtained by optical system andvisual markers.

0 10 20 30 40−0.2

−0.1

0.0

0.1

0.2

0.3

0.4

z[m

]

−5 −4 −3 −2 −1 0 1 2

x[m]

−8

−6

−4

−2

0

y[m

]

Holodeck

Cayley-based KF

Madgwick + KF

Path estimated by state-of-the-art algorithm(green), our model fusion technique (blue)and ground-truth data(red).

−60 −40 −20 0 20 40

errx[mm]

−60

−40

−20

0

20

40

60

err y

[mm

]

P−5,5 = 0.03

P−10,10 = 0.11

Cayley-based KF

−60 −40 −20 0 20 40

−60

−40

−20

0

20

40

60

P−5,5 = 0.11

P−10,10 = 0.20

Madgwick + KF (β = 0.01)2ndWalk rectangles otherDir 020810 16 33

−30 −20 −10 0 10 20 30 40

errx[mm]

−20

−10

0

10

20

30

err y

[mm

]

P−5,5 = 0.15

P−10,10 = 0.44

Cayley-based KF

−30 −20 −10 0 10 20 30 40

−20

−10

0

10

20

30

P−5,5 = 0.49

P−10,10 = 0.74

Madgwick + KF (β = 0.01)3rdWalk straight 020810 16 41

−100 −80 −60 −40 −20 0 20 40

errx[mm]

−10

−5

0

5

10

15

20

err y

[mm

]

P−5,5 = 0.08

P−10,10 = 0.28

Cayley-based KF

−100 −80 −60 −40 −20 0 20 40

−10

−5

0

5

10

15

20

P−5,5 = 0.34

P−10,10 = 0.68

Madgwick + KF (β = 0.01)13thWalk withoutCarpet shape10th 060810 12 09

Probability of error on single step.

Most of the errors are introduced on certain steps (on turns and duringsudden movements), in which sensors are not able to properly record thestride due to the sensor bandwith and range. Our simple approachoutperforms existing state-of-the art implementation using the fact thatacceleration vector should be aligned with respect to the orientation estimates— this works the best for estimating orientation in motion.

QSA, QSF and ZUPT

Illustation of different correction possible during single stride [4].

Quasi-Static Acceleration Update: if v = 0 or v attains local extremum wecan assume that aN ≈ g0 therefore analytical correction of roll and pitchangles can be derived.

Quasi-Static Field Update: yaw angle can be corrected using magnetometer— additional detection should be applied due to much interference inmagnetic field indoors. See [4] for details.

Future Work

I Two shoe system (with synthetic magnetic field update),I Inertial INS with inter-agent ranging via UWB TOF,I Path-level correction using buildings maps.

Acknowledgement

The research was supported by the Polish National Centre for Research and Development(NCBiR) — Grant No. O ROB/0010/03/001 in the frame of Defence and SecurityProgrammes and Projects: “Modern engineering tools for decision support for commandersof the State Fire Service of Poland during Fire&Rescue operations in the buildings.”

Bibliography

[1] Michael Angermann, Patrick Robertson, Thomas Kemptner, and Mohammed Khider.A high precision reference data set for pedestrian navigation using foot-mounted inertial sensors.In Indoor Positioning and Indoor Navigation (IPIN), 2010 International Conference on, pages 1–6, Sept 2010.

[2] Eric Foxlin.Pedestrian tracking with shoe-mounted inertial sensors.Computer Graphics and Applications, IEEE, 25(6):38–46, Nov 2005.

[3] Sebastian O. H. Madgwick, Ravi Vaidyanathan, and Andrew J. L. Harrison.An efficient orientation filter for inertial measurement units (IMUs) and magnetic angular rate and gravity (MARG) sensor arrays.Technical report, Department of Mechanical Engineering, April 2010.

[4] Valerie Renaudin, Christophe Combettes, and Camille Marchand.Toward a free inertial pedestrian navigation reference system.In International Conference on Indoor Positioning and Indoor Navigation, volume 27, page 30th, 2014.

http://www.icra-project.org mich@mimuw.edu.pl