Repository - Scientific Journals of the Maritime University of Szczecin - Navigational Data Fusion in a...

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

W artykule przedstawiono problem wielosensorowej fuzji danych nawigacyjnych. Podano konfigurację wielosensorowego filtru Kalmana, opracowanego w celu wyzna-czania wektora stanu obiektu, opartego na jednorodnych danych pozyskiwanych za po-mocą zestawu odbiorników satelitarnych.

Navigational Data Fusion in a Kalman Filter Space

The article deals with a problem of multisensor navigational data fusion. The mul-tisensor Kalman filter’s configuration is given. The filter has been developed to deter-mine an object vector based on uniform data obtained by a set of satellite receivers.

Wstęp

W procesie prowadzenia nawigacji przetwarzaniu podlegają informacje za-równo ze źródeł zewnętrznych, jak i wewnętrznych statku. Pokładowymi, we-wnętrznymi źródłami informacji nawigacyjnych w zdecydowanej większości sytuacji będą odbiorniki systemu nawigacji satelitarnej oraz morski radar nawi-gacyjny. 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 nawigacyj-nych i ich przetwarzanie w procesie kierowania statkiem, w tym również w pro-cesie zapobiegania kolizjom [4, 8, 9]. Problem wspólnej analizy danych nawiga-cyjnych pozyskiwanych za pomocą wielu urządzeń nawiganawiga-cyjnych 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 na-wigacyjnych jest wykorzystanie filtru Kalmana. W artykule przedstawiono mo-del 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 korygu-ją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). ) ( ) ( ) 1 (t xt wt x    (1) ) ( ) ( ) (t H xt v t yi  i  i i = 1, 2, ...l, (2) gdzie:

x(t), yi(t) – wektory stanu na wejściu i wyjściu układu, w(t), vi(t) – zakłócenia stanu układu,

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. , 0 )] ( ) ( [v t v k  Ε T i i i ≠ j; t, k, (3)





gdzie E jest wartością oczekiwaną, a zmienne w(k) i vi(k) są wektorami transpo-nowanymi 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]: ) \ ( ˆ ) ( ... ) \ ( ˆ ) ( ) \ ( ˆ 1 1 0 t t A t X t t A t X t t X    _{l l} (4) gdzie: , ,..., 2 , 1 ), (t i l Ai  są macierzami wag, ) \ ( ˆ 0 t t

X są to wartości estymowane z poszczególnych zbiorów danych z 1,2,...,l, wskaźników wynikających bezpośrednio z procesu fil-tracji.

Macierz wag wyliczana jest ze wzoru (6): 1 1 1 ) (    _   e e e A T (5) gdzie : T l A A A

A[ ₁, ₂,... ] 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;

T l I I

e[ 1,... ] macierz e składa się z bloków I ,...1 Il, gdzie każdy blok I jest macierzą jednostkową po głównej przekątnej o wymia-rach n × n. l n e n n                             1 0 0 1 1 0 0 1 1 1        n l A A A l                     1

Macierz Σ jest diagonalną macierzą złożoną z diagonalnych elementów macierzy kowariancji Pij: nl nl Pij    ( ) gdzie Pij,i,j1,2,...l (6) l l ij ij ij ij P P P P                ... ... ... ... ... n n nn n n n n j i P P P P P P P P P P                         ... ... ... ... ... .... ... ... ... ... ... .... ... ... ... ... ... ... ... ... ... ... ... ... ... ... ... 2 1 2 22 21 1 12 11

Macierz kowariancji wyliczana jest ze wzoru:

T j j n T T ij i i n ij t t I K t H P t t Q I K t H P( 1\ 1)[  ( 1) ][ ( \ )   ][  ( 1) ] (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:

1 1 0 ( )     e e P T (8) lub 1 1 1 1 1 1 0 ( ) ( ) ( )       _{   }   e e e P nl nl e e e P T T _ij T (9)

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: , ) 0 | 0 ( ˆ ₀ x Pi(0|0)P₀ (10) P0 =Pij(0\0) = n n             2 3 2 2 2 1 0 0 0 0 0 0    , przykładowo ₁2 = ₂2 = ₃2 = 0,1

Filtr Kalmana możemy przedstawić za pomocą następujących równań: ) \ 1 ( ] ) 1 ( [ ) 1 \ 1 (t t I K t H P t t Pi    n i  i i  (11) T T i i t t P t t Q P( 1\ ) ( \ )   (12) ] ) \ 1 ( [ ) \ 1 ( ) 1 ( _{i i}T _{i i i}T _i i t P t t H HP t t H R K       (13) ) \ 1 ( ˆ ) 1 ( ) 1 (t yi t Hixt t i       (14) ) \ ( ˆ ) \ 1 ( ˆ _{t t X t t} X   (15) ) 1 ( ) 1 ( ) \ 1 ( ˆ ) 1 \ 1 ( ˆ _{t t X t t K t t} X _i _i (16)

3. Aplikacja wielosensorowego filtru Kalmana

Wykonany eksperyment badawczy zakładał posiadanie zestawu odbiorni-kó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].

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ć:

YK XK YK YK YK XK XK XK XK a x v a u u T v T x x _       0 0 0 0 0 2 1 2 1 (17) YK XK YK YK YK XK XK XK XK x v T a x v a u u v _10 1   0 0 0 0 0 (18) YK XK YK YK YK XK XK XK XK x v a x v a u u a _10 0 1 0 0 0 1 0 (19) YK XK YK YK YK XK XK XK YK a u u T v T x a v x x _             0 0 2 1 0 0 0 2 1 (20) YK XK YK YK YK XK XK XK YK x v a x v T a u u v _10 0 0 0 1   0 0 (21) YK XK YK YK YK XK XK XK YK x v a x v a u u a 10 0 0 0 0 1 1 1 (22) Wynikające z powyższych równań macierze mają następującą postać:

                        1 0 0 0 0 0 1 0 0 0 0 2 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 2 1 2 2 T T T T T T                         1 0 0 0 0 0 0 1 0 0 0 0               ) , 0 ( ) , 0 ( 1 1 Y X Y X N N U U U          0 0 1 0 0 0 0 0 0 0 0 1 T H               Y X Y X V V V 2 2   czyli    oraz    .

Podsumowanie

Zastosowanie filtracji multi-sensorowym filtrem Kalmana pozwoli na wy-znaczenie przestrzennego wektora stanu obiektu. Dzięki temu możliwa będzie prezentacja trójwymiarowego modelu statku z wizualizacji wektorów, umiej-scowionych w wybranych miejscach statku, np.: dziób, rufa, lewa i prawa burta.

Literatura

1. 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, Gli-wice 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.

Wpłynęło do redakcji w lutym 2006 r.

Recenzent

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