© 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:
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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!
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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)
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
⇥!
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
PARTICLE FILTER Bayesian Estimation using
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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)
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Particle Prediction
Assume the previous posterior pdf is given by
then the prior pdf has the form
p(xk−1|Zk−1) =
N⇤
i=1
wi
k−1|k−1δ(xk−1 − x
i
k−1|k−1)
p(xk |Zk−1) =
⌅dxk−1
N⇤i=1
wi
k−1|k−1p(xk |xk−1)δ(xk−1 − x
i
k−1|k−1)
=N⇤i=1
⌅dxk−1 w
i
k−1|k−1p(xk |xk−1)δ(xk−1 − x
i
k−1|k−1)
=N⇤i=1
wi
k−1|k−1p(xk |x
i
k−1|k−1) Prediction density for
particles
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Prediction Density for Particles
We know from the transition equation:
therefore
Prediction algorithm:
for i = 1,…,N
§ draw (MC random generator)
§ set
xk = f(xk−1) + vk
−
p(xk |xi
k−1|k−1) = δ (xk − (f(xk−1) + vk))
i
−
−
vi
k∼ p(vk)
∼
xi
k|k−1= f(xi
k−1|k−1) + v
i
k
wi
k|k−1= w
i
k−1|k−1
The prior pdf has the following particle representation:
| − − | −
p(xk |Zk−1) =
N⇤
i=1
wi
k|k−1δ(xk − x
i
k|k−1)
xi
k|k−1
∼
= f(xik−1|k−1
) +
) + vi
k
k |xi
k−1|k−1)
= f(
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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)
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Derivation of the Posterior PDF
The solution is given by BAYES theorem:
Prior PDF
Sensor model / Likelihood function
p(xk |Zk) =
p(zk |xk ,Zk−1) · p(xk |Z
k−1)∫
dxkp(zk |xk ,Zk−1) · p(xk |Zk−1)
=p(zk |xk) · p(xk |Z
k−1)∫
dxkp(zk |xk) · p(xk |Zk−1)
Posterior y) ∝ Likelihood function Prior ) ·
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Particle Solution to Bayesian Filtering
Applying the particle representation of the prior pdf yields
therefore we set
and obtain
p(xk |Zk) ∝ p(zk |xk) · p(xk |Z
k−1)
=N⇥
i=1
wi
k|k−1p(zk |x
i
k|k−1)δ(xk − x
i
k|k−1)
The posterior pdf has the following particle representation:
p(xk |Zk) =
N⇥
i=1
wi
k|kδ(xk − xi
k|k)
xi
k|k−1
p(zk |
· p(zk |xi
k|k−1)
wik|k = w
ik|k−1
· p(zk |xik|k−1
)
wik|k =
wik|k
⌥Nj=1
wj
k|k
xik|k = x
ik|k−1
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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.
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Resampling of Particles
Idea: Draw N particles where
with probability
and set
But: how to compute efficiently (O(N log N))?
xj
k|k = xik|k
wi
k|k
| |
wj
k|k =1
N
p m
ass
0 1
w(1) w(2) W(…) w(N)
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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 ...
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
EMITTER LOCALIZATION IN MULTIPATH ENVIRONMENTS
Application Example
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Scenario
25
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Concept
Channel-Parameters for
Hypothetical Emitter Positions
26
Direction Finding Channel Parameter Estimation
Ray Tracing Data Fusion
Localization and Tracking
Estimation of Target State
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Data Fusion
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Experimental Campaigne
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
Video (Simulation)
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
PROGRAM ASSIGNMENT Hands on,
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
!
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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!
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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
© Fraunhofer FKIE Sensor Data Fusion – Methods and Applications, 6th Lecture
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.