introduction to sensor data fusion methods and applications · sensor data fusion – methods and...

© Fraunhofer FKIE

Introduction to Sensor Data Fusion Methods and Applications

WS 16/17

Felix Govaers

Dr. rer. nat., Dipl.-Math.

© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture

PARTICLE REPRESENTATION Let machines do the actual work:


Parameters of Interest: Estimate and Covariance

The expectation of a pdf is its centroid:

E [x ] =

ZRn

dx p(x) · x

The covariance is a measure of spreading around the expectation value:

cov [x ] = E⇥

(x − E [x ])2⇤

E

−

⇤

x = E [x]i

⇥ ⇤

cov [x] = E

h

(x− x) · (x− x)>i

However: There is no general solution for the integral!


Delta Peak: a Limiting Case of a Gaussian §  Secure knowledge: delta peaks.

§  Can be interpreted as a limiting case:

−0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.20

10

20

30

40

50

60

70

80

δ(x;µ) ‘=’

⇤ x = µ

0 x ⌅= µ

δ(x;µ) ‘=’ limσ→0

N (x;µ, σ) =

•  The Kronecker Delta represents “sure knowledge”

•  It holds that

al:

δ(f) =

Zdx δ(x)f(x) = f(0)


Particle “Hypotheses”

§  Particles represent sample instances of a RV

§  A sample is an exemplary (simulated) realization of a RV

§  Conditioned on the individual sample, the particle represents “sure knowledge”:

XH

Hi = {x = xi}

H { }

p(x|Hi) = δ(x− xi)

p(Hi) =: ωiZp(x) =

XHi

p(Hi)p(x|Hi)H

Xi

ωi =

Zdx p(x) = 1


Particle Representation of a PDF

A continuous PDF can be approximated by

where

§ 

§ 

§ 

p(x) =N⌥

i=1

wiδ(x− xi )

N⇤

i=1

wi = 1

⇤

E [x] = x =N⇤

i=1

wixi

⇤

cov [x] =N⇤

i=1

wi

(

xi− x

⇥ (

xi− x

⇥!


Sampling

a) simple sampling: X(i)0:k gleichverteilt

w(i)k ∝ p(X

(i)0:k |Z1:k)

→ sehr ineffizient

X

p

b) perfect sampling: X(i)0:k ∼ p(X

(i)0:k |Z1:k)

optimale Abtastung w(i)k = 1/N

des Phasenraums → sampling ∼ p schwierig

X

p

c) importance sampling: X(i)0:k ∼ q(X

(i)0:k |Z1:k)

w(i)k ∝

p(X(i)0:k

|Z1:k )

q(X(i)0:k

|Z1:k )

→ q beliebig (im Prinzip)X

p

q

uniformly distributed

very efficient

optimal sampling of phase space

sampling difficult, depending on p

arbitrary


PARTICLE FILTER Bayesian Estimation using


Initialization of Particle Filters

Particle filters can be initialized (almost) arbitrarily.

Some methodologies:

§  Uniformly distributed

§  Next to initial measurements

§  According to a proposal distribution

Prediction

Filtering

Retrodiction (optional)

Initialization

p(x) =1

⇥FoV⇥ wi =1

N

p(x) ≈ p(z0)

≈

p(x) = q0(x)


Prediction in Bayesian Estimation

Prediction is the prior target state pdf for the next time step.

p(xk−1|Zk−1) −−−−−−−−−−→

dynamics modelp(xk |Z

k−1)

p(xk |Zk−1)

measurement zk

−−−−−−−−−−→

sensor modelp(xk |Z

k)

−3

−2

−1

0

1

2

3

−3

−2

−1

0

1

2

3

0

0.5

1

1.5

x

y

Pro

ba

bili

ty D

en

sity


Derivation of the Prior PDF

Use marginalization:

The last equation follows from the Markov property!

p(xk |Zk−1) =

dxk−1p(xk , xk−1|Z

k−1)

=

dxk−1p(xk |xk−1,Z

k−1) · p(xk−1|Zk−1)

=

dxk−1p(xk |xk−1) · p(xk−1|Z

k−1)

Use particle representation to solve the integral!

Previous posterior pdf

dynamics model


Filtering in Bayesian Estimation

Filtering is the posterior target state pdf after processing the current measurement z.

p(xk−1|Zk−1) −−−−−−−−−−→

dynamics modelp(xk |Z

k−1)

p(xk |Zk−1)

measurement zk

−−−−−−−−−−→

sensor modelp(xk |Z

k)


Particle Filter Algorithm

Init:

set x(i), w(i) for i=1,...,N

Prediction:

for i=1,...,N

draw noise v_i ~ N(0,Q)

set x(i) = f(x(i)) + v_i

Filtering:

for i=1,...,N

w(i) = w(i) * N(z; h(x(i)), R)

sum_w = sum(w)

for i=1,...,N

w(i) = w(i) / sum_w


Particle Filter Example Two simulated targets are tracked using a particle filter. The targets have a crossing trajectory. The filter keeps the number of targets correct.


Risks and Chances of a Particle Filter

Risks:

§  Numerical complexity

§  Curse of dimensionality

§  Labeling of targets is missing

§  Covariance of a state estimate

§  Degradation of particles

Chances

§  In theory optimal

§  Works for arbitrary densities and models

§  “Pointwise” evaluation of likelihood function

§  Algorithm can be parallelized

§  Constraints can be matched easily


Curse of Dimensionality

Number of particles needed for a equidistant sampling of a n-cube of size 1 with a distance of 0.01:

xk =

(

x

y

⇥

n = 2

N = 104

xk =

x

y

z

x

y

z

x

y

z

n = 9

N = 1018


Degradation of Particles

In the above scheme, particles degrade after a few steps.

Reason: one particle tends to gain the whole weight.

A measure of degradation is

If N_eff is below a threshold, e.g.

a resampling is performed.

Neff =1

⌥N

i=1w i 2

k|k

⌥

Neff ≤N

2


Resampling Algorithm

alpha = rand / N

for i = 1,...,N

s(i) = sum(w(1:i))

j = 1

for i = 1,...,N

beta = alpha + (i-1) / N

for k = j,...,N

if s(k) > beta

j = k

x‘(i) = x(k)

break

x = x‘

0 1 s(1) s(2) s(…) s(N)

alpha

beta ...


EMITTER LOCALIZATION IN MULTIPATH ENVIRONMENTS

Application Example


Scenario

25


Concept

Channel-Parameters for

Hypothetical Emitter Positions

26

Direction Finding Channel Parameter Estimation

Ray Tracing Data Fusion

Localization and Tracking

Estimation of Target State


Ray Tracer Output

multipath characterisation 1.  Relative Time of

Arrival (RToA) 2.  Azimut of Arrival

(AoA)

27

Set of predicted multipaths for an hypothetical emitter:

[ ]

1

21

ξ ξ

ξξ

ξ

θπ π

τ

ξ

=

+

=

⎛ ⎞= ∈ × −⎜ ⎟⎜ ⎟⎝ ⎠

∈ ∈

°

°

: { }

,

{ ,..., },

,

k M

k

k

k

k

h h

h

Mk


Data Fusion


Experimental Campaigne


Video (Simulation)


PROGRAM ASSIGNMENT Hands on,


Program Assignment

Presentation of Assignments: January 25, 2017 (Lecture time).

No groups.

Submit solution with name and Matrikel-no: January 22, 2017

Trajectory:

Consider an object moving in 2D on the following trajectory:

r(t) =⇣

x(t)y(t)

⌘

= A⇣

sin(ωt)sin(2ωt)

⌘

with A = v2

q, ω = q

2v

and speed and acceleration parameters: v = 300 ms

, q = 9 ms2

!


Exercise 5.4

Consider a sensor in the coordinate origin with constant revisitinterval ∆t = 5 s measuring the Cartesian position rk = r(tk) of theobjects at times tk = k∆t, k = 0,1, . . . with Gaussian measurementerrors (independent, identically distributed: i.i.d.), covariance matrix

R = σ2I, σ = 50 m.

1. Simulate with a random generator measurements zk = (zxk , zyk) according to:

zk = Hxk + σ⇣

uxk

uy

k

⌘

, mit xk =(

r(tk)>, r(tk)

>, r(tk)>)

)>,

H = (I,O,O), I = ( 1 00 1 ) , O = ( 0 0

0 0 ) , uxk, u

yk ⇠ N(0,1)

and plot the time series over the trajectory r(t)!

2. Initiate the Kalman recursion by the a priori knowledge:

p(x0) = N(

x0; x0|0, P0|0

)

, x0|0 =( z0

oo

)

, P0|0 =

✓

R O O

O v2maxI O

O O q2maxI

◆

, o = ( 00 )

3. Realize the Kalman recursion as a program and calculate the prediction xk|k−1, Pk|k−1

and filtering xk|k, Pk|k for k = 1,2, . . .. Use the dynamics model:

F =

✓

I ∆t I 1

2∆t2 I

O I ∆t IO O e−∆t/θ I

◆

, D = Σ2(1− e−2∆t/θ)⇣

O O OO O OO O I

⌘

, Σ = qmax, θ = 60s

4. Plot measurements, predictions, filtering with corresponding error ellipses (eigen va-lues/vectors!) and compare with the truth! Play with the parameters!


Exercise 6.1: Non-Linear Sensor

Replace the linear sensor from Exercise 5.4 to a non-linear one.

The measurement is given by a range-bearing observation defined as:

zk = h(xk) + uk

=

✓

α

ρ

◆

+ uk

where the parameters are given by ✓ ◆

α = arctan(x2

x1

)

ρ =q

x2

1+ x

2

2

( )

q

uk ∼ N(

O, Rk

)

Rk =

✓

σ2α 00 σ

2ρ

◆

◦

✓

σα = 1◦

σρ = 1m


Exercise 6.2: Particle Filter

Implement a particle filter which estimates the state in position and velocity using the range-bearing measurements.

Play with the parameters! N = 10000 is a good start.

Convert the first (or the first two) measurements into an initial state.

Distribute the initial particles according to a Gaussian with a covariance chosen by you.

introduction to sensor data fusion methods and applications · sensor data fusion – methods and...

Documents