1
W O J S K O W A A K A D E M I A T E C H N I C Z N A im. Jarosława Dąbrowskiego
Zintegrowane Systemy Nawigacyjne
Ćwiczenie laboratoryjne pt.:
Analiza działania systemu INS/GPS
Warszawa, 2018
3 1. Cel ćwiczenia
Celem ćwiczenia jest analiza systemu zintegrowanego INS/GPS. W ramach ćwiczenia należy:
• dokonać analizy działania systemu INS/GPS
• zaimplementować równania filtru Kalmana w oprogramowaniu Matlab,
• dokonać analizy uzyskanych rezultatów wyznaczania położenia.
2. Opis systemu nawigacyjnego
Modelowany zintegrowany system nawigacyjny składa się z systemu nawigacji inercjalnej INS oraz odbiornika GPS (rys.2.1). W systemie tym intergracja danych realizowana jest metodą filtracji pośredniej z korekcją w przód.
Rys.2.1. Model systemu INS/GPS
2.1. Model błędów INS
W prezentowanym systemie modelowane są błędy wyznaczania położenia w płaszczyźnie horyzontalnej, tzn. wzdłuż osi północnej:
+
−
=
+
=
=
E N E
BN E N
N
u v
u v
v N
R 1 g
(1)
oraz osi wschodniej:
+
=
+
−
=
=
N E N
BE N E
E
u v
u v
v E
R 1 g
(2)
gdzie:
N - błąd wyznaczenia położenia przez INS w osi północnej, vN
- błąd wyznaczenia prędkości przez INS w osi północnej,
E - błąd wyznaczenia orientacji kątowej b-frame względem n-frame, wyrażony wokół osi wschodniej,Uncorrected INS errors
GPS errors
Corrected INS errors
Estimated INS errors
Comparison of North errors Comparison of East errors
--- Note:
Doub le-click b locks to change parameters or configure sub systems.
z(1) z(2)
x(1) x(4) Kalman Filter dN
dE INS
[dE_GPS]
[dN_GPS]
[dE_INS]
[dN_INS]
[dE_INSGPS]
[dN_INSGPS]
dN dE GPS
[dE_GPS]
[dE_INS]
[dE_INSGPS]
[dN_GPS]
[dN_INS]
[dN_INSGPS]
4
E - błąd wyznaczenia położenia przez INS w osi wschodniej, vE
- błąd wyznaczenia położenia przez INS w osi wschodniej,N - błąd wyznaczenia orientacji kątowej b-frame względem n-frame, wyrażony wokół osi północnej, g – przyspieszenie ziemskie,
R – promień Ziemi (przy założeniu modelu kulistego, R = 6371 km).
Równania (1) i (2) zaimplementowano w środowisku Simulink (rys. 2.2 – 2.4). Parametrami wejściwymi modelu błędów są widmowe gęstości mocy szumu przyspieszeniomierzy (SBN, SBE), widmowe gęstości mocy szumu giroskopów (SWN, SWE) oraz błędy orientacji osi pomiarowych czujników. Szumy modelowane są jako ograniczone pasmowo szumy białe.
Rys. 2.2. Model błędów INS
Rys. 2.3. Model błędów INS - kanał N
5
Rys. 2.3. Model błędów INS - kanał E
2.2. Model błędów odbiornika GPS
Błędy wyznaczania położenia wzdłuż osi północnej i wschodniej przez odbiornik GPS modelowane są jako szumy o rozkładzie normalnym, z zerową wartością oczekiwaną. W modelu możliwe jest także ustawienie wartości wariancji ww. błędów.
Rys. 2.3. Model błędów odbiornika GPS
2.3. Ciągły model dynamiki systemu
Ciągły model dynamiki systemu dany jest zależnością:
( ) t Fx ( ) t Gu ( ) t
x = +
(3)gdzie:
G
- macierz sterowań stochastycznych,F
- macierz podstawowa (przejścia),( ) t
x
- n-wymiarowy wektor stanu systemu w chwili t,6
( ) t
u
- p-wymiarowy wektor wymuszeń stochastycznych (zakłóceń procesu).Wektor stanu zawiera błędy wyznaczania położenia, prędkości i orientacji kątowej przez system INS, tzn.:
( )
=
N E E
N
v E v
N
t
x
(4)Uwzględniając zależności (1)-(4) otrzymuje się:
+
−
= −
N BE E BN
N E E
N
N E E
N
u u u u
v E v
N
v E v
N
1 0
0 1
0 0
0 0
0 0
0 0
0 0
0 0
0 0
1 0
0 1
0 0
R 0 0 1
g 0 0
0 1 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
R 0 0 1
g 0 0
0 1 0
(5)
Macierz kowariancji zakłóceń procesu:
=
N BE E BN
S 0
0 S 0 0
0 0
0 0
0 0 S
0 0 S
Qc (6)
Ciągły model systemu (3) często wygodnie jest zastąpić modelem dyskretnym, ponieważ dane z wielu urządzeń nawigacyjnych są dostępne jedynie w dyskretnych chwilach czasu, a do ich przetwarzania w systemach zintegrowanych zwykle stosuje się urządzenia mikroprocesorowe, realizujące dyskretne algorytmy filtracji. Dyskretny, liniowy model dynamiki dany jest zależnością:
(
k)
x( ) ( )
k wkx
+ 1 = +
(7)Gdzie
Φ
to macierz tranzycyjna (przejścia) związana z macierzą podstawową poprzez zależność:( ) ( ) ( )
... !
! 2
2
n T T T
e k
n
T F F
F
Φ
=
F=
I+ + + +
(8)Dyskretny model dynamiki systemu INS/GPS przyjmuje postać:
7
( )
( )
( )
( )
( )
( )
( )
( ) ( )
( ) ( ) ( ) ( )
( ) ( )
( ) ( ) ( ) ( )
+
=
+
+
+
+ +
+
k w
k w
k w
k w
k w
k w
k k v
k E
k k v
k N
k
k k v
k E
k k v
k N
N vE
E E vN
N
N E E
N
N E E
N
Φ
1 1 1 1 1 1
(9)
2.4. Model obserwacji
Dyskretny model obserwacji danych jest następującym równaniem:
( )
k H( ) ( ) ( )
k xk vkz
= +
(10)gdzie:
( ) k
H
- macierz obserwacji dyskretnego modelu systemu w chwili kT,( ) k
v
- m-wymiarowy wektor błędów pomiarowych w chwili kT,( ) k
z
- m-wymiarowy wektor pomiarowy w chwili kT.W omawianym systemie INS/GPS wektor pomiarowy z(k) ma postać:
( )
−
−
=
GPS INS
GPS INS
E E
N k N
z (11)
gdzie:
N
INS i E
INS to błędy wyznaczania położenia z modelu sytemu INS, natomiast N
GPS i E
GPS to błędy wyznaczania położenia z modelu odbiornika GPS.Macierz kowariancji błędów pomiarowych
R ( ) k
określana jest dla wariancji danych pochodzących z odbiornika GPS i dana jest zależnością:
=
= 2 2
0 0
E N
E vvT
R (12)
3. Liniowy filtr Kalmana
W przentowanym systemie integracja danych INS i GPS realizowana jest za pomocą filtru Kalmana.
Ponieważ zarówno model dynamiki jak i model obserwacji są liniowe możliwe jest zastosowanie klasycznego, liniowego filtru Kalmana, który w omawianym przypadku jest algorytmem filtracji optymalnej.
Obliczenia realizowane w filtrze przedstawiono na rys. 3.1.
8
Rys. 3.1. Algorytm liniowego filtru Kalmana
Zmienne wykorzystane na rys. 3.1:
I - macierz jednostkowa,
( )
kkxˆ - estymata wektora stanu w chwili kT – wynik filtracji,
( )
kkP - macierz kowariancji błędów filtracji w chwili kT,
(
k 1k)
ˆ +
x - estymata wektora stanu w chwili (k+1)T – wynik predykcji,
(
k 1+
k)
P - macierz kowariancji błędów predykcji w chwili (k+1)T,
( ) k
K
- macierz wzmocnień Kalmana w chwili kT.Kluczowe operacje wykonywane podczas pracy filtru to: inicjalizacja (1), predykcja wektora stanu i macierzy kowariancji błędów (2), wykonywanie pomiaru (3), obliczanie macierzy wzmocnień Kalmana i korekcja wyników predykcji w wykorzystaniem ostatnio wykonanego pomiaru (4).
4. Instrukcja ćwiczenia laboraatoryjnego Zadania do wykonania:
4.1. Zaimplementować równania filtru Kalmana (w funkcji z pliku mdlUpdate() Lab1_KF.m),
4.2. Wyznaczyć wartości elementów macierzy tranzycyjnej przy ograniczeniu rozwinięcia w szereg Fouriera do wyrazu związanego z wielomianem stopnia pierwszego,
4.3. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym i wschodnim). Parametry symulacji:
Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 5𝑚2
𝑠3, PSD giroskopów 1𝑒 − 8𝑟𝑎𝑑2
𝑠 , odchylenie standardowe błędów GPS 10 m,
INS: PSD przyspieszeniomierzy 2𝑒 − 5𝑚2
𝑠3, PSD giroskopów 1𝑒 − 8𝑟𝑎𝑑2
𝑠 , GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,
4.4. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym i wschodnim). Parametry symulacji:
9
Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 6𝑚2
𝑠3, PSD giroskopów 1𝑒 − 9𝑟𝑎𝑑2
𝑠 , odchylenie standardowe błędów GPS 10 m,
INS: PSD przyspieszeniomierzy 2𝑒 − 5𝑚2
𝑠3, PSD giroskopów 1𝑒 − 8𝑟𝑎𝑑2
𝑠 , GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,
4.5. Wyznaczyć wartości elementów macierzy tranzycyjnej przy ograniczeniu rozwinięcia w szereg Fouriera do wyrazu związanego z wielomianem stopnia drugiego,
4.6. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym i wschodnim). Parametry symulacji:
Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 5𝑚2
𝑠3, PSD giroskopów 1𝑒 − 8𝑟𝑎𝑑2
𝑠 , Odchylenie standardowe błędów GPS 10 m,
INS: PSD przyspieszeniomierzy 2𝑒 − 5𝑚𝑠32, PSD giroskopów 1𝑒 − 8𝑟𝑎𝑑𝑠2, GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,