© Fraunhofer FKIE

Advanced Sensor Data Fusion in

Distributed Systems

SS 2019

Felix Govaers,

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

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20192

PRODUCT FORMULAIntermediate Result

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20193

Product Formula (1st & 2nd Formulation)

We have shown that

where

N�

x; y, P�

· N�

z; z, S�

= N�

x; y, P�

· N�

z; Hx, R�

S = HPH> +R

W = PH>S�1

ν = z−Hy

P =

(

P−WSW>

(P�1 +H>R�1H)�1

y =

(

y +Wν

P�P�1y +H>R�1z

�

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20194

ACCUMULATED STATE DENSITIESTrajectory as a state

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20195

Out-of-Sequence measurements

Out of sequence (OoS) measurements appear because of

n Communication delays

n Multi-path delivery in multi-hop networks

n Local data caching

n

k

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20196

Accumulated State Density (ASD)

§ Most tracking algorithms compute

§ for an object state

§ Conditioned on time series of sensor

data

§ The ASD approach considers the

trajectory

§ and computes the conditional joint

density

densities p(xl|Zk)

describe the xl

series Zk

states xk, . . . ,xn, n ≤ k,

window from a past instant

them, p(xk, . . . ,xn|Zk),

series Zk. These densities . . ,xn|

theof the

p(xk, .

p(xk�1|

,...,

tk, t

k, tk�1,

. , tn of

time

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20197

Kalman Filter ASD Fusion

z

Retrodictionz

+ Wk|k�1(z

+ Wk|k�1(z

| | �

− Wl|kWk|k�1S

Wk�1|k

� | �

Wn|n+1

|

Wn|kWk�1|k

Unified Treatment of Filtering and Smoothing

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20198

Notion of Accumulated State Densities

§ We introduce the ASD state:

§ The Markov property and Bayes theorem yield a

recursive formulation of the ASD posterior:

xk:n =

0

B

@

xk

...

xn

1

C

A

p(xk:n|Zk) =

=p(Zk,mk|xk) p(xk|xk�1) p(xk�1:n|Z

k�1)Rdxk:n p(Zk,mk|xk) p(xk|xk�1) p(xk�1:n|Zk�1)

.

(6)

Linear Gaussian Assumptions

) = N�xk; Fk|k�1xk�1, Qk|k�1

�

) = N�

zk; Hkxk, Rk

�

.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 20199

Recursive Application

What happens, if we apply these steps on the posterior of time k-1?

p(xk:n|Zk) =p(zk|xk) p(xk|xk−1) p(xk−1:n|Zk−1)

⌅

dxk:n p(zk|xk) p(xk|xk−1) p(xk−1:n|Zk−1)(

=N

�

zk; Hkxk, Rk

⇥

N�

xk; Fk|k−1xk−1, Qk|k−1

⇥

p(xk−1:n|Zk−1)⌅

dxk:n N�

zk; Hkxk, Rk

⇥

N�

xk; Fk|k−1xk−1, Qk|k−1

⇥

p(xk−1:n|Zk−1)(

=

⇤

k

l=n+1 N�

zl; Hlxl, Rl

⇥

N�

xl; Fl|l−1xl−1, Ql|l−1

⇥

N�

xn; xn|n, Pn|n

⇥

⌅

dxk:n

⇤

k

l=n+1 N�

zl; Hlxl, Rl

⇥

N�

xl; Fl|l−1xl−1, Ql|l−1

⇥

N�

xn; xn|n, Pn|n

⇥. (

Looks like one could use the product formula!

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201910

Continue Calculation (2)

What happens if we use the RTS equations on our posterior?

We should end up with something like

p(xk:n|Zk) = N�

xk; xk|k, Pk|k

⇥

k�1⌃

l=n

N�

xl − Wl|l+1xl+1; xl|k − Wl|l+1xl+1|k, Ul|k

⇥

,

(3.19)where

Ul|k = Pl|k − Wl|l+1Pl+1|kW⇥

l|l+1.

Using

Πlxk:n =

⇤

(I, O, . . . , O)xk:n, l = k

(O, . . . , −Wl|l+1, I, . . . , O) xk:n, n ≤ l < k

=

⇤

xk, l = k

xl − Wl|l+1xl+1, n ≤ l < k.

p(xk:n|Zk) =

we have

) =k

✏

l=n

N�

Πlxk:n; Πlxk:n|k, Ul|k

⇥

..here:|

xk:n|k = (x⇥

k|k, x⇥

k�1|k, . . . , x⇥

n|k)⇥,

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201911

Elementary Operations

Elementary operations on this product lead to

p(xk:n|Zk) =k

✏

l=n

N�

Πlxk:n; Πlxk:n|k, Ul|k

⇥

∝

k✏

l=n

e�

1

2(Πlxk:n�Πlxk:n|k)

⇥U

�1

l|k (Πlxk:n�Πlxk:n|k)

= e�

1

2(xk:n�xk:n|k)

⇥

⇤

�

k

l=nΠ

⇥

lU

�1

l|kΠl

⌅

(xk:n�xk:n|k)

= N�

xk:n; xk:n|k, Pk:n|k

⇥

ariance matrix P , which is given by an harmonic mean:with an ASD covariance matrix

Pk:n|k =⇤

k�

l=n

Π⇥

l U�1

l|k Πl

⌅�1

.

A Gaussian.

I knew it.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201912

The Inverse ASD Covariance Matrix

Now that is easy. I simply have to sum up..

P�1

k:n|k =⌅

�

�

�

�

�

�

�

�

�

⌦

Tk|k −W⇥

k�1|kU�1

k�1|k O · · · O

−U�1

k�1|kWk�1|k Tk�1|k −W⇥

k�2|kU�1

k�2|k

. . ....

O −U�1

k�2|kWk�2|k. . .

. . . O

.... . .

. . . Tn+1|k −W⇥

n|kUn|k

O · · · O −Un|kWn|k Tn|k

⇧

�

�

�

�

�

�

�

�

�

↵

(3.23)

Are we done now?

Not yet – we “only” have to invert this accumulated information matrix.

where| ≤ ≤

Tl|k =

⌃

�

⌥

P�1

k|k + W⇥

l�1|lU�1

l�1|kWl�1|l for l = k

U�1

l|k + W⇥

l�1|lU�1

l�1|kWl�1|l for n < l < k

U�1

n|k for l = n

.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201913

ASD Covariance Matrix

Pk:n|k =⌥

↵

↵

↵

↵

↵

↵

↵

↵

Pk|k Pk|kW⇥

k�1|k Pk|kW⇥

k�2|k · · · Pk|kW⇥

n|k

Wk�1|kPk|k Pk�1|k Pk�1|kW⇥

k�2|k�1∗ Pk�1|kW

⇥

n|k�1

Wk�2|kPk|k Wk�2|k�1Pk�1|k Pk�2|k ∗

...... ∗ ∗ ∗ Pn+1|kW

⇥

n|n+1

Wn|kPk|k Wn|k�1Pk�1|k · · · Wn|n+1Pn+1|k Pn|k

�

�

�

�

�

�

�

�

�

⌦

(3.31)

Wow! How did he do that?Wl|k =

k�1✏

λ=l

Wλ|λ+1 =

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201914

Matrix Inversion – Computed Solution

From equation block A, we obtain

BG = −BD�1CE

AE −BD�1CE = I

E = (A−BD�1C)�1

G = −D−1

CE

= −D−1

C(A−BD−1

C)−1

H = D−1

−GBD−1

= D−1 +D

−1C(A−BD

−1C)−1

BD−1

CF = −CA−1

BH

−CA−1

BH +DH = I

F = −A−1

B(D − CA−1

B)−1

From equation block B, we obtain

E = A−1

− FCA−1

= A−1 +A

−1B(D − CA

−1B)−1

CA−1

F = −(A−BD−1

C)−1BD

−1 GB = −HCA−1

B

−HCA−1

B +HD = I

H = (D − CA−1

B)−1

G = −(D − CA−1

B)−1CA

−1

Putting everything together:

✓

A B

C D

◆

−1

=

✓

(A−BD−1C)−1−(A−BD−1C)−1BD−1

−D−1C(A−BD−1C)−1 D−1 +D−1C(A−BD−1C)−1BD−1

◆

=

✓

A−1 +A−1B(D − CA−1B)−1CA−1−A−1B(D − CA−1B)−1

−(D − CA−1B)−1CA−1 (D − CA−1B)−1

◆

(584)

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201915

What the Magician did

I know these conjurors. They have elaborated tricks! Probably, the

magician defined a recursive formulation of the covariance

matrix:

Pl:n|k =

⇧

Pl|k Pl|kW⇥

l�1:n

Wl�1:nPl|k Pl�1:n|k

⌃

,

|

Wl:n =

⇤

Wl|l+1

Wl�1:nWl|l+1

⌅

where

Applying the Inversion Lemma and an induction

(k=>k+1) works!

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201916

Recursive Measurement Fusion

OK, so we have the posterior density. But how

does the prior look like?

Let zm bsian like

ates xk, .

ps ( )

. . , xn s

. . , xm, .

at xk, . . . , xm, . . . , xn =: xk:m:n ar

p(xk:m:n|Zk) ∝ p(zk|xk) p(xk|xk�1) · · ·

p(xm+1|xm) p(xm|xm�1) p(zm�1|xm�1) · · · p(xn|Zn)

=⇤

l ⇥=m

N�

zl; Hlxl, Rl

⇥

N�

xl; Fl|l�1xl�1, Ql|l�1

⇥

· N�

xm; Fm|m�1xm�1, Qm|m�1

⇥

N�

xn; xn|n, Pn|n

⇥

.

And what about the gap at time m?

N�

xm�1; xm�1|m�1, Pm�1|m�1

⇥

N�

xm; Fm|m�1xm�1, Qm|m�1

⇥

· N�

xm+1; Fm+1|mxm, Qm+1|m

⇥

= N�

xm�1; hm�1|m(xm), Rm�1|m

⇥

N�

xm; xm|m�1, Pm|m�1

⇥

· N�

xm+1; Fm+1|mxm, Qm+1|m

⇥

= N�

xm�1; hm�1|m(xm), Rm�1|m

⇥

N�

xm; hm|m+1(xm+1), Rm|m+1

⇥

· N�

xm+1; xm+1|m�1, Pm+1|m�1

⇥

. (

Fm+1|m�1 = Fm+1|mFm|m�1,

Qm+1|m�1 = Fm+1|mQm|m�1F⇤m+1|m + Qm+1|m.

⇥

at F· and Q· describe a linear flow:

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201917

And Bayes finishes it.

We obtain (m < k)

p(xk:m:n|Zk) = N�

xk; xk|k, Pk|k

⇥

k�1⌅

l=n

N�

xl; hl|l+1(xl+1), Rl|l+1

⇥

.

or (m = k+1)

p(xk:m:n|Zk) = N�

xm; xm|m�1, Pm|m�1

⇥

k⌅

l=n

N�

xl; hl|l+1(xl+1), Rl|l+1

⇥

.

and end up with

p(xk:m:n|Zk) = N�

xk:m:n; xk:m:n|k, Pk:m:n|k

⇥

.

introduce a projection matrix Π , defined by Π x

Define projections⇥

by Πmxk:m:n = xm, wctor x . The likeland a likelihood

p(zm|xk:m:n) = N�

zm; HmΠmxk:m:n, Rm

⇥

.

Then Bayes finishes it:

p(xk:m:n|zm, Zk) =p(zm|xk:m:n) p(xk:m:n|Zk)

⇤

dxk:m:n p(zm|xk:m:n) p(xk:m:n|Zk)

=�

x ; x P⇥

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201918

What are the Parameters?

The updated track state is given by

xk:m:n|k,m = xk:m:n|k + Wk:m:n(zm − HmΠmxk:m:n|k)

Pk:m:n|k,m = Pk:m:n|k − Wk:m:nSk:m:nW⇥

k:m:n,

where

Sk:m:n = HmΠmPk:m:n|kΠ⇥

mH⇥

m + Rm

Wk:m:n = Pk:m:n|kΠ⇥

mH⇥

mS�1

k:m:n.

That is the updated state. Finally.

But, can you derive a block line version?

Rudolph E. Kalman

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201919

Comparison: Kalman vs. ASD Filter

Kalman Filter ASD - Filter

z

Retrodiction /

Smoothing

z

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201920

ASD Filter

In-Sequence Processing of ASD Filter

The measurements are processed iteratively, in-

sequence.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201921

OoS Processing using ASD Filter

Out-of-Sequence Processing using ASD Filter

The measurements are processed with a random Poisson

distributed delay.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201922

DISTRIBUTED TRACKINGASD Application

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201923

Multi Sensor ASD Posterior

For S sensors, we have

p(xk:n|Zk) =

|Z

p(Z1k, . . . , ZS

k|xk) · p(xk:n|Z

k�1)✓

dxk:np(Z1k, . . . , ZS

k|xk) · p(xk:n|Zk�1)

⌘

✓

| · |Z

=

⌘S

s=1p(Zs

k|xk) · p(xk:n|Z

k�1)✓

dxk:n

⌘S

s=1p(Zs

k|xk) · p(xk:n|Zk�1)

⌘

✓⌘

| · |Z

∝

S

s=1

p(Zs

k|xk) · p(xk|xk�1:n)p(xk�1:n|Zk�1).

N�

xk; Fk|k�1xk�1, Qk|k�1

⇥

�

� | �

⇥

∝ N�

xk; Fk|k�1xk�1, SQk|k�1

⇥S

Now, use the following fact:

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201924

DASD Product Representation

Similar to the single sensor ASD we have

p(xk:n|Zk) ∝

k

l=n+1

�

S

s=1

⇧

N�

zsl ; Hs

lxl, Rs

l

⇥

� ⇥⌃

�

�

�

⇧ � ⇥

N�

xl; Fl|l�1xl�1, SQl|l�1

⇥⌃

�

· N�

xn; xn|n, Pn|n

⇥

,

(12)Assume furthermore an independent initialization at time n. Then:

p(xk:n|Zk) ∝

S

s=1

N�

xk:n; xs

k:n|k, Ps

k:n|k

⇥

,

and the global state estimate and covariance are given by

◆

Pk:n|k =

S◆

s=1

Ps �1

k:n|k

⌦�1

xk:n|k = Pk:n|k

S◆

s=1

Ps �1

k:n|kxs

k:n|k

⌦

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201925

DASD Prediction

The prior parameters at sensor node s is given by

xs

k:n|k�1 =

↵

✏

✏

✏

�

xs

k|k�1

xs

k�1|k�1

...

xs

n|k�1

�

⇣

⇣

⇣

�

, Ps

k:n|k�1 =

Ps

k|k�1Ps

k|k�1Ws ⇥

k�1:n

Ws

k�1:nPs

k|k�1Ps

k�1:n|k�1

⌦

known from previous posterior!

where

�

| �

�

where xs

k|k�1= Fk|k�1x

s

k�1|k�1

Kalman filter prediction. For the ASDPs

k|k�1 = Fk|k�1Ps

k�1|k�1F⇥

k|k�1 + SQk|k�1, ⌦

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201926

DASD Filtering

Define a projection onto the current state:

Πk = (1,O, . . . ,O)

then:

p(zsk|xk) = p(zsk|Πkxk:n)

= N�

zsk; ΠkHs

kxk:n, Rs

k

⇥

,

Measurement

function

è Kalman filter update possible!

xs

k:n|k = xs

k:n|k�1+Ws

k:n|k�1(zs

k−Hs

kΠkx

s

k:n|k�1),

(26)Ws

k:n|k�1= Ps

k:n|k�1Π

⇥

kHs ⇥

kSs �1

k,

Ss

k= Hs

kΠkP

s

k:n|k�1Π

⇥

kHs ⇥

k+Rs

k,

Ps

k:n|k = Ps

k:n|k�1−Ws

k:n|k�1Ss

kWs ⇥

k:n|k�1.

© Fraunhofer FKIE Advanced SDF in Distributed Systems #9

SS 201927

Numerical Results (RMSE relative to optimal central KF)

− − −

−

−

−

0 5 10 15 20 25 30 35 40 45 500

0.2

0.4

0.6

0.8

1

0 10 20 30 40 50 60 70 80 900.93

0.94

0.95

0.96

0.97

0.98

0.99

1

1.01

Time [s]

0 10 20 30 40 50 60 70 80 900.95

0.96

0.97

0.98

0.99

1

Time [s]

0 10 20 30 40 50 60 70 80 900.96

0.97

0.98

0.99

1

1.01

Time [s]

ASD length 1ASD length 5

ASD length 10 Infinite length

CKF

DASD

Federated_KF