1

Andrzej BANACHOWICZ

Katedra Metod Sztucznej Inteligencji i Matematyki Stosowanej

ANALIZA SYSTEMOWA

Szczecin 2011

2

ESTYMACJA I IDENTYFIKACJA

UKŁADÓW DYNAMICZNYCH



Metoda najmniejszych kwadratów



Filtr Kalmana

3

IDENTYFIKACJA

Metoda najmniejszych kwadratów.

Metoda ta została opracowana przez Legendre’a i

opublikowana w 1805 roku oraz niezależnie przez Gaussa
(1809 r.). Chociaż niektóre źródła podają inne daty. Była to
jedna z metod opracowywania pomiarów orbit ciał
niebieskich.

4

Polega ona na założeniu, że:
1. wynik pomiaru jest sumą wartości rzeczywistej i błędu

pomiaru, tj.

,

i

i

x

x







(1)

czyli błąd jest równy

,

x

x

i

i







(2)

5

2. należy tak dobrać wartości estymowane błędów (dla

wszystkich i), żeby suma kwadratów błędów (odchyleń)
była minimalna, tj.



















n

i

i

n

i

i

x

x

1

2

1

2

.

min

ˆ



(3)

Jak wiemy funkcja osiąga minimum (maksimum), gdy:
1. pierwsza pochodna jest równa zeru,
2. druga pochodna jest większa (mniejsza) od zera.

6

Liniowa aproksymacja średniokwadratowa.

Rozpatrzmy przypadek liniowy, gdy zmienne losowe X, Y

związane są funkcją liniową:

.









x

y

(4)

Jako kryterium przyjmujemy





.

1

2











n

i

i

i

x

y

S





(5)

7

Warunek wystarczający istnienia ekstremum funkcji jednej
zmiennej jest następujący.

Twierdzenie.

Jeśli funkcja f jest klasy C

2

(tzn. jest dwukrotnie

różniczkowalna i obie pochodne są ciągłe) w otoczeniu
punktu x

0

i jeśli

,

0

)

(

0





x

f

,

0

)

(

0





x

f

(6)

8

to funkcja f ma w punkcie x

0

ekstremum właściwe, przy czym

jest to:



minimum, jeśli

,

0

)

(

0





x

f



maksimum, jeśli

.

0

)

(

0





x

f

9

Twierdzenie.

Jeśli funkcja f jest klasy C

2

(tzn. jest dwukrotnie

różniczkowalna i obie pochodne są ciągłe) w otoczeniu
punktu P

0

= (x

0

, y

0

) i jeśli ma obie pochodne cząstkowe 1

rzędu w tym punkcie równe zeru

, (7)

a wyznacznik pochodnych cząstkowych 2 rzędu funkcji f jest
w tym punkcie dodatni

10

, (8)

to funkcja ta ma w punkcie P

0

ekstremum właściwe.

Charakter tego ekstremum zależy od znaku drugich
pochodnych czystych w punkcie P

0

. (9)

Jeśli są one dodatnie, to funkcja ma w punkcie P

0

minimum

właściwe, a jeśli ujemne, to – maksimum właściwe.

11

Obliczmy pierwsze pochodne wyrażenia (5) względem



i



. Korzystamy przy tym z twierdzeń dotyczących pochodnej

sumy funkcji oraz funkcji złożonej. Otrzymamy po kolei



 

















n

i

i

i

i

x

x

y

S

1

2







(10)

oraz



 

















n

i

i

i

x

y

S

1

.

1

2







(11)

12

Zgodnie z powyższym twierdzeniem powinniśmy znaleźć
takie wartości



i



, dla których pochodne (7) i (8) będą

równe zeru. Warunki te będą następujące:















n

i

i

i

i

x

x

y

1

0





(12)

oraz















n

i

i

i

x

y

1

.

0





(13)

13

Oznaczmy estymaty (oceny)



i



przez a oraz b, otrzymamy

wówczas z warunków (12) i (13) układ równań

,

1

1

1

2

















n

i

n

i

i

i

i

n

i

i

y

x

x

b

x

a

(14)













n

i

i

n

i

i

y

nb

x

a

1

1

.

(15)

Rozwiązaniem tego układu są następujące wartości stałych a i
b:

14











,

1

2

1

















n

i

i

n

i

i

i

x

x

y

y

x

x

a

(16)

.

x

a

y

b





(17)

15

Dowód

Zauważmy, że

x

n

x

n

i

i









1

(18)

oraz

y

n

y

n

i

i









1

. (19)

16

Wstawiając (18) i (19) do (15) otrzymamy

y

n

b

n

a

x

n













. (20)

A po podzieleniu obustronnie przez n oraz uporządkowaniu
otrzymamy wzór (17). Teraz podstawmy prawą stronę wzoru
(17) w miejsce b we wzorze (14) i po uwzględnieniu (18)
będziemy mieli:























n

i

i

i

n

i

i

y

x

x

a

y

x

n

x

a

1

1

2

. (21)

17

Po rozwinięciu otrzymamy























n

i

i

i

n

i

i

y

x

x

n

a

y

x

n

x

a

1

2

1

2

. (22)

Uporządkujmy względem a

































n

i

i

i

n

i

i

y

x

y

x

n

x

n

x

a

1

2

1

2

(23)

i dalej

18

2

1

2

1

x

n

x

y

x

n

y

x

a

n

i

i

n

i

i

i





















. (24)

Do licznika zależności (24) dodajmy i odejmijmy następujący
iloczyn

y

x

n





,

a do mianownika dodajmy i odejmijmy iloczyn

2

x

n



.

19

Po tych operacjach będziemy mieli

2

2

2

1

2

1

x

n

x

n

x

n

x

y

x

n

y

x

n

y

x

n

y

x

a

n

i

i

n

i

i

i









































. (25)

Wykorzystajmy ponownie zależności (18), (19). Otrzymamy

2

2

1

2

1

1

1

2

x

n

x

n

x

y

x

n

y

x

x

y

y

x

a

n

i

i

n

i

i

n

i

i

n

i

i

i





































. (26)

20

A po włączeniu pod wspólny znak sumy równanie (26)
otrzyma następującą postać



































n

i

i

i

n

i

i

i

i

i

x

x

x

n

x

y

x

y

x

x

y

y

x

a

1

2

2

1

2

, (27)

a po przekształceniu na iloczyny będziemy mieli wzór (16)



























n

i

i

n

i

i

i

x

x

y

y

x

x

a

1

2

1

.

21

Korzystając z tej postaci oraz uwzględniając wzory na
kowariancję oraz wariancje, otrzymamy inną postać tego
wzoru

2

x

xy

S

S

a



, (28)

gdzie: S

xy

– estymata kowariancji zmiennych x i y, S

x

– estyma

ta odchylenia standardowego zmiennej x.

22

Korzystając z pojęcia korelacji ostatecznie otrzymamy wzór
na współczynnik kierunkowy prostej regresji

xy

x

y

r

S

S

a



, (29)

bo

,

gdzie: r

xy

– współczynnik korelacji zmiennych x i y.

23

Obliczmy drugie pochodne S względem



oraz



.

Wykorzystamy do tego wzory (10) i (11). W tym celu wzór
(10) zapiszmy w postaci





















n

i

i

i

i

x

x

y

S

1

2







. (30)

Po wymnożeniu i rozbiciu na sumę otrzymamy

























n

i

i

n

i

i

n

i

i

i

x

x

y

x

S

1

1

2

1

2

2

2







. (31)

24

Różniczkując (31) względem





otrzymamy



0

2

1

2

2

2













n

i

i

x

S



. (32)

Analogicznie, przedstawmy wzór (11) w następującej postaci





















n

i

i

i

x

y

S

1

.

2







(33)

Stąd

.

2

2

2

1

1







n

x

y

S

n

i

i

n

i

i





















(34)

25

Obliczmy teraz drugą pochodną

0

2

2

2









n

S



. (35)

Sprawdźmy jeszcze wyznacznik (8). W tym celu obliczmy

pochodne mieszane











S

2

. Ze wzoru (31) otrzymamy, że













n

i

i

x

S

1

2

2





. (36)

Po podstawieniu (32), (35) i (36) do (8) będziemy mieli

26





























































































2

1

2

2

2

1

2

2

1

1

2

1

1

1

2

0

4

4

4

2

2

2

2

)

(

x

n

x

n

x

n

x

n

x

x

n

n

x

x

x

P

W

n

i

i

n

i

i

n

i

i

n

i

i

n

i

i

n

i

i

n

i

i

.

(37)

Po przekształceniach analogicznych jak i dla mianownika
wzoru na współczynnik a, otrzymamy że wyznacznik ten
będzie równy





0

4

4

)

(

2

2

1

2

0













x

n

i

i

S

n

x

x

n

P

W

. (38)

Wynika z tego, że spełnione są warunki twierdzenia 2. I jest
to minimum właściwe.

27

Rys. Metoda najmniejszych kwadratów.

28

Wielomianowa aproksymacja średniokwadratowa.

W przypadku aproksymacji wielomianowej będziemy

przyjmujemy funkcję aproksymującą w postaci wielomianu
ustalonego stopnia.

Załóżmy, że dany zbiór punktów

,

aproksymujemy wielomianem szóstego stopnia, tj.

.

29

Zgodnie z metodą najmniejszych kwadratów musimy teraz

oszacować

wartość

siedmiu

parametrów:

.

Szukamy więc minimum następującej funkcji (sumy

kwadratów odchyleń):

30

Sumy te są liniowe względem szacowanych parametrów!

W następnym kroku obliczamy pierwsze pochodne sumy

względem każdego parametru

.

Przyrównujemy do zera (warunek konieczny istnienia
ekstremum) i po rozwinięciu sum oraz uporządkowaniu
wyrazów otrzymamy układ liniowy:

,

31

gdzie:

jest wektorem szukanych parametrów, – macierzą,

której elementami są sumy zmiennej

w różnych potęgach,

– wektor, którego elementami są iloczyny zmiennych

.

,

32

33

,

34

35

FILTR KALMANA

Metody filtracji Kalmanowskiej można stosować na

różnych poziomach obróbki informacji nawigacyjnej.
Poczynając od obróbki pierwotnej – estymacji błędów
pomiarów nawigacyjnych (na poziomie pomiaru wielkości
fizycznych takich jak: faza, czas, amplituda itd.), a kończąc
na estymacji współrzędnych pozycji oraz innych parametrów
nawigacyjnych (wielkości geometrycznych).

36

W każdym z tych przypadków posługujemy się takim

samym algorytmem obliczeniowym.

Ze względu na to, że współcześnie posługujemy się

cyfrowymi układami pomiarowo-obliczeniowymi, to istotę
tego algorytmu przedstawimy na przykładzie dyskretnego
losowego układu dynamicznego. Dyskretny losowy układ
dynamiczny opisują dwa równania:

37



równanie stanu (model strukturalny)

x

i+1

= A

i+1,i

x

i

+ w

i

, (1)



równanie pomiarów (model pomiarowy)

z

i+1

= C

i+1

x

i+1

+ v

i+1

, (2)

38

gdzie:

x



n-wymiarowy wektor stanu,

w



r-wymiarowy wektor zakłóceń stanu,

z



m-wymiarowy wektor pomiarów,

v



p-wymiarowy wektor zakłóceń pomiarów (szum

pomiarowy),

A



n



n-wymiarowa macierz przejścia,

C



m



n-wymiarowa macierz pomiarów,

r



n, p



m.

39

Ponadto dla wektorów zakłóceń w i v zakładamy, że są to

szumy gaussowskie (o rozkładzie normalnym), o zerowym
wektorze średnim i są wzajemnie nieskorelowane.

Równanie stanu opisuje zmiany (trend) interesującego nas

wektora, a model pomiarów podaje zależność funkcyjną
pomiarów od tego wektora. Rozwiązaniem układu równań
(1), (2), przy uwzględnieniu ograniczeń nałożonych na
wektory zakłóceń, jest filtr Kalmana.

40

Estymację wektora stanu w filtrze możemy przedstawić za

pomocą poniższego schematu:



prognoza wektora stanu

~



,

x

A

x

i+1,i

i+1,i

i



(3)

gdzie

x

~



wartość prognozowana wektora stanu,

xˆ



wartość estymowana wektora stanu,

41



macierz kowariancji prognozowanego wektora stanu

P

A

P A

Q

i+1,i

i+1,i

i

i+1,i

i





T

,

(4)

gdzie Q macierz kowariancji zakłóceń stanu (wektora w),



proces innowacji



i+1

i+1

i+1 i+1,i





z

C x~

,

(5)

42



macierz kowariancji procesu innowacji

S

R

C P

C

i

i+1

i+1 i+1,i

i+1







1

T

,

(6)

gdzie: R macierz kowariancji zakłóceń pomiarów, szum
pomiarowy (wektora v),



macierz (Kalmana) wzmocnienia filtru

K

P

C S

i+1

i+1,i

i+1 i







T

1

1

,

(7)

43



ocena (estymata) wektora stanu z filtracji po wykonaniu

pomiaru z

k+1



~

,

x

x

K

i+1

i+1,i

i+1 i









1

(8)



macierz kowariancji estymowanego wektora stanu





P

I K C

P

i+1

i+1

i+1

i+1,i

 

.

(9)

44

Jak już wcześniej wspomniano algorytm obliczeniowy

pozostaje ten sam, ale w konkretnych zastosowaniach
będziemy mieli różne postacie i wymiary poszczególnych
wektorów oraz macierzy. Poniżej przedstawiamy warianty
rozwiązania nawigacji zintegrowanej oparte o różne modele
strukturalne i pomiarowe.

Przyjmując konkretny model nawigacji zintegrowanej

musimy określić dwa równania: model strukturalny oraz
model pomiarowy. Model strukturalny zdeterminowany jest
przyjętym przez nas modelem procesu nawigacyjnego.

45

Proces ten jest określony poprzez składowe wektora stanu
oraz jego ewolucję (macierz A). Wektor stanu dobieramy w
zależności od tego, jakie parametry chcemy estymować, tj.
końcowe parametry nawigacyjne lub też ich błędy (składowe
systematyczne w postaci poprawek). Ponadto musimy już z
góry uwzględnić to, czy dysponujemy możliwością
wykonywania pomiarów wielkości fizycznych pozostających
w związku funkcyjnym z estymowanymi parametrami.
Wynika z tego, że przy projektowaniu modelu strukturalnego
musimy mieć co najmniej przybliżony obraz modelu
pomiarowego. W rzeczywistości tak też postępujemy.

46

Przyjmujemy wstępną koncepcję określającą jakie

wielkości chcemy estymować i sprawdzamy, czy istnieją
odpowiednie możliwości pomiarowe.

Model pomiarowy (równanie 2) opisuje zależność

pomiarów

od

wektora

stanu.

W

przypadku

deterministycznego obliczania współrzędnych pozycji (bez
uwzględniania zakłóceń losowych stanu i pomiarów) lub
estymacji metodą najmniejszych kwadratów zależność tą
ujmujemy za pomocą macierzy Jacobiego (macierzy
gradientów powierzchni pozycyjnych). Zilustrujmy to na
przykładzie dwóch modeli nawigacji.

47

W pierwszym wykorzystujemy pomiary nawigacji

zliczeniowej (DR), satelitarnego systemu nawigacyjnego oraz
naziemnego systemu radionawigacyjnego. W drugim modelu
zastosowano tylko jeden system pozycyjny (GPS lub DGPS)
oraz dwa układy nawigacji zliczeniowej – log-żyrokompas i
nawigację inercjalną (INS).

48

Integracja nawigacji zliczeniowej z satelitarnym

systemem nawigacyjnym

i naziemnym systemem radionawigacyjnym

W tym przypadku dysponujemy pomiarami satelitarnego

systemu

nawigacyjnego

(GPS,

GLONASS,

DGPS,

DGLONASS), naziemnego systemu radionawigacyjnego
(LORAN lub system bliskiego zasięgu) oraz pomiarami z
logu i żyrokompasu.

49

W nawigacji morskiej jako elementy wektora stanu

przyjmujemy przede wszystkim współrzędne pozycji (









oraz ich pochodne, np. składowe wektora prędkości, wektora
przyspieszeń itd. Załóżmy, że wielkościami estymowanymi
będą następujące parametry: współrzędne pozycji (









, rzuty

wektora prędkości względem dna na południk i równoleżnik
(V

N

, V

E

), błąd systematyczny kąta drogi względem dna (ang.

COG – Course Over Ground) (



COG) oraz błąd

systematyczny szybkości względem dna (ang. SOG – Speed
Over Ground) (



SOG).

50

Dla tej sytuacji wektor stanu będzie miał postać:





.

,

,

,

,

,

T

SOG

COG

V

V

E

N











x

(10)

Jak pamiętamy model strukturalny tworzy równanie stanu
(wzór 1). Dlatego musimy określić także strukturę macierzy
przejścia A. Przyjmijmy ją w następującej postaci:

51

,

1

0

0

0

0

0

0

1

0

0

0

0

0

0

1

0

0

0

0

0

0

1

0

0

0

0

0

1

0

0

0

0

0

1

,

1





























































E

N

i

i

i

i

V

V

t

k

t

k





A

(11)

gdzie:

,

N

N

N

N

V

V

V

V







(12)

,

E

E

E

E

V

V

V

V







52









k

e

a

e











1

1

2

2

3

2

sin

,

k

e

a











1

2

2

sin

cos

,

(13)





szerokość geograficzna,





długość geograficzna,

a



duża półoś elipsoidy ziemskiej,

e



pierwszy mimośród elipsoidy ziemskiej.

53

Składowe prędkości średniej

N

V

i

E

V

mogą być obliczane jako

prędkość

wypadkowa

z

ciągu

pozycji

systemu

radionawigacyjnego.

Często

przyjmujemy

także

w

uproszczeniu dla pomiarów synchronicznych, że



t

i

= 1

sekunda – jest to zazwyczaj stosowane w przypadku
pomiarów synchronicznych, taktowanych z odbiornika GPS.

Elementem uzupełniającym model strukturalny jest

macierz kowariancji wektora zakłóceń stanu Q. Poszczególne
elementy tej macierzy określają rozkłady apriori zakłóceń
estymowanych wielkości.

54

Interpretacja tej macierzy z punktu widzenia praktyki

nawigacyjnej jest następująca – elementy jej wyznaczają
przedziały ufności, w których mogą znajdować się
estymowane parametry nawigacyjne. Na przykład elementy
(1,1) i (2,2) macierzy Q wyznaczają przedział myszkowania
statku, ściślej mówiąc określają zakłócenia ruchu po
szerokości i długości geograficznej. Dla wektora stanu
zdefiniowanego wzorem (10) macierz Q może przyjąć postać:

55

,

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

)

(

0

0

0

0

)

(

2

2

2

2

2

2

2

2

2

2

2

2

2

2



























































SOG

COG

V

V

V

V

V

V

V

i

V

V

i

V

V

i

V

i

i

E

E

N

E

N

N

E

N

E

N

t

k

t

k

k

t

k

k

t

k













































Q

(14)

gdzie:







zakłócenie ruchu statku po szerokości

geograficznej,







zakłócenie ruchu statku po długości geograficznej,



 







,

sin

cos

2

2

2

COG

SOG

COG

COG

SOG

V

N











(15)

56



 







,

cos

sin

2

2

2

COG

SOG

COG

COG

SOG

V

E











(16)





,

2

sin

2

1

2

2

2

COG

SOG

COGd

SOG

V

V

E

N











(17)

COG – Course Over Ground,
SOG – Speed Over Ground,

COG





błąd pomiaru COG,

SOG





błąd pomiaru SOG,

COG







błąd określenia poprawki



COG,

SOG







błąd określenia poprawki



SOG.

57

Równania (10) – (17) określają model strukturalny procesu
nawigacji, gdy estymowanymi wielkościami są współrzędne
pozycji, składowe wektora prędkości względem dna oraz
poprawki – kąta drogi względem dna, prędkości względem
dna.

Jako wielkości mierzone w modelu pomiarowym

przyjmijmy następujące parametry: współrzędne pozycji
systemu DGPS

(



DGPS

,



DGPS

), naziemnego systemu

radionawigacyjnego

(



L

,



L

), kąt drogi względem dna (COG)

i szybkość względem dna (SOG). Elementy wektora
pomiarów będą więc następujące:

58





.

,

,

,

,

,

T

SOG

COG

L

L

DGPS

DGPS











z

(18)

Macierz pomiarów będzie macierzą Jacobiego

.



































































































SOG

VSOG

COG

SOG

V

SOG

V

SOG

SOG

VSOG

SOG

COGd

COG

COG

V

COG

V

COG

COG

COG

SOG

COG

V

V

SOG

COG

V

V

SOG

COG

V

V

SOG

COG

V

V

L

L

L

L

L

L

L

L

L

L

L

L

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS

DGPS







































































































































































C

(19)

59

Po obliczeniu poszczególnych pochodnych cząstkowych i
odpowiednim uporządkowaniu otrzymamy następującą
postać macierzy C:

,

1

0

0

0

1

0

0

0

0

0

0

1

0

0

0

0

0

0

1

0

0

0

0

1

0

0

0

0

0

0

1

3

2

1

2

1















































E

E

E

B

B

C

60

,

0

dla

2

,

2

,

0

dla

2

=

,

2

2

2

1

1















E

E

N

N

V

COG

B

V

COG

B

V

COG

B

V

COG

B

.

0

i

0

dla

0

,

0

i

0

dla

0

,

cos

sin

,

sin

cos

sin

,

cos

sin

cos

2

2

2

2

3

2

2

2

2

2

2

2

2

2

1









































E

N

E

N

N

E

N

E

N

E

E

N

N

E

N

N

E

E

N

N

E

E

N

E

N

E

N

E

V

V

V

V

V

V

V

V

V

V

COG

V

COG

V

E

COG

V

V

V

V

COG

V

V

V

V

COG

E

COG

V

V

V

V

COG

V

V

V

V

COG

E

61

Uzupełnieniem

modelu

pomiarowego

jest

macierz

kowariancji zakłóceń pomiarów (wektora pomiarów):

.

2

,

,

2

2

2

2

2











































SOG

SOG

COG

SOG

SOG

SOG

SOG

SOG

COG

COG

COG

COG

COG

COG

SOG

COG

SOG

COG

SOG

COG

SOG

COG

L

L

DGPS

DGPS

L

L

DGPS

DGPS

L

L

L

L

L

DGPS

L

DGPS

L

L

L

L

L

DGPS

L

DGPS

DGPS

DGPS

L

DGPS

L

DGPS

DGPS

DGPS

DGPS

DGPS

L

DGPS

L

DGPS

DGPS

DGPS

























































































































































R

(20)

62

Ponieważ niektóre wielkości mierzone nie są ze sobą
skorelowane – np. pomiary DGPS i radionawigacyjny system
naziemny, to macierz ta uprości się do postaci:

.

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

2

2

2

2

2

2

2

2

2

2











































SOG

COG

L

L

L

L

DGPS

DGPS

DGPS

DGPS

k

k

k

k

k

k

k

k

k

k

k

k





























































R

63

Jeśli przyjmiemy konkretne wartości poszczególnych
wariancji i kowariancji występujących w tej macierzy, to
otrzymamy:

.

25

.

0

0

0

0

0

0

0

0007

.

0

0

0

0

0

0

0

8

.

6

6

.

0

0

0

0

0

6

.

0

6

.

3

0

0

0

0

0

0

3

.

2

0

0

0

0

0

0

4

2

2

2

2



























































k

k

k

k

k

k

k

k

R

Model ten został zastosowany w nawigacyjnym systemie
stabilizacji pozycji okrętu ratowniczego.

64

W algorytmie i oprogramowaniu przyjęto następujące
parametry błędów pomiarów:



system DGPS









m,







m, współrzędne są

nieskorelowane; badania przeprowadzono na Zalewie
Szczecińskim i Zatoce Pomorskiej;



radionawigacyjny system naziemny AD-2









m,







m,







m

2

(kowariancja);

badania

przeprowadzono na Zatoce Gdańskiej;



kąt drogi względem dna





COG

= 1,5

0

;



prędkość względem dna



SOG



= 0,5 węzła.

65

Integracja nawigacji inercjalnej z GPS

Innym rozwiązaniem jest sytuacja, gdy wielkościami

estymowanymi będą: współrzędne pozycji (



), rzuty

wektora prędkości względem dna na południk i równoleżnik
(V

N

, V

E

), rzuty wektora przyspieszenia względem dna na

południk i równoleżnik (a

N

, a

E

) oraz rzuty pochodnych

wektora przyspieszenia względem dna na południk i
równoleżnik (a’

N

, a’

E

). W przypadku tym wektor stanu będzie

posiadał następujące elementy:

66





.

,

,

,

,

,

,

,

T

'

'

E

N

E

N

E

N

a

a

a

a

V

V







x

(21)

Zaś macierz przejścia A będzie określona następująco:

.

1

0

0

0

0

0

0

0

0

1

0

0

0

0

0

0

0

1

0

0

0

0

0

0

0

1

0

0

0

0

2

/

0

0

1

0

0

0

0

2

/

0

0

1

0

0

6

/

0

2

/

0

0

1

0

0

6

/

0

2

/

0

0

1

2

2

3

2

3

2

,

1

















































































i

i

i

i

i

i

i

i

i

i

i

i

i

i

t

k

t

k

t

k

t

k

t

k

t

k

t

k

t

k

t

k

t

k

t

k

t

k

A

























(22)

67

Teraz ewolucja stanu jest określona przez pochodne
wyższych

rzędów

poszczególnych

estymowanych

parametrów nawigacyjnych. Macierz kowariancji zakłóceń
stanu również otrzyma postać dostosowaną do elementów
nowego wektora stanu. Tak więc będziemy mieli:

.

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

2

2

2

2

2

2

2

2

2

2

2

2

2

2

2

2

'

'

'

'

'

'



































































E

E

N

E

N

N

E

E

a

E

a

N

N

E

N

E

N

N

a

a

a

a

a

a

a

a

a

a

a

a

V

V

V

V

V

V

V

i

V

V

i

V

V

i

V

i

i

t

k

t

t

t

k

























































Q

(23)

68

Macierze kowariancji zakłóceń stanu (14) i (23) różnią się

tylko tymi elementami, które odpowiadają różnym elementom
odpowiadającym im wektorom stanu.

W tym modelu za wielkości mierzone przyjmijmy:

współrzędne pozycji systemu DGPS (



DGPS

,



DGPS

), składowe

prędkości względem południka i równoleżnika z nawigacji
zliczeniowej (V

N

, V

E

), składowe przyspieszenia względem

południka i równoleżnika z przetwornika inercjalnego (a

N

,

a

E

). Przy tych założeniach wektor pomiarów będzie wyglądał

następująco:

69





.

,

,

,

,

,

T

E

N

E

N

DGPS

DGPS

a

a

V

V







z

(24)

Macierz pomiarów, podobnie jak i wyżej, będzie macierzą
Jacobiego

.

/

/

/

/

/

/



















































































E

E

N

E

E

E

N

E

E

E

N

E

E

E

E

N

N

N

E

N

N

N

E

N

N

N

N

N

E

E

N

E

E

E

N

E

E

E

N

E

E

E

E

N

N

N

E

N

N

N

E

N

N

N

N

N

E

DGPS

N

DGPS

E

DGPS

N

DGPS

E

DGPS

N

DGPS

DGPS

DGPS

E

DGPS

N

DGPS

E

DGPS

N

DGPS

E

DGPS

N

DGPS

DGPS

DGPS

a

a

a

a

a

a

a

a

V

a

V

a

a

a

a

a

a

a

a

a

a

a

V

a

V

a

a

a

a

V

a

V

a

V

a

V

V

V

V

V

V

V

a

V

a

V

a

V

a

V

V

V

V

V

V

V

a

a

a

a

V

V

a

a

a

a

V

V

































































































































































































C

(25)

70

Obliczmy

poszczególne

pochodne

cząstkowe

i

uporządkujemy. Otrzymamy wówczas następującą bardzo
prostą postać macierzy C:

Teraz macierz pomiarów jest macierzą blokową, co

znacznie upraszcza obliczenia i w dużym stopniu zmniejsza
błędy numeryczne.

71

Macierz kowariancji zakłóceń pomiarów (wektora

pomiarów) w tym przypadku będzie wyglądała następująco:

R =

.

(26)

72

Ponieważ niektóre wielkości nie są ze sobą skorelowane –
podobnie jak i w poprzednim modelu, to macierz ta otrzyma
postać:

R =

.

73

Przyjmując konkretne wartości poszczególnych wariancji i
kowariancji możemy macierz tą uprościć do macierzy o
stałych elementach:

R =

.

74

W tym przypadku przyjęto następujące wartości wariancji i
kowariancji poszczególnych pomiarów:



systemu DGPS









m,







m, współrzędne są

nieskorelowane; badania przeprowadzono na Zalewie
Szczecińskim i Zatoce Pomorskiej, tak samo jak i w
poprzednim modelu;



składowych prędkości





V

= 0.1 m/s;



składowych przyspieszeń





a

= 0.01 m/s

2

.

Ze względu na lepsze uwzględnienie dynamiki statku, model
ten ma istotną przewagę nad modelem pierwszym.

75

Okazuje się, że dla prędkości bliskich zeru oraz pracujących
sterach aktywnych, log Dopplerowski charakteryzuje się
dużymi błędami pomiarowymi. Wtedy koniecznością,
pomimo dość wysokiej ceny przetwornika inercjalnego, jest
stosowanie modelu INS/GPS.

76