Andrzej BANACHOWICZ
Katedra Metod Sztucznej Inteligencji i Matematyki Stosowanej
ANALIZA SYSTEMOWA
Szczecin 2011
1
ESTYMACJA I IDENTYFIKACJA
UKAADÓW DYNAMICZNYCH
Metoda najmniejszych kwadratów
Filtr Kalmana
2
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 zródła podają inne daty. Była to
jedna z metod opracowywania pomiarów orbit ciał
niebieskich.
3
Polega ona na założeniu, że:
1. wynik pomiaru jest sumą wartości rzeczywistej i błędu
pomiaru, tj.
xi = x + ei ,
(1)
czyli błąd jest równy
ei = xi - x,
(2)
4
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 n
2
Ći2
e = (x - x) = min .
i
(3)
i=1 i=1
Jak wiemy funkcja osiąga minimum (maksimum), gdy:
1. pierwsza pochodna jest równa zeru,
2. druga pochodna jest większa (mniejsza) od zera.
5
Liniowa aproksymacja średniokwadratowa.
Rozpatrzmy przypadek liniowy, gdy zmienne losowe X, Y
związane są funkcją liniową:
y =ax + b.
(4)
Jako kryterium przyjmujemy
n
2
S =
(y -axi - b) .
i
(5)
i=1
6
Warunek wystarczający istnienia ekstremum funkcji jednej
zmiennej jest następujący.
Twierdzenie.
Jeśli funkcja f jest klasy C2 (tzn. jest dwukrotnie
różniczkowalna i obie pochodne są ciągłe) w otoczeniu
punktu x0 i jeśli
ó óó
f (x0) = 0, f (x0) ą 0,
(6)
7
to funkcja f ma w punkcie x0 ekstremum właściwe, przy czym
jest to:
óó
f (x0) > 0,
minimum, jeśli
óó
f (x0 ) < 0.
maksimum, jeśli
8
Twierdzenie.
Jeśli funkcja f jest klasy C2 (tzn. jest dwukrotnie
różniczkowalna i obie pochodne są ciągłe) w otoczeniu
punktu P0 = (x0, y0) 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
9
, (8)
to funkcja ta ma w punkcie P0 ekstremum właściwe.
Charakter tego ekstremum zależy od znaku drugich
pochodnych czystych w punkcie P0
. (9)
Jeśli są one dodatnie, to funkcja ma w punkcie P0 minimum
właściwe, a jeśli ujemne, to maksimum właściwe.
10
Obliczmy pierwsze pochodne wyrażenia (5) względem a i
b. Korzystamy przy tym z twierdzeń dotyczących pochodnej
sumy funkcji oraz funkcji złożonej. Otrzymamy po kolei
n
śS
= 2
(y -axi - b)(- xi )
i
(10)
śa
i=1
oraz
n
śS
= 2
(y -axi - b)(-1).
i
(11)
śb
i=1
11
Zgodnie z powyższym twierdzeniem powinniśmy znalezć
takie wartości a i b, dla których pochodne (7) i (8) będą
równe zeru. Warunki te będą następujące:
n
(y -axi - b)xi = 0
i
(12)
i=1
oraz
n
(y -axi - b)= 0.
i
(13)
i=1
12
Oznaczmy estymaty (oceny) a i b przez a oraz b, otrzymamy
wówczas z warunków (12) i (13) układ równań
n n n
2
a
x + bx = x yi ,
i i i
(14)
i=1 i=1 i=1
n n
a yi.
x + nb =
i
(15)
i=1 i=1
Rozwiązaniem tego układu są następujące wartości stałych a i
b:
13
n
(x - x)(yi - y)
i
i=1
a = ,
n
2
(16)
(x - x)
i
i=1
b = y - ax.
(17)
14
Dowód
Zauważmy, że
n
x = n x
i
(18)
i=1
oraz
n
yi = n y
. (19)
i=1
15
Wstawiając (18) i (19) do (15) otrzymamy
n x a + nb = n y
. (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 n
2
a
x + n x (y - ax)= x yi .
i i
(21)
i=1 i=1
16
Po rozwinięciu otrzymamy
n n
2 2
a
x + n x y - a n x = x yi .
i i
(22)
i=1 i=1
Uporządkujmy względem a
n n
ł
2
aę i2 - n x + n x y =
x x yi
i
ś
(23)
i=1 i=1
i dalej
17
n
x yi - n x y
i
i=1
a =
n
2 2
(24)
x - n x .
i
i=1
Do licznika zależności (24) dodajmy i odejmijmy następujący
iloczyn
n x y
,
a do mianownika dodajmy i odejmijmy iloczyn
2
n x
.
18
Po tych operacjach będziemy mieli
n
x yi - n x y - n x y + n x y
i
i=1
a =
n
2 2 2 2
. (25)
x - n x - n x + n x
i
i=1
Wykorzystajmy ponownie zależności (18), (19). Otrzymamy
n n n
yi + n x y
x yi - yx - x
i i
i=1 i=1 i=1
a =
n
2 2 2
. (26)
x - 2n x + n x
i
i=1
19
A po włączeniu pod wspólny znak sumy równanie (26)
otrzyma następującą postać
n
(x yi - yxi - xyi + x y)
i
i=1
a =
n
2
, (27)
(xi2 - 2n x xi + x )
i=1
a po przekształceniu na iloczyny będziemy mieli wzór (16)
n
(x - x)(yi - y)
i
i=1
a =
n
2
.
(x - x)
i
i=1
20
Korzystając z tej postaci oraz uwzględniając wzory na
kowariancję oraz wariancje, otrzymamy inną postać tego
wzoru
Sxy
a =
2
, (28)
Sx
gdzie: Sxy estymata kowariancji zmiennych x i y, Sx estyma
ta odchylenia standardowego zmiennej x.
21
Korzystając z pojęcia korelacji ostatecznie otrzymamy wzór
na współczynnik kierunkowy prostej regresji
S
y
a = rxy
, (29)
Sx
bo
,
gdzie: rxy współczynnik korelacji zmiennych x i y.
22
Obliczmy drugie pochodne S względem a oraz b.
Wykorzystamy do tego wzory (10) i (11). W tym celu wzór
(10) zapiszmy w postaci
n
śS
= -2
(y -axi - b)xi .
i
(30)
śa
i=1
Po wymnożeniu i rozbiciu na sumę otrzymamy
n n n
śS
2
= -2
x yi + 2ax + 2bx .
i i i
(31)
śa
i=1 i=1 i=1
23
Różniczkując (31) względem a, otrzymamy
n
ś2S
2
= 2
x > 0 .
i
2
(32)
śa
i=1
Analogicznie, przedstawmy wzór (11) w następującej postaci
n
śS
= -2
(y -axi - b).
i
(33)
śb
i=1
Stąd
n n
śS
= -2 yi + 2a
x + 2nb.
i
(34)
śb
i=1 i=1
24
Obliczmy teraz drugą pochodną
ś2S
= 2n > 0
2
. (35)
śb
Sprawdzmy jeszcze wyznacznik (8). W tym celu obliczmy
ś2S
pochodne mieszane . Ze wzoru (31) otrzymamy, że
śaśb
n
ś2S
= 2
x .
i
(36)
śaśb
i=1
Po podstawieniu (32), (35) i (36) do (8) będziemy mieli
25
n n
2
2 2
x 2x
i i n n n n
ć
ć ć
2 2 2 2 2
i=1 i=1
W (P0 ) = = 4n =
x - ćx = 4nx - n2x 4nx - nx
n i i i i
.
i=1 Ł i=1 ł Ł i=1 ł Ł i=1 ł
Ł ł
2 2n
x
i
i=1
(37)
Po przekształceniach analogicznych jak i dla mianownika
wzoru na współczynnik a, otrzymamy że wyznacznik ten
będzie równy
n
2
2
W (P0 ) = 4n
(x - x) = 4n2Sx > 0.
i
(38)
i=1
Wynika z tego, że spełnione są warunki twierdzenia 2. I jest
to minimum właściwe.
26
Rys. Metoda najmniejszych kwadratów.
27
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.
.
28
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ń):
29
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:
,
30
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 .
,
31
32
,
33
34
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).
35
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:
36
równanie stanu (model strukturalny)
xi+1 = Ai+1,i xi + wi, (1)
równanie pomiarów (model pomiarowy)
zi+1 = Ci+1xi+1 + vi+1, (2)
37
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 - nn-wymiarowa macierz przejścia,
C - mn-wymiarowa macierz pomiarów,
r Ł n, p Ł m.
38
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.
39
Estymację wektora stanu w filtrze możemy przedstawić za
pomocą poniższego schematu:
prognoza wektora stanu
~
$
xi+1,i = Ai+1,ixi,
(3)
~
x
gdzie -wartość prognozowana wektora stanu,
Ć
x
- wartość estymowana wektora stanu,
40
macierz kowariancji prognozowanego wektora stanu
Pi+1,i = Ai+1,iPiAT + Qi,
i+1,i
(4)
gdzie Q macierz kowariancji zakłóceń stanu (wektora w),
proces innowacji
ei+1 = zi+1 - Ci+1~i+1,i,
x
(5)
41
macierz kowariancji procesu innowacji
Si+1 = Ri+1 + Ci+1Pi+1,iCT ,
i+1 (6)
gdzie: R macierz kowariancji zakłóceń pomiarów, szum
pomiarowy (wektora v),
macierz (Kalmana) wzmocnienia filtru
-1
Ki+1 = Pi+1,iCT Si+1,
i+1
(7)
42
ocena (estymata) wektora stanu z filtracji po wykonaniu
pomiaru zk+1
~
$
xi+1 = xi+1,i + Ki+1ei+1,
(8)
macierz kowariancji estymowanego wektora stanu
Pi+1 = I - Ki+1Ci+1 Pi+1,i.
( )
(9)
43
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.
44
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.
45
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.
46
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).
47
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.
48
W nawigacji morskiej jako elementy wektora stanu
przyjmujemy przede wszystkim współrzędne pozycji (j, l)
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 (j, l), rzuty
wektora prędkości względem dna na południk i równoleżnik
(VN, VE), błąd systematyczny kąta drogi względem dna (ang.
COG Course Over Ground) (DCOG) oraz błąd
systematyczny szybkości względem dna (ang. SOG Speed
Over Ground) (DSOG).
49
Dla tej sytuacji wektor stanu będzie miał postać:
T
x = [j,l,VN ,VE ,DCOG,DSOG] .
(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:
50
1 0 kj Dti 0 0 0
ł
ę0 1
0 kl Dti 0 0ś
ę ś
ę ś
0 0 1+ DVN 0 0 0
Ai+1,i = ,
ę0 0
0 1+ DVE 0 0ś
ę ś
(11)
ę0 0
0 0 1 0ś
ę ś
0 0 0 1
ę ś
0 0
gdzie:
VN -VN
DVN = ,
(12)
VN
VE -VE
DVE = ,
VE
51
3
1- e2 sin2 j
( )
kj = ,
a 1- e2
( )
1- e2 sin2 j
kl = ,
(13)
a cosj
j - szerokość geograficzna,
l - długość geograficzna,
a - duża półoś elipsoidy ziemskiej,
e - pierwszy mimośród elipsoidy ziemskiej.
52
Składowe prędkości średniej VN i V mogą być obliczane jako
E
prędkość wypadkowa z ciągu pozycji systemu
radionawigacyjnego. Często przyjmujemy także w
uproszczeniu dla pomiarów synchronicznych, że Dti = 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.
53
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ć:
54
2 2 2
kj (sj + Dti 2sV ) kj kl Dti 2sV VE ł
0 0 0 0
j N
ę ś
2 2 2
kj klDti 2sV VE kl (s + Dti 2sV ) 0 0 0 0
ę ś
l
N l
2
ę ś
0 0 sV sV VE 0 0
N N
Qi = ę ś,
2
0 0 sV VE sV 0 0
ę ś
N E
ę ś
2
0 0 0 0 s 0
DCOG
ę ś
2
ę ś
0 0 0 0 0 s
DSOG
(14)
j
gdzie: s - zakłócenie ruchu statku po szerokości
geograficznej,
sl - zakłócenie ruchu statku po długości geograficznej,
2 2
2
sV =[(s cosCOG) + (SOGsCOG sin COG) ],
SOG
(15)
N
55
2 2
2
sV =[(s sin COG) + (SOGsCOG cosCOG) ],
SOG
(16)
E
1
2 2
sV VE = (s - SOG2s )sin 2COG,
SOG COGd
N
(17)
2
COG Course Over Ground,
SOG Speed Over Ground,
sCOG
- błąd pomiaru COG,
s
- błąd pomiaru SOG,
SOG
s
DCOG
- błąd określenia poprawki DCOG,
s
- błąd określenia poprawki DSOG.
DSOG
56
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 (jDGPS, lDGPS), naziemnego systemu
radionawigacyjnego (jL , lL), kąt drogi względem dna (COG)
i szybkość względem dna (SOG). Elementy wektora
pomiarów będą więc następujące:
57
T
z = [jDGPS ,lDGPS ,jL,lL,COG, SOG] .
(18)
Macierz pomiarów będzie macierzą Jacobiego
śj śj śj śj śj śj
ł
DGPS DGPS DGPS DGPS DGPS DGPS
ę
śj śl śVj śVl śDCOG śDSOGś
ę ś
śl śl śl śl śl śl
ę ś
DGPS DGPS DGPS DGPS DGPS DGPS
ę
śj śl śVj śVl śDCOG śDSOGś
ę ś
śj śj śj śj śj śj
L L L L L L
ę ś
ę
śj śl śVj śVl śDCOG śDSOGś
C =
ę ś.
śl śl śl śl śl śl
L L L L L L
ę ś
(19)
ę śj śl śVj śVl śDCOG śDSOGś
ę ś
śCOG śCOG śCOG śCOG śCOG COGd
ę ś
śj śl śVj śVl śDCOG śDSOGś
ę
ęśVSOG śSOG śSOG śSOG śSOG śVSOG ś
ę ś
śj śl śVj śVl śDCOG śDSOGś
ę
58
Po obliczeniu poszczególnych pochodnych cząstkowych i
odpowiednim uporządkowaniu otrzymamy następującą
postać macierzy C:
1 0 0 0 0 0
ł
ę0 1 0 0 0 0 ś
ę ś
ę ś
1 0 0 0 0 0
C =
ę0 1 0 0 0 0 ś,
ę ś
ę0 0 B1 B2 -1 0 ś
ę ś
ę ś
0 0 E1 E2 E3 -1
59
COG COG
B1 = , B1 =
dla VN = 0,
2 VN 2
COG COG
B2 = , B2 =
dla VE = 0,
2 VE 2
VE VE
E1 = cos COG + VN sin COG - VE cos COG,
2 2 2 2
VN +VE VN +VE
VN VN
E2 = sin COG + VE cos COG - VN sin COG,
2 2 2 2
VN +VE VN +VE
E3 = VN sin COG -VE cos COG,
VE
= 0 dla VN = 0 i VE = 0,
2 2
VN +VE
VN
= 0 dla VN = 0 i VE = 0.
2 2
VN +VE
60
Uzupełnieniem modelu pomiarowego jest macierz
kowariancji zakłóceń pomiarów (wektora pomiarów):
2
ł
sj sjl sj jL sj lL sj COG sj SOG
DGPS DGPS DGPS DGPS DGPS DGPS
ę ś
2
sjl s s s s s
lDGPS lDGPSjL lDGPSlL lDGPSCOG lDGPSSOG
ę ś
DGPS
2
ę
sj jL s sj sjl sj COG sj SOG ś
lDGPSjL
DGPS L L L L
R = ę ś
.
2
sj lL s sjl s s s
ę lDGPSlL lL lLCOG lLSOG ś
DGPS L
2
ęsj COG s lDGPSCOG sj COG s lLCOG ś
s s
COG COG,SOG
DGPS L
ę ś
2
s
ęsj SOG s lDGPSSOG sj SOG s lLSOG s COG,SOG ś
SOG
DGPS L
(20)
61
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:
2 2
ł
kjsj kj klsjl 0 0 0 0
DGPS DGPS
ęk klsjl ś
2 2
kls 0 0 0 0
j lDGPS
ę ś
DGPS
2 2
ę ś
0 0 kjsj kj klsjl 0 0
L L
R = .
ę ś
2 2
0 0 kj klsjl kls 0 0
ę lL ś
L
2
ę ś
0 0 0 0 s 0
COG
ę ś
2
0 0 0 0 0 s
ę ś
SOG
62
Jeśli przyjmiemy konkretne wartości poszczególnych
wariancji i kowariancji występujących w tej macierzy, to
otrzymamy:
2
ł
4kj 0 0 0 0 0
ę ś
2
0 2.3kl 0 0 0 0
ę ś
2
ę ś
0 0 3.6kj 0.6kj kl 0 0
R = .
ę ś
2
0 0 0.6kj kl 6.8kl 0 0
ę ś
ę ś
0 0 0 0 0.0007 0
ę ś
0 0 0 0 0 0.25
ę ś
Model ten został zastosowany w nawigacyjnym systemie
stabilizacji pozycji okrętu ratowniczego.
63
W algorytmie i oprogramowaniu przyjęto następujące
parametry błędów pomiarów:
system DGPS - sj = 2,0 m, sl = 1,5 m, współrzędne są
nieskorelowane; badania przeprowadzono na Zalewie
Szczecińskim i Zatoce Pomorskiej;
radionawigacyjny system naziemny AD-2 - sj = 1,9 m,
sl = 2,6 m, sjl = 0,6 m2 (kowariancja); badania
przeprowadzono na Zatoce Gdańskiej;
kąt drogi względem dna - sCOG = 1,50;
s
SOG
prędkość względem dna - = 0,5 węzła.
64
Integracja nawigacji inercjalnej z GPS
Innym rozwiązaniem jest sytuacja, gdy wielkościami
estymowanymi będą: współrzędne pozycji (j, l), rzuty
wektora prędkości względem dna na południk i równoleżnik
(VN, VE), rzuty wektora przyspieszenia względem dna na
południk i równoleżnik (aN, aE) 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:
65
T
' '
x =[j,l,VN ,VE ,aN ,aE ,aN ,aE] . (21)
Zaś macierz przejścia A będzie określona następująco:
1 0 kj Dti ł
0 kj Dti 2 / 2 0 kj Dti 3 / 6 0
ę ś
0 klDti 2 / 2 0 klDti 3 / 6ś
ę0 1 0 klDti
ę0 0 1 ś
0 kj Dti 0 kj Dti 2 / 2 0
ę ś
1 0 klDti 0 klDti 2 / 2ś
ę0 0 0
Ai+1,i =
ę0 0 0 ś.
0 1 0 kj Dti 0
ę ś
0 0 0 0 0 1 0 klDti
ę ś
ę0 0 0 ś
0 0 0 1 0
ę ś
ę ś
0 0 0 0 1
0 0 0
(22)
66
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:
2 2 2
sj kj + Dti 2sV ł
Dti 2sV Vl 0 0 0 0 0 0
j j
ę ś
2 2 2
Dti 2sV Vl s kl + Dti 2sV 0 0 0 0 0 0
ę ś
l
j l
2
ę ś
0 0 sV sV VE 0 0 0 0
N N
ę ś
2
ę 0 0 sV VE sV 0 0 0 0 ś
N N
Qi =
ę ś.
2
0 0 0 0 s s 0 0
aN aaaE
ę ś
(23)
2
ę ś
0 0 0 0 s s 0 0
aaaE aE
ę ś
2
0 0 0 0 0 0 s s
' ' '
ę ś
aN aN aE
2
ę ś
0 0 0 0 0 0 s s
' ' '
aN aE aE
67
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 (jDGPS, lDGPS), składowe
prędkości względem południka i równoleżnika z nawigacji
zliczeniowej (VN, VE), składowe przyspieszenia względem
południka i równoleżnika z przetwornika inercjalnego (aN,
aE). Przy tych założeniach wektor pomiarów będzie wyglądał
następująco:
68
T
z =[jDGPS ,lDGPS ,VN ,VE ,aN ,aE] .
(24)
Macierz pomiarów, podobnie jak i wyżej, będzie macierzą
Jacobiego
śj śj śj śj śj śj śj śj
ł
DGPS DGPS DGPS DGPS DGPS DGPS DGPS DGPS
/
ę
ó
śj śl śVN śVE śaN śaE śaN śaE ś
ę ś
śl śl śl śl śl śl śl
DGPS DGPS DGPS DGPS DGPS DGPS DGPS DGPS
ęśl ś
/
ę
ó
śj śl śVN śVE śaN śaE śaN śaE ś
ę
śVN śVN śVN śVN śVN śVN śVN śVN ś
ę ś
/
ó
śj śl śVN śVE śaN śaE śaN śaE ś.
ę
C =
ę ś
śVE śVE śVE śVE śVE śVE śVE śVE
ę
/
ó (25)
śj śl śVN śVE śaN śaE śaN śaE ś
ę ś
śaN śaN śaN śaN śaN śaN śaN śaN ś
ę
/
ę
ó
śj śl śVN śVE śaN śaE śaN śaE ś
ę
śaE śaE śaE śaE śaE śaE śaE śaE ś
ę ś
/
ó
ę śj śl śVN śVE śaN śaE śaN śaE
ś
69
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.
70
Macierz kowariancji zakłóceń pomiarów (wektora
pomiarów) w tym przypadku będzie wyglądała następująco:
R = .
(26)
71
Ponieważ niektóre wielkości nie są ze sobą skorelowane
podobnie jak i w poprzednim modelu, to macierz ta otrzyma
postać:
R = .
72
Przyjmując konkretne wartości poszczególnych wariancji i
kowariancji możemy macierz tą uprościć do macierzy o
stałych elementach:
.
R =
73
W tym przypadku przyjęto następujące wartości wariancji i
kowariancji poszczególnych pomiarów:
systemu DGPS - sj = 2,0 m, sl = 1,5 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 - sV = 0.1 m/s;
składowych przyspieszeń - sa = 0.01 m/s2.
Ze względu na lepsze uwzględnienie dynamiki statku, model
ten ma istotną przewagę nad modelem pierwszym.
74
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.
75
76
Wyszukiwarka
Podobne podstrony:
5 Analiza systemowa wykłady PDF 11 z numeracją6 Analiza systemowa wykłady PDF 11 z numeracjąanaliza systemowa wyklad2analiza systemowa wyklad3analiza systemowa wyklad4analiza systemowa wyklad1Analiza Wykład 8 (25 11 10)Analiza Wykład 5 (04 11 10) ogarnijtemat comAnaliza Finansowa Wykład 03 04 11 09Analiza Wykład 6 (16 11 10) ogarnijtemat comAnaliza Wykład 7 (18 11 10) ogarnijtemat comwyklad 7 zap i, 11 2013socjo wykład z 26 11wyklad 8 zap i, 11 2013Techniki negocjacji i mediacji w administracji wykłady 05 11 2013wyklad pdfanaliza systemu oceny okresowej pracownikówanaliza finansowa wyklad KONwięcej podobnych podstron