Upload
austyn-pittmon
View
220
Download
3
Tags:
Embed Size (px)
Citation preview
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