Tracking Algorithms for Multipath-Aided Indoor Localization · i to VAs 2. False positive...

TU Graz - SPSC Lab

Tracking Algorithms for Multipath-Aided Indoor

Localization

Paul Meissner and Klaus Witrisal

Graz University of Technology, Austria

6th UWB Forum on Sensing and Communication, May 5, 2011

Meissner, Witrisal UWB Forum 2011 Slide 1/19

TU Graz - SPSC Lab

Presentation overview

◮ Virtual-anchor based localization

◮ Statistical modeling of influences on multipath-aidedlocalization

◮ Tracking algorithms to enhance robustness

◮ Performance results and discussion


TU Graz - SPSC Lab

Virtual Anchor based Localization – Scenario

Scenario

◮ Room, floor plan given

◮ One single anchor node(known location)

◮ Range-based localization

−10 0 10 20 30 40 50

−15

−10

−5

0

5

10

15

20

25

30

35

x [m]

y [m

]

Room

Agent pos.

Anchor

Doors


TU Graz - SPSC Lab


Scenario




Multipath propagation

◮ Signal reaches agent alsovia reflections

◮ Reflections mapped toanchors outside room

◮ Location computable →virtual anchors (VAs)

−10 0 10 20 30 40 50

−15

−10

−5

0

5

10

15

20

25

30

35

x [m]

y [m

]

Room

Agent pos.

Anchor

Doors


TU Graz - SPSC Lab


Scenario








−10 0 10 20 30 40 50

−15

−10

−5

0

5

10

15

20

25

30

35

x [m]

y [m

]

Room

Agent pos.

Anchor

Doors

VA


TU Graz - SPSC Lab


Scenario








−10 0 10 20 30 40 50

−15

−10

−5

0

5

10

15

20

25

30

35

x [m]

y [m

]

Room

Agent pos.

Anchor

Doors

VAs


TU Graz - SPSC Lab

Virtual Anchor based Localization – Overview

How to localize?

◮ Usage of UWB signals

◮ Multipath componentsresolvable

◮ Perform ranging to each VA

Multipath extraction

◮ Extract N pseudorangesz1, . . . , zN , possible errors:

1. Mapping zi to VAs

2. False positive detections

3. Obstructed, ”invisible” VAs

0 50 100 150 200 250 300−140

−130

−120

−110

−100

−90

−80

τ [ns]C

IR [dB

]

UWB CIR

Figure: UWB-CIR, BW 7.5 GHz


TU Graz - SPSC Lab


How to localize?









0 50 100 150 200 250 300−140

−130

−120

−110

−100

−90

−80

τ [ns]C

IR [dB

]

z1

z2

z3

z4

z5

z6

z7

UWB CIR

zi (extracted)



TU Graz - SPSC Lab


How to localize?









0 50 100 150 200 250 300−140

−130

−120

−110

−100

−90

−80

τ [ns]C

IR [dB

]

z1

z2

z3

z4

z5

z6

z7

VA1

VA2

VA3

VA4

VA5

VA6

UWB CIRz

i (extracted)

VA delays



TU Graz - SPSC Lab

Statistical model for ranges and ML estimation (1)◮ A model accounting for uncertainties in zi is

pzi|p(zi|p) = PVA

∑

k

vkN (zi | ||p−pk||, σ2k)+(1−PVA)pzi,VA

(zi)

◮ p . . . Position variable

◮ PVA . . . Prob. that zi corresponds to VA

◮ vk . . . Prob. that k-th VA is visible

◮ pk . . . Position of k-th VA

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

z [m]

p(z

i | p

)

p(z

i | p)


TU Graz - SPSC Lab

Statistical model for ranges and ML estimation (1)◮ A model accounting for uncertainties in zi is

pzi|p(zi|p) = PVA

∑

k

vkN (zi | ||p−pk||, σ2k)+(1 − PVA)pzi,VA

(zi)





◮ VA-caused zi noisy observationsof distance to VA

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

z [m]

p(z

i | p

)

p(z

i | p)


TU Graz - SPSC Lab

Statistical model for ranges and ML estimation (1)

◮ A model accounting for uncertainties in zi is

pzi|p(zi|p) = PVA

∑

k

vkN (zi | ||p − pk||, σ2k)+(1−PVA)pzi,VA

(zi)






◮ Non-VA-matched zi (falsedetections)

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

z [m]

p(z

i | p

)

p(z

i | p)


TU Graz - SPSC Lab



pzi|p(zi|p) = PVA

∑

k

vkN (zi | ||p − pk||, σ2k)+(1 − PVA)pzi,VA

(zi)







◮ No mapping of zi to VAassumed, VAs might be invisible

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

z [m]

p(z

i | p

)

p(z

i | p)


TU Graz - SPSC Lab



pzi|p(zi|p) = PVA

∑

k

vkN (zi | ||p−pk||, σ2k)+(1−PVA)pzi,VA

(zi)







◮ No mapping of zi to VAassumed, VAs might be invisible

0 2 4 6 8 10 12 14 16 18 200

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

z [m]

p(z

i | p

)

p(z

i | p)


TU Graz - SPSC Lab


→ . . . multimodality, straightforward ML leads to large outliers◮ ML estimator maximizes the objective

p̂ML = arg maxp

pz|p(z|p) = arg maxp

N∏

i=1

pzi|p(zi|p)

x [m]

y [m

]

0 5 10 15 20 25

25

20

15

10

5

0

TX

RX

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

0.2

0.4

0.6

0.8

1

Errp [m]

CD

F

−25 −20 −15 −10 −5 0 5 10 15 20 250

0.2

0.4

0.6

0.8

Errp,x,ML

[m]

Norm

aliz

ed H

isto

gra

m

Err

p,x,ML

Cauchy fit

ML−estimate


TU Graz - SPSC Lab

How to obtain an accurate and robust estimator?

◮ Introduce position prior PDF and perform MAP estimation

p̂MAP = arg maxp

pp(p)pz|p(z|p)

◮ Mitigation of the multimodality

◮ Prior information by propagation of position estimate fromone time step to the next for a moving agent

◮ Use a standard state-space model

xk+1 = f(xk,wk)

yk = h(xk,vk)

◮ Include statistical models in these equations


TU Graz - SPSC Lab

Moving agent

◮ Agent moves along trajectory

◮ Characterized by motion model

xk+1 =

1 0 ∆T 00 1 0 ∆T

0 0 1 00 0 0 1

︸︷︷︸

F

xk+wk

xk = [px, py, vx, vy]Tk

◮ Models e.g. pedestrian motion

→ Prior knowledge by correlationof successive positions andfinite agent velocity

−5 0 5 10 15 20 25 30

0

2

4

6

8

10

x [m]

y [

m]

Room

Trajectory

Doors

0 10 20 30 40 50 60 70 80 90 100−2

−1

0

1

2

3Velocities

k

vx,k

, v

y,k,

[m/s

]

v

x,kv

y,kv

k

Figure: Example trajectory andvelocities


TU Graz - SPSC Lab

State-space model and estimation – Noise models

◮ Probabilistic description of observation equationyk = h(xk,vk) via PDFs of noise vk:

◮ Two possibilities for measurements yk

◮ ML-estimates: Heavy-tail distribution → Model: Bivariatesymmetric Cauchy distribution

◮ Pseudoranges: No deterministic linear h(xk, vk) →Model: Likelihood function pz|p(z|p)

→ Both issues render a Kalman filter useless


TU Graz - SPSC Lab

Modifying the Kalman filter to alleviate outliers

◮ ML-estimates as yk → KF isheavily influenced

◮ Assume x̂+

k−1is ”good”

◮ Next position probably invicinity → prior knowledge

0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [

m]

Tx Room Trajectory pML

Figure: Trajectory and MLestimates


TU Graz - SPSC Lab



◮ Assume x̂+

k−1is ”good”


◮ Place Gaussian position priorpk(p) over predicted position

0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [

m]


x̂−k

x̂+

k−1

Figure: k → k + 1 (Prediction)


TU Graz - SPSC Lab



◮ Assume x̂+

k−1is ”good”



◮ Refine the ML-measurement:

yk = arg maxp

pz|p(z|p)0 2 4 6 8 10

−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [m

]

Figure: log-likelihood of zk


TU Graz - SPSC Lab



◮ Assume x̂+

k−1is ”good”



◮ Refine the ML-measurement:

yk = arg maxp

pk(p)pz|p(z|p)

◮ Finally, Kalman update

0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [m

]

Figure: log-likelihood · prior


TU Graz - SPSC Lab

KF with measurement refinement – Performance (example)

−5 0 5 10 15 20 25 30

0

2

4

6

8

10

x [m]

y [m

]

Room

Traj.

TX

Doors

0 10 20 30 40 50 60 70 80 90 100−2

−1

0

1

2

3Velocities

k

vx,k

, v

y,k, [m

/s]

v

x,kv

y,kv

k

Figure: Performance of KF, perfect initialization, refinement makes KF arobust estimator, parameter tuning effort reduced


TU Graz - SPSC Lab

KF with measurement refinement – Performance (example)

0 5 10 15 20 25

−5

0

5

10

15

x [m]

y [m

]


KFref

Figure: Performance of KF, perfect initialization, refinement makes KF arobust estimator, parameter tuning effort reduced


TU Graz - SPSC Lab

Initialization – Gaussian sum filter (GSF)

◮ Previous KF variant assumesgood estimate at k − 1

◮ Initialization? E.g. at doors

◮ GSF: M parallel (refined) KFs

◮ Posterior PDF of xk mixture ofM Gaussians

0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [m

]

Figure: Initialization of KF


TU Graz - SPSC Lab






◮ KFs can compute”responsibility” for currentmeasurement

◮ Weights of all but one KFdecay quickly

0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [m

]

Figure: GSF initialization


TU Graz - SPSC Lab








0 2 4 6 8 10−1

0

1

2

3

4

5

6

7

8

9

x [m]

y [

m]


Figure: First 10 ML-estimates


TU Graz - SPSC Lab








0 1 2 3 4 5 6 7 8 9 10

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

k

KF

we

igh

ts

KF 1

KF 2

KF 3

Figure: Weights of KFs


TU Graz - SPSC Lab

Gaussian sum filter – Performance (example)

0 5 10 15 20 25

−5

0

5

10

15

x [m]

y [m

]

Tx Room Trajectory pML GSF

Figure: Performance of GSF, random init with M = 10


TU Graz - SPSC Lab

Gaussian sum filter – Performance (example)

−1 0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3

4

5

6

7

8

x [m]

y [m

]

Tx Room Trajectory pML GSF

Figure: Performance of GSF, random init with M = 10, zoom


TU Graz - SPSC Lab

Bayesian State estimation – Particle filters

◮ KF-based schemes can not fully account for statistical model◮ yk . . . ML-estimate: Cauchy distribution◮ yk . . . Pseudorange-vector: Likelihood function pz|p(z|p)

→ Particle filter for Bayesian state estimation

◮ Initialization: Select set of initial particles, equal weight

◮ Prediction: Propagate each particle via state equation

◮ Update: Likelihood p(yk|particle) for measurement

1. ML-estimates: Bivariate Cauchy distr. centered at ML-est.2. Pseudoranges: Value of pz|p(z|p) at particle

◮ Resampling: Posterior particles drawn from predicted onesaccording to likelihoods

◮ Compute estimate from particles (e.g. x and y median)


TU Graz - SPSC Lab

Example – Particle filter with pseudorages

0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab


0 2 4 6 8 10

0

2

4

6

8

x [m]

y [m

]

x [m]

y [m

]

0 2 4 6 8 10

0

2

4

6

8

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

px

rel. #

part

icle

s

0 2 4 6 80

0.2

0.4

0.6

0.8

1

py

rel. #

part

icle

s


TU Graz - SPSC Lab

Particle filters – Performance (example)

0 5 10 15 20 25

−5

0

5

10

15

x [m]

y [m

]


PFrange

Figure: Performance of PF with pseudorange measurements


TU Graz - SPSC Lab

Particle filters – Performance (example)

0 5 10 15 20 25

−5

0

5

10

15

x [m]

y [m

]


PFML

Figure: Performance of PF with ML measurements, Cauchy model


TU Graz - SPSC Lab

All estimators – Performance comparison

0 0.5 1 1.5 2 2.5 30

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Errp [m]

CD

F

MLKF

ref

GSFPF

range

PFML

Figure: Average performance for 50 runs over trajectory


TU Graz - SPSC Lab

All estimators – Performance – Correlated Visibilities◮ We expect VA-visibilities to have correlated behavior→ KFs drawn away from true trajectory (prior is wrong)→ PFs show less sensitivity

0 0.5 1 1.5 2 2.5 30

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Errp [m]

CD

F

MLKF

ref, iid

PFrange, iid

PFML, iid

KFref

, HMM

PFrange, HMM

PFML, HMM


TU Graz - SPSC Lab

Conclusions and ongoing work

◮ Concept for multipath-aided localization and tracking

◮ Dealing with multimodal/heavy-tail models in our concept

◮ Variants of state-space estimators to gain robustness

◮ Effective use of floor plans and signal reflections

Ongoing work

◮ Multipath extraction from UWB impulse response

◮ Validation/extension of models with measurements

◮ Tracking of VAs

◮ Thanks for your attention!


Date post:	13-Jul-2020
Category:	Documents
Upload:	others
View:	1 times
Download:	0 times

Tracking Algorithms for Multipath-Aided Indoor Localization · i to VAs 2. False positive...

Documents