1

Multisensor Data Fusion

1. The Filtering Approach:

F1(s)

F2(s)

Fk(s)

x

n1

n2

nk

z

y1

y2

yk

1 i iy x n ;i , ...,k .

1 1

1

1

1

k k

i ii i

k

ii

k

ii

z [ F (s )] x( s ) F ( s )n( s );

F (s ) .

Let : ( s ) F ( s )n( s );

D min .

(1)

(2)

(3)

2. The Compensation Approach:

1 0 1 2 2

0 1 2

n ( t ) n ( t ) ( t );n ( t );

y( t ) n ( t ) ( ( t ) ( t ));

S1

S2

n1

n2

Filter

z

y

xx

x y1+-

(4) +

(5)

2

Goals of Optimization

1 0M{ y ( t )} n ( t );

1 2( ( t ) ( t )); D min

Accelerometers

Gyro's

Instrum.ErrorsCompensation

axm

aym

azm

zm

ym

xm

External Corrections

Rotati-onalMech

Coordinates Transf

orm

Linear Velocit

ies Calculation

Position

Calculation

Rel. & Abs. Ang.

Veloci-ties

Earth Angular Rotat.

Components

AttitudeDeterminati-

on

Position R

Velocity V

Attitude

Strapdown INS

(5)1. Unbiasedness:

2. Minimal variance: (6)

, ,T

a

3

Kalman Filter Built-in to the Commercial Navigation Measurement System

4

GPS+SINS Integration

, ,T

cor cor corA V R

,T

GPS GPSV R

Sensors SINS

Kalman Filter

, ,T

A V R

, ,T

A V R GPS

1 ,k k k kx x n

2

12 12

2

0 0

02

02

0 0 0

; .k k

I a T

TCT I Ca

RT

C I T I

I

,

A

vx

r

c

(8)

(7)State Space Equations of SINS Errors:

Tc

5

Mathematical Formulation of the Kalman Filtering Problem

3 2

3 1

2 1

0

0

0

;

w w

C w w

w w

1 2 3, , .m m mx x y y z zw a g w a g w a g

;k k ky Hx Measurement: (9)0 0 0

0 0 0.

IH

I

(10)

3 3: , ( , , )ij ijDCM a R a a

where:

1 0

0

; ( ) , cov( ) .

; ( ) , cov( ) .

k k k k k k k

k k k k k k k

X X M Q

Y H X M R

(11)

1 1, , , ; , , , .n m n n m mk k k k k k k kX R Y R Q R H R R

Let: ( ) .k kM X X Filtration error: .k k ke X X

1/ ,k k k kX X (12)

: cov( ).Tk k kLet P e e

Prediction:

6

Main Requirements:

1. Zero-bias (see (5)): 0( ) ( ) .k k kM e M X X

2. Minimal Variance: ( ) ( [ ]) min .Tk k ktr P tr M e e (14)

(13)

Prediction error (derived from (10) and (12)) :

1 1 1/ / ( ) .k k k k k k k k k k k ke X X X X e (15)Covariance matrix of prediction error :

1 1 1/ / /( ) .T Tk k k k k k k k k kP M e e P Q (16)

Estimation of the prediction based on the measurement results (measurementupdate):

01 1 1/ .k k k kX K X KY (17)

01 1 1 1 1 1

01 1 1 1

01 1 1

/

/

/

( ) ( ) ( )

[( ) )

[( ) ].

k k k k k k k

k k k k k

k k k k

M e M X X M X K X KY

M X KH X K X

M E KH X K X

(18)

7

From (13) it follows:1 1/( ) .k k kM X X (19)

01 1 1( ) [( ) ] ( ).k k kM e I KH K M X (20)

From (13) and (20) it follows:0

1( )kI KH K (21)

Substituting (21) in (17), we obtain:

1 1 1 1 1/ /( ).k k k k k k kX X K Y H X (22)

Determination of the Kalman Gain K from requirement (14)

1 1 1 1 1 1 1( ) ( [ ]) ( [( )( ) ].T Tk k k k k k ktr P tr M e e tr M X X X X

1 1 1 1 1 1/ /( )k k k k k k k ke X X K Y H X

(23)

(25)

Substituting (11) in (25), we obtain:

1 1 1 1/( ) .k k k k ke I K H e K (26)

8

1 1 1 1 1T T

k k k / k k kP ( I K H )P ( I K H ) KR K . (27)

Condition of optimality:

1 0k

d[ tr( P )] .

dK

(29)

Differentiation of the traces of matrices:

2T Tdtr( AQA ) AQ, if : Q Q

dA

(28)

1 1 1 12 2 0k k / k k k( I K H )P H K R (30)

(31)11 1 1 1 1 1 1

T Tk k / k k k k / k k kK P H ( H P H R )

Taking in account (30) and (31), expression (27) can be simplified:

1 1 1k k k / kP ( I K H )P . (32)

Basic expression for Kalman Filtering are: (12), (16), (22), (31), (32).

9

Time-dependant Kalman Filter Algorithm

P(0),X(0),n, Hn,Qn,Rn.

Initial data:

1 1T

n / n n n n nP P Q 1 1n / n n nX X

11 1

T Tn n / n n n n / n n nK P H ( H P H R )

1n n n n / nP ( I K H ) P

y[n]

Measurements

z-1

z-1

n n ny H x

1

(16) (12)

(31)

(32) (22)

7

1n n n/n-1 n n n n / nX X K (Y H X )

nX

10

Discrete Stationary Kalman Filter

1 T TK P H (H PH R ) .

1 T TM PH ( HPH R ) ; Z ( I MH ) P

(33)

(34)

Command in MATLAB for discrete models (3):

[kest, K, P, M, Z]=kalman(‘sys’,Q,R) (35)

11

Block Diagram of Discrete Kalman Filter

(1/z)* I

K

text

M

y[n]

X[n/n-1]

1[ / ]X n n

Y [n/n-1]

X [n ] H

Y [n]

Hn

n

ee

12

Example: fusion of the dead reckon and radio-navigation systems

RNS

DRS

FKw1

w2

v

w

yn

1

24 1

3

4

11 0 0

10 0 00

10 0 1

10 0 0

n n n

x ( n )T

x ( n )TX W ; W

x ( n )T

x ( n )T

1 0 0 0

0 0 1 0n n nY X V ;

1 2 DRSe ( k ) x Tx . (1)

(2)

(3)

13

Multisensor Data Fusion

The Filtering Approach:

F1(s)

F2(s)

Fk(s)

x

n1

n2

nk

z

y1

y2

yk

1 i iy x n ;i , ...,k .

1 1

1

1

1

k k

i ii i

k

ii

k

ii

z [ F (s )] x( s ) F ( s )n( s );

F (s ) .

Let : ( s ) F ( s )n( s );

D min .

(1)

(2)

(3)

14

Optimal Filtration in Scalar Case.

W(s) F(s)x

n

I(s)=1

ey

1 e( s ) [W ( s )F( s ) ] x( s ) W ( s )F( s )n( s ); (4)

1 1

ee xx

nn

S ( s ) [W ( s )F( s ) ] [W ( s )F( s ) ]S ( s )

W ( s )F( s )W ( s )F( s )S ( s );

(6a)

(5)

Wiener-Hopf Equation:

1

eexx

( )nn

S ( s )W ( s )[W ( s )F( s ) ]S ( s )

F( s )

W ( s )W ( s )F( s )S ( s ) ( s );

1 ( )

xx nn xxW( s )F( s )[ S ( s ) S ( s )] S ( s ) ( s ); (7)

eeS ( s )

;F( s )

(6)

15

Wiener Factorization:

xx nnS ( s ) S ( s ) ( s ) ( s );

Wiener Separation:

0 xxS ( s )

N N ( s ) N ( s );( s )

Optimal Filter: 0

N N ( s )

F( s ) ;( s )W ( s )

(8)

(9)

(10)

Example: 2 2

25 100 10 5

0 01 10 5 1 2

xx nnS ( s ) ; S . ; W ( s ) ;

. s. s s

10 1 10 1

2 2

xx nn

( s . ) ( s . )S ( s ) S ( s ) ( s ) ( s ) ;

( s ) ( s )

100

2 0 71 10 1

8 68 0 01 10 86

2 0 07 1

xxS ( s )N ( s ) N ( s );

( s ) ( s )( . s . )

. . sN ; F( s ) . .

s . s

(11)

(12)

(13)

16

Optimal Fusion of 2 sensors.

F (s)

F1(s)

x

n

n1

z

y

y1

( s )

W(s)

W1(s)

ε

1 1 1y( s ) W ( s )x( s ) n( s ); y ( s ) W ( s )x( s ) n ( s );

01

0 1

0 1

T

W( s )Let : W ( s ) ;

W ( s )

F ( s ) F( s ) F ( s ) ;

n n n .

(1)

1 1 1 0 0 0z F(Wx n ) F (W x n ), or : z F (W x n ). (2)

0 0 0 0 0If : x W x, then : z F ( x n ). (3)

i

10 0 0 0i( s ) ( s )x( s ) x ; where : W . (4)

(5)0 0 0 0 0z i ( F )x F n .

(6)0 0 0 00 0 0 0 0 0T * * T *

x x n nS ( F )( S ) ( F ) F S F .

17

Wiener-Hopf equation:

0 0 0 00 0 00

TT T *

x x n n*

S( F )( S ) F S

F

0 0 0 0 0 00 0T T *

x x n n x xF ( S S ) S

(7)

0 0 0 0 0 0

10 0

T * * Tx x n n x x( S S ) ; ( ) S N N N ;

(9)

(8)

10 0F ( N N ) .

Example: fusion of Doppler and barometric speed sensors:

Barometric:

1 1

2

2

2

1

1 100

10 0 25

1 0 25

nn

n n

mW( s ) ; S ( s ) ;

s . s sec

mW ( s ) , S . ;

sec

Doppler:

2 3 2

0

19 203 97 0 85 10 2 17 5 8 17s s . s . s . s .F ( s ) ;

D( s ) D( s )

1.

0 5 9 63 20 85D( s ) ( s . )( s . )( s . ) where:

(10)

(11)

(12)

18

General Block Diagram of the Information Processing in the ACS.

Sources of Infor-mation

(sensors)

PP SP&AS ToI Receiver

BITE

Sources of Infor-mation

(sensors)

19

Computer Network Architecture of Boeing-787

CCR

CDN

Remote Data Concentrators

20

Transmission of Information

1. Coding of Signals. Hamming’s distance:

1st word

0 0 1 1 0 1

2nd

word1 0 0 1 0 0

Hd=3

1

nd

d ii

H h ;

Grey Code.

XOR : x y

Truth Table:

Encoding:

1

1 0

i i i

i

G B B ;

if i N , B .Decoding:

1

1 0

i i i

i

B G G ;

if i N , G .

LSB MSB

MSB LSB 0 1 1

1 00

1 0

x

y

Example: 3-digit word

0011117

1010116

1111015

0110014

0101103

1100102

1001001

0000000

GreyBin.Dec.

Max Hd= 1.

21

Angle-Code Converter

Light

0

0

0

0

0

01

1

2

34

5

6

7

0

00

0

0

0

1

1

2

3

4

5

6

7

LightBINARY ENCODER

GRAY ENCODER

22

2. Modulation of Signals

Com. Ch.

CSG

LPF

0cos( t )

yy my dy

0

20

0

11 2

21

2

m

d

y ( t ) y( t )cos( t );

y ( t ) y( t )cos ( t )

y( t )( cos( t )).

y( t ) y( t ).

0

0 0

1

2

m

Let : y( t ) cos( t ); then :

y ( t ) cos( t )cos( t )

[cos( )t cos( )t ].

ymym