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