Pobierz - Akademia Morska w Szczecinie

ISSN 1733-8670
ZESZYTY NAUKOWE NR 11(83)
AKADEMII MORSKIEJ
W SZCZECINIE
IV MIĘDZYNARODOWA KONFERENCJA NAUKOWO-TECHNICZNA
E X P L O-S H I P 2 0 0 6
Andrzej Stateczny,
Andrzej Lisaj,
Chafan Mohammad
Fuzja danych nawigacyjnych w przestrzeni filtru Kalmana
Słowa kluczowe: filtr Kalmana, multisensorowa fuzja danych
W artykule przedstawiono problem wielosensorowej fuzji danych nawigacyjnych.
Podano konfigurację wielosensorowego filtru Kalmana, opracowanego w celu wyznaczania wektora stanu obiektu, opartego na jednorodnych danych pozyskiwanych za pomocą zestawu odbiorników satelitarnych.
Navigational Data Fusion in a Kalman Filter Space
Key words: Kalman filter, multisensor data fusion
The article deals with a problem of multisensor navigational data fusion. The multisensor Kalman filter’s configuration is given. The filter has been developed to determine an object vector based on uniform data obtained by a set of satellite receivers.
279
Andrzej Stateczny, Andrzej Lisaj, Chafan Mohammad
Wstęp
W procesie prowadzenia nawigacji przetwarzaniu podlegają informacje zarówno ze źródeł zewnętrznych, jak i wewnętrznych statku. Pokładowymi, wewnętrznymi źródłami informacji nawigacyjnych w zdecydowanej większości
sytuacji będą odbiorniki systemu nawigacji satelitarnej oraz morski radar nawigacyjny. Zewnętrznymi źródłami informacji będą natomiast brzegowe stacje
radarowe oraz systemy AIS i VTS [10, 11].
Konieczne jest uporządkowanie zbioru różnorodnych danych nawigacyjnych i ich przetwarzanie w procesie kierowania statkiem, w tym również w procesie zapobiegania kolizjom [4, 8, 9]. Problem wspólnej analizy danych nawigacyjnych pozyskiwanych za pomocą wielu urządzeń nawigacyjnych jest typowym
problemem wielosensorowej fuzji danych. Zadanie polega na skonstruowaniu
modelu do analizy oraz selekcji informacji nawigacyjnej, jej weryfikację oraz
zarządzanie zbiorem danych nawigacyjnych w aspekcie dokładności i jakości
danych w celu udostępnienia przetworzonej informacji oficerowi wachtowemu.
Jednym z możliwych rozwiązań zadania wielosensorowej fuzji danych nawigacyjnych jest wykorzystanie filtru Kalmana. W artykule przedstawiono model wielosensorowego filtru Kalmana dla estymacji stanu danych jednorodnych.
1. Model matematyczny fuzji danych oparty na równaniach filtru
Kalmana
Działanie filtru Kalmana najogólniej polega na predykcji i korekcji. Filtr
dokonuje predykcji na podstawie wcześniejszych obliczeń, zaś składnik korygujący powstaje w wyniku pomnożenia macierzy wzmocnienia K przez różnicę
wektora pomiaru faktycznego i wektora estymowanego [5].
W przypadku, gdy obiekt jest opisany nieliniowym równaniem stanu
i nieliniowym równaniem wyjścia, można zastosować model matematyczny,
przedstawiony poniżej [6]. Składa się on z dwóch równań:
– modelu procesu (opisującego obserwowany obiekt),
– modelu pomiaru (opisującego przeprowadzony na nim pomiar).
x(t  1)  x(t )  w(t )
yi (t )  H i x(t )  vi (t )
i = 1, 2, ...l,
gdzie:
x(t), yi(t) – wektory stanu na wejściu i wyjściu układu,
w(t), vi(t) – zakłócenia stanu układu,
Φ, Hi, Γ – stałe macierze o odpowiednich wymiarach.
280
(1)
(2)
Fuzja danych nawigacyjnych w przestrzenni filtru Kalmana
Zakładamy że:
– stan początkowy x0 jest zmienną losową o rozkładzie normalnym
N(0, σx),
– zakłócenie m stanu wejścia i wyjścia układu, w(t) i vi(t), t = 1,2,...,l jest
białym szumem gaussowskim o zerowej wartości średniej i macierzami
wariancji Q i Ri. Macierz Q stanowi zaburzenie pomiarów na wejściu
układu i posiada taką samą wartość, jak macierz R będącą zaburzeniem
pomiarów na wyjściu.
Ε[vi (t )viT (k )]  0,

 w(t ) 
Ε 


vi (t ) 
w (k )
T
i ≠ j; t , k,
(3)

 Q 0 
viT (k )   
  tk

  0 Ri 

gdzie E jest wartością oczekiwaną, a zmienne w(k) i vi(k) są wektorami transponowanymi oraz δtk jest deltą funkcji Kroneckera.
2. Algorytm fuzji danych z wykorzystaniem wielosensorowego filtru
Kalmana
Fuzja zbioru danych l sensorów polega na tym, że [1, 6, 7]:
Xˆ 0 (t \ t )  A1 (t ) Xˆ 1 (t \ t )  ..... Al (t ) Xˆ l (t \ t )
(4)
gdzie:
Ai (t ), i  1,2,...,l, są macierzami wag,
Xˆ 0 (t \ t ) są to wartości estymowane z poszczególnych zbiorów danych
z 1,2,...,l , wskaźników wynikających bezpośrednio z procesu filtracji.
Macierz wag wyliczana jest ze wzoru (6):
A   1e(eT  1e) 1
(5)
gdzie :
A  [ A1, A2 ,......Al ]T macierz A składa się z bloków A1 , A2 ,..., Al , w których
każdy z bloków ma wymiar n × n, gdzie n jest ilością
zmiennych wchodzących w wektor stanu obiektu;
281
Andrzej Stateczny, Andrzej Lisaj, Chafan Mohammad
e  [ I1 ,.....I l ]T macierz e składa się z bloków I1 ,.....I l , gdzie każdy blok I
jest macierzą jednostkową po głównej przekątnej o wymiarach n × n.
11 0

 0 11


e  


1 0
n
 0 1n





 nl






 A1


A


 Al



 nl



Macierz Σ jest diagonalną macierzą złożoną z diagonalnych elementów macierzy
kowariancji Pij:
  ( Pij )nl  nl gdzie Pij , i, j  1,2,...l
 Pij

   ...

 Pij
...
...
...
Pij 

... 

Pij 
l l
 P11

 P21
 ...
Pi j  
 ...

 ...

 Pn1
P12
... ... ...
P22 ... ... ...
...
... ... ...
...
... .... ...
...
... .... ...
Pn 2 ... ... ...
(6)
P1n 

P2 n 
... 
... 

... 

Pnn  nn
Macierz kowariancji wyliczana jest ze wzoru:
Pij (t  1 \ t  1)  [ I n  Ki (t  1) H i ]  [Pij (t \ t )T  QT ]  [ I n  K j (t  1) H j ]T (7)
gdzie Pij(t\t), i,j = 1,2....l są macierzami wzajemnych kowariancji błędów po
filtracji.
Wartości początkowe Pij(0\0)= P0, gdzie P0 to jest ocena dokładności fuzji
zbioru danych wyliczana ze wzoru:
282
Fuzja danych nawigacyjnych w przestrzenni filtru Kalmana
P0  (eT 1e)1
(8)
P0  (eT  1e) 1 eT  1 ( Pij )nl  nl 1e(eT  1e) 1
(9)
lub
Dalszym etapem fuzji danych są zdefiniowane lokalne filtry Kalmana,
w których ich ilość jest równa ilości poszczególnych sensorów.
Warunki początkowe:
xˆ (0 | 0)  0 ,
Pi (0 | 0)  P0
(10)
 12 0
0 


2
P0 =Pij(0\0) =  0  2 0  , przykładowo  12 =  22 =  32 = 0,1


0  32 
 0
n n
Filtr Kalmana możemy przedstawić za pomocą następujących równań:
Pi (t  1 \ t  1)  [ I n  Ki (t  1) Hi ]Pi (t  1 \ t )
(11)
Pi (t  1 \ t )  Pi (t \ t )T  QT
(12)
Ki (t  1)  Pi (t  1 \ t ) HiT  [Hi Pi (t  1 \ t ) HiT  Ri ]
(13)
 i (t  1)  yi (t  1)  Hi xˆ(t  1 \ t )
(14)
Xˆ (t  1 \ t )  Xˆ (t \ t )
(15)
Xˆ (t  1 \ t  1)  Xˆ (t  1 \ t )  Ki (t  1) i (t  1)
(16)
3. Aplikacja wielosensorowego filtru Kalmana
Wykonany eksperyment badawczy zakładał posiadanie zestawu odbiorników GPS umiejscowionych centralnie dziób – rufa, lewa burta – prawa burta,
gdzie wektor położenia obiektu umiejscowiony będzie w środku statku. Dane
uzyskane z GPS zostały poddane w dalszej obróbce estymacji filtrem Kalmana
[2, 3, 5].
283
Andrzej Stateczny, Andrzej Lisaj, Chafan Mohammad
Wektor stanu obiektu będzie miał 6 współrzędnych: X = [xx, vx, ax, xy, vy, ay]T.
Układ równań różnicowych stanu przyjmie następującą postać:
x XK 1  1  x XK  T  v XK 
T2
 a XK  0  xYK  0  vYK  0  aYK  0  u XK  0  uYK (17)
2
v XK1  0  x XK  1 v XK  T  a XK  0  xYK  0  vYK  0  aYK  0  u XK  0  uYK
(18)
a XK 1  0  xXK  0  vXK  1 aXK  0  xYK  0  vYK  0  aYK  1 u XK  0  uYK
(19)
xYK 1  0  x XK  0  v XK  0  a XK  1  xYK  T  vYK 
T2
 aYK  0  u XK  0  uYK (20)
2
vYK 1  0  x XK  0  v XK  0  a XK  0  xYK  1 vYK  T  aYK  0  u XK  0  uYK
(21)
aYK 1  0  xXK  0  vXK  0  a XK  0  xYK  0  vYK  1 aYK  1 u XK  1 uYK
(22)
Wynikające z powyższych równań macierze mają następującą postać:

1

0

  0
0

0

0
0 0
1
T2
2
T
0
1
0 0
0
0
1 T
0
0
0 1
0
0
0 0
T
0 0

0 

0 
0 
T 2 
2 
T 

1 
0

0
1

0

0

0
U X   N (0,  1 X ) 
U 


U Y   N (0,  1Y ) 
1 0 0 0 0 0 
HT  

0 0 0 1 0 0 
VX   2 X 
V  

VY   2Y 
czyli  1X   1Y   1 oraz  2 X   2Y   2 .
284
0

0
0 
0

0

1 
Fuzja danych nawigacyjnych w przestrzenni filtru Kalmana
Podsumowanie
Zastosowanie filtracji multi-sensorowym filtrem Kalmana pozwoli na wyznaczenie przestrzennego wektora stanu obiektu. Dzięki temu możliwa będzie
prezentacja trójwymiarowego modelu statku z wizualizacji wektorów, umiejscowionych w wybranych miejscach statku, np.: dziób, rufa, lewa i prawa burta.
Literatura
Dąbrowski A., Przetwarzanie sygnałów przy użyciu procesów sygnałowych,
Poznań 1997.
2. Gessing R., Duda Z., Estymacja i sterowanie statystycznie optymalne, Gliwice 2005.
3. Kaczorek T., Teoria sterowania i systemów, PWN, Warszawa 1996.
4. Kantak T., Stateczny A., Urbański J., Podstawy automatyzacji nawigacji.
Podręcznik AMW Gdynia 1988.
5. Lisaj A., Mohammad C., Intelligent Fusion of Navigational Data, Polish
Journal of Environmental Studies, Hard Olsztyn-2005.
6. Shu-li Sun., Multi-sensor optimal information fusion Kalman filters with
application, Aerospace Science and Technology 2003.
7. Stateczny A., AIS and RADAR Data Fusion for Maritime Navigation,
ZN AM w Szczecinie 2004.
8. Stateczny A., Integration of RADAR, EGNOS/GALILEO, AIS and 3D
ECDIS. International Radar Symposium, Warszawa, 2004.
9. Stateczny A., Urbański J., Nawigacja morska jako proces przetwarzania
informacji nawigacyjnej. ZN AMW 1995 nr 3.
10. Stateczny A., Urbański J., Systemy ekspertowe w nawigacji morskiej.
ZN AMW 1996 nr 2.
11. Stranneby Dag, Cyfrowe przetwarzania sygnałów, BTC-2004.
1.
Wpłynęło do redakcji w lutym 2006 r.
Recenzent
dr hab. inż. kpt.ż.w. Roman Śmierzchalski, prof. AM w Gdyni
285
Andrzej Stateczny, Andrzej Lisaj, Chafan Mohammad
Adresy Autorów
prof. dr hab. inż. Andrzej Stateczny
Instytut Inżynierii Ruchu Morskiego, Zakład Bezpieczeństwa Nawigacyjnego
mgr inż. Andrzej Lisaj
Instytut Nawigacji Morskiej, Zakład Łączności i Cybernetyki Morskiej
Akademia Morska w Szczecinie
ul. Wały Chrobrego 1-2, 70-500 Szczecin
mgr inż. Chafan Mohammad
Politechnika Szczecińska
Wydział Informatyki
Instytut Grafiki Komputerowej i Systemów Multimedialnych
ul. Żołnierska 49, 71-210 Szczecin
286