MobilisRobotokNavigacioja PDF

139
MOBILIS ROBOTOK NAVIGCIJA

11. KITERJESZTETT KALMAN-SZR
Ebben a fejezetben bemutatjuk, hogyan lehet llapotot becslni Kalman-szr alkalmazsval diszkrtidej rendszer s sztochasztikus jelek esetn. Elszr bemutatjuk idben vltoz lineris rendszer esetn az n. aktulis Kalman-szr algoritmust, majd a mdszert ltalnostjuk diszkrtidej idben vltoz nemlineris
rendszer esetre a kiterjesztett Kaman-szr (EKF, extended Kalman filter) alakjban. Ez a megolds lesz az alapja a GPS s IMU szenzorokon, valamint beltri
esetben a GPS-t kivlt marker-bzis kpfeldolgozson alapul llapotbecslsnek.
11.1 Aktulis Kalman-szr idben vltoz diszkrtidej

lineris rendszer esetn
Az "aktulis" Kalman-szr az x k llapot becslsnl mr az "aktulis" y k kimenetet veszi figyelembe (s az elz u k 1 bemenetet). Ezltal az u k = K k x k llapot-
visszacsatolsban szmthatv vlik (mert csak az elz u k 1 kell hozz), de mr

figyelembe lesz vve az aktulis y k mrsi eredmny, ami nyilvn jobb irnytsi
tulajdonsgok elrst teszi lehetv.
Tekintsk a zajokkal terhelt diszkrtidej lineris rendszert:
x k +1 = Ak x k + Bk u k + v k ,
x(0)
y k = C k xk + z k
(11.1a)
(11.1b)
Itt u k determinisztikus jel. A v k s z k zajokra s az x(0) kezdeti llapotra a kvetkez sztochasztikus hipotzis legyen rvnyes ( kl a Kronecker-delta):
x(0) fggetlen v k -tl s z k -tl,
(11.2a)
Ex(0) = x 0 , E[( x(0) x 0 )( x(0) x 0 ) T ] = 0 0 (pozitv szemidefinit) (11.2b)

Ev k = 0 , E[v k vlT ] = Rv, k kl , Rv,k 0 (pozitv szemidefinit)
(11.2c)
Ez k = 0 , E[ z k z lT ] = R z , k kl , R z , k > 0 (pozitv definit)
(11.2d)
E[v k z lT ] = 0 , E[ z l v kT ] = 0 ( v k s z l korrellatlanok)
(11.2e)
140
Lantos: MOBILIS ROBOTOK NAVIGCIJA
Keressk azt a lineris szrt, amely az u k s y k +1 mrsi eredmnyekbl az

x k +1 = Fk x k + G k +1 y k +1 + H k u k ,
x (0)
(11.3)
algoritmus alapjn az x k llapot optimlis x k becslst adja abban az rtelemben,

hogy
E[ x k x k ] = 0 minden k esetn,
(11.4a)
E[( x k x k )( x k x k ) ] = k infimum .
(11.4b)
Az infimum az M nsn szimmetrikus mtrixok (kvadratikus alak rvn definilt)

rszben rendezse szerint rtend. Az optimlis megolds a kovarianciamtrix szszes sajtrtkt egyszerre teszi minimliss. A felttelek alapjn teljeslnie kell,
hogy
Ex (0) = x 0 := x 0 E[( x 0 x 0 )( x 0 x 0 ) T ] = 0 .
(11.5)
Kpezzk a becslt llapotot s a fut hibt k + 1 esetn:

x k +1 = Fk x k + G k +1 (C k +1 x k +1 + z k +1 ) + H k u k =
= Fk x k + G k +1{C k +1 ( Ak x k + Bk u k + v k ) + z k +1 } + H k u k =
= Fk x k + G k +1C k +1 Ak x k + (G k +1C k +1 B k + H k )u k + G k +1C k +1 v k + G k +1 z k +1
x k +1 = Ak x k + Bk u k + v k
x k +1 x k +1 = ( I G k +1C k +1 ) Ak x k + [( I G k +1C k +1 ) Bk H k ]u k
Fk x k + ( I G k +1C k +1 )v k G k +1 z k +1 .
(11.6)
A hiba vrhat rtkre vonatkoz (11.4a) felttel szerint

Fk := ( I G k +1C k +1 ) Ak ,
H k := ( I G k +1C k +1 ) B k ,
(11.7)
ezrt
~
x k +1 := x k +1 x k +1 =
= ( I G k +1C k +1 ) Ak ( x k x k ) + ( I G k +1C k +1 )v k G k +1 z k +1 =
= ( I G k +1C k +1 ){ Ak x k + B k u k + v k Ak x k B k u k } G k +1 z k +1
Vezessk be az
(11.8)
x k +1 := Ak x k + Bk u k
M k +1 := E[( x k +1 x k +1 )( x k +1 x k +1 ) T ]
141
(11.9)
jellst, akkor
k +1 = ( I G k +1C k +1 ) M k +1 ( I G k +1C k +1 ) T + G k +1 R z ,k +1G kT+1 .
(11.10)
Mivel k +1 -et infimumm kell tenni, ezrt k +1 -nek a G k +1 szerinti derivltjnak a O() nulla transzformcit kell adnia. Kpezzk ezrt a G k +1 szerinti differencilt:
dG k +1C k +1 M k +1 ( I G k +1C k +1 ) T ( I G k +1C k +1 ) M k +1C kT+1 (dG k +1 ) T +
+ dG k +1 R z ,k +1G kT+1 + G k +1 R z ,k +1 (dG k +1 ) T = O(dG k +1 ).
(11.11)
Vezessk be az U () s T () transzformcikat:
U ( X ) := X + X T ,
T ( X ) := X T .
(11.12)
A differencilt trhatjuk az
U {[( I G k +1C k +1 ) M k +1C kT+1 + G k +1 R z , k +1 ]T (dG k +1 )} = O(dG k +1 )
(11.13)
alakba, ahonnan kvetkezik az infimum albbi felttele opertor alakban:

U {[( I G k +1C k +1 ) M k +1C kT+1 + G k +1 R z , k +1 ]T ()} = O() .
(11.14)
Tetszleges X esetn teljesl, hogy

U { XT ()} = O() U { XT (Y )} = 0 minden Y esetn X = 0 .
(11.15)
Az infimum felttele ezrt

0 = ( I G k +1C k +1 ) M k +1C kT+1 + G k +1 R z ,k +1 =
= M k +1C kT+1 + G k +1C k +1 M k +1C kT+1 + G k +1 R z , k +1 =
= M k +1C kT+1 + G k +1 (C k +1 M k +1C kT+1 + R z ,k +1 ),
ahonnan kvetkezik, hogy az optimlis megolds
(11.16)
142

G k +1 = M k +1C kT+1 (C k +1 M k +1C kT+1 + R z ,k +1 ) 1 .
(11.17)
Helyettestsk vissza az optimlis G k +1 megoldst a (11.83) egyenletbe:
k +1 = ( I G k +1C k +1 ) M k +1 ( I G k +1C k +1 ) M k +1C kT+1G kT+1 + G k +1 R z ,k +1G kT+1 =

= ( I G k +1C k +1 ) M k +1 + { M k +1C kT+1 + G k +1 (C k +1 M k +1C kT+1 + R z , k +1 )}G kT+1 =
1444444442444444443
0
= ( I G k +1C k +1 ) M k +1 = M k +1 G k +1C k +1 M k +1
A becslsi hiba optimlis kovariancia mtrixa ezrt
k +1 = M k +1 M k +1C kT+1 (C k +1 M k +1C kT+1 + R z , k +1 ) 1 C k +1 M k +1 .
(11.18)
Bontsuk fel a becslt llapot kpzst a mrsi idpontok kztt elvgezhet

x k +1 frisstsre s az j mrs y k +1 pillanatban elvgezhet x k +1 llapotbecslsre,
akkor
x k +1
x k +1 = Ak x k + B k u k ,
x k +1 = Ak ( x k x k ) + v k ,
M k +1 =
Ak k AkT
(11.19)
+ Rv ,k ,
ahol M k +1 szintn meghatrozhat a mrsi idpontok kztt. Msrszt a becslt

llapot felrhat
x k = Fk 1 x k 1 + G k y k + H k 1u k 1 =
= ( I G k C k ) Ak 1 x k 1 + G k y k + ( I G k C k ) Bk 1u k 1 =
= Ak 1 x k 1 + B k 1u k 1 + G k { y k C k ( Ak 1 x k 1 + B k 1u k 1 )} =
(11.20)
= xk + Gk ( y k C k xk )
alakban is. Vgl felhasznlvn a (11.17) egyenletet G k tovbb egyszersthat:
G k = M k C kT (C k M k C kT + R z ,k ) 1
M k C kT = G k (C k M k C kT + R z ,k ) = G k C k M k C kT + G k R z , k
G k R z ,k = ( I G k C k ) M k C kT = k C kT
G k = k C kT R z,1k .
(11.21)
143
Az "aktulis" Kalman-szr algoritmusa:

1.
Frissts mrsi idpontok kztt (time update between measurements):

x k = Ak 1 x k 1 + B k 1u k 1 ,
M k = Ak 1 k 1 AkT1 + Rv ,k 1 ,
k = M k M k C kT (C k M k C kT + R z ,k ) 1 C k M k ,
(11.22)
G k = M k C kT (C k M k C kT + R z ,k ) 1 = k C kT R z,1k .
2.
Mrsi eredmny frisstse (measurement update):

x k = x k + G k ( y k C k x k )
(11.23)
Megjegyzs:
1. Klnleges eset, ha nincs kimeneti zaj, mert ekkor R z , k = 0 s G k szmtsra
(11.21) nem hasznlhat. Ekkor (11.17) hasznland.
2. Vegyk szre, hogy a (11.22)-(11.23) egyenletek elretart rekurzv egyenletek,
ezrt a Kalman szrsi algoritmus online realizlhat. A Kalman szrsi algoritmus idben vltoz rendszer esetn a kvetkez smval brzolhat:
Inicializls: x 0 := x 0 = Ex(0), 0 := E[( x(0) x 0 )( x(0) x 0 ) T ]
Elretart rekurzi k = 1,2, K :
y k , (11.23)
11.22 )
x k 1 (
M k , k , G k , x k
x k
Megjegyzs: Ha az llapotegyenletben a rendszerzaj Bv ,k v k , a mrsi zaj pedig

C z , k z k alakban szerepel, akkor elegend elvgezni az Rv ,k := Bv.k Rv , k BvT, k s
R z , k := C z ,k R z ,k C zT,k helyettestseket, mert pldul

E[( Bv, k v k )( Bv,k v k ) T ] = Bv,k E[v k v kT ]BvT,k = Bv, k Rv , k BvT,k
A Kalmn-szr algoritmusa knnyen ltalnosthat korrellt rendszerzaj s
mrsi zaj esetre is, ebben a tekintetben az irodalomra utalunk (lsd pl. Lantos: Irnytsi rendszerek elmlete s tervezse II. Akadmiai Kiad, 2003).
144
11.2 Kiterjesztett Kalman-szr (EKF)

Tekintsk a kvetkez nemlineris rendszert diszkrt idben:
x k +1 = f ( x k , u k , v k ) ,
(11.24a)
y k = g (xk , z k ) .
(11.24b)
Teljesljenek a lineris eset sztochasztikus hipotzisei, ezen bell legyen az egyszersg kedvrt a rendszerzaj s a mrsi zaj korrellatlan.
Tegyk fel, hogy ismerjk a rendszer llapotnak x k 1 becslst a [(k 1)T , kT ]
idintervallum bal oldali vgpontjban, s keressk az llapot x k becslst a jobb
oldali vgpontban. Ha a rendszer sima, akkor prblkozhatunk a nemlineris modell
linearizlsval az ismert x k 1 s v k 1 = 0 esetn az llapot, valamint az x k 1 s
u k 1 rtkbl kiszmthat x k s z k = 0 esetn a kimenet szmra:
f ( x k 1 , u k 1 ,0)
f ( x k 1 , u k 1 ,0)
v k 1 (11.25a)
( x k x k 1 ) +
x
v
g ( x k ,0)
g ( x k ,0)
y k g ( x k ,0 ) +
( xk xk ) +
zk .
(11.25b)
x
z
x k f ( x k 1 , u k 1 ,0) +
Vegyk szre, hogy

f ( x k 1 , u k 1 ,0)
f ( x k 1 , u k 1 ,0)
x k 1
x
(11.26a)
g ( x k ,0)
xk
x
(11.26b)
s
g ( x k ,0)
ismert jeleknek tekinthetk.

Vezessk be a kvetkez jellseket:
f ( x k 1 , u k 1 ,0)
x
f ( x k 1 , u k 1 ,0)
Bv ,k 1 :=
v
g ( x k ,0)
C k :=
x
g ( x k ,0)
C z ,k :=
z
Ak 1 :=
(11.27a)
(11.27b)
(11.27c)
(11.27d)
12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK
145
Rv ,k 1 := Bv ,k 1 Rv, k 1 BvT,k 1
(11.27e)
R z , k := C z ,k R z ,k C zT,k
(11.27f)
Foglaljuk ssze az eddigieket. Tekintsk a nemlineris rendszert a [(k 1)T , kT ]

intervallumban, amelynek balodali vgpontjban mr ismert az llapot x k 1 becslse. Linearizljuk a nemlineris rendszert az x k 1 , v k 1 = 0 esetn az llapot, illetve
az x k , z k = 0 esetn a kimenet szmra. A linearizlt rendszerre alkalmazzuk a lineris rendszerekre kidolgozott aktulis Kalman szr algoritmust.
Az gy kapott llapotbecsl az .n. kiterjesztett Kalman-szr (EKF, extended
Kalman filter):
x k = f ( x k 1 , u k 1 ,0)
A nemlineris rendszer linearizlsa (11.27a-f) alapjn.
M k = Ak 1 k 1 AkT1 + Rv ,k 1 ,
k = M k M k C kT (C k M k C kT + R z ,k ) 1 C k M k ,
(11.28)
G k = M k C kT (C k M k C kT + R z ,k ) 1 = k C kT R z,1k .
x k = x k + G k ( y k g ( x k ,0))
(11.29)
Megjegyzs: A kiterjesztett Kalman-szr algoritmusban az f () s g () nemlineris fggvnyekhez gy jutottunk, hogy a Taylor sorok helyett a nemlineris fggvnyek rtkt rtuk vissza, s figyelembe vettk Ak 1 s C k defincijt:
f ( x k 1 , u k 1 ,0)
x k 1 f ( x k 1 , u k 1 ,0)
x
g ( x k ,0)
x k = x k + G k ( y k g ( x k ,0) +
x k C k x k )) x k + G k ( y k g ( x k ,0))
x
x k = Ak 1 x k 1 + f ( x k 1 , u k 1 ,0)
146

Ebben a fejezetben a navigcinl hasznlt fontosabb koordintarendszerekkel
(ECI, ECEF, NED, ABC=BODY) ismerkednk meg. Ezek a koordinta-rendszerek
a 3D navigci alapjt kpezik. A koordinta-rendszereket a 12.1. brn foglaltuk
ssze.
12.1. bra. A navigcinl hasznlt koordinta-rendszerek
12.1 Az ECI inercia rendszer

A K ECI (Earth Centered Inertia) koordinta-rendszer a bolygkzi mozgsok tervezsekor fontos. gy keletkezik, hogy egy megvlasztott fix idpontban tekintjk a
nap krl kering s a tengelye krl forg Fld aktulis kzppontjt, amely az
ECI origja lesz. A tovbbiakban egy tvoli fix csillaghoz kpest kpezzk az orientcit. Az ECI rendszer azrt fontos, mert a klasszikus mechanika alaptrvnyei, gy
a NewtonEuler-egyenletek is inerciarendszerben rvnyesek.
147
12.2 Az ECEF koordinta-rendszer

A K ECEF (Earth Centered Earth Fixed) koordinta-rendszer origja a Fld kzppontja, az ECEF koordinta-rendszer egytt mozog a Flddel (kering a Nap krl s
forog a tengelye krl), az ECEF z -tengelye a Fld forgstengelye, amely merleges az egyenlt skjra. Az ECEF x -tengelye a Greenwichi Obszervatriumon keresztlhalad fmeridin (nulladik hosszsgi kr) skjnak s az egyenlt skjnak
metszsvonala, az y -tengely jobb sodrs koordinta-rendszerre egszt ki.
Az ECEF szgsebessge az ECI inerciarendszerhez kpest
ie =
(1 + 365.25) cycles 2 rad / cycle
= 7.2921151467 10 5 rad / s .
365.25 24 h
3600 s / h
(12.1)
A Fld forgsellipszoiddal approximlhat a GPS alapjt kpez WGS-84 szabvny szerint. Az ellipszis fltengelyei:
a = 6388137.0 m ,
b = 6356752.3142 m ,
( a > b)
(12.2)
A forgsellipszoid egy ellipszisnek a z -tengely krli forgatsval keletkezik. Az

ellipszis f simasga s e excentricitsa a kvetkezskpp van definilva:
ab
= 0.0034, e = f (2 f ) = 0.0818
a
b2
ab
a b (a b)(a + b) a 2 b 2
b2
2
2
e2 =
=
=
e
=
1
1
=
a
a
a2
a2
a2
a2
f =
(12.3)
(12.4)
Az ECEF koordinta-rendszerben egy P pont az origbl a pontba mutat

r = ( x, y, z ) T vektorral jellemezhet. A pont megadsa ECEF-ben azonban a geodetikus ( , , h) T koordintkkal trtnik, ahol , , h angol neve rendre latitude,
longitude s altitude, ezekbl kell az ( x, y, z ) T vektor komponenseit meghatrozni.
Ez a kvetkezkppen trtnhet.
A P ponton s a z -tengelyen tfektetnk egy skot, s meghatrozzuk eme
sknak a metszett a forgsellipszoiddal. Ez a forgatott ellipszis egy pldnya, a
keletkez ellipszis tengelyeit jellje z s y . A sk kimetsz az egyenlt skjbl
egy y 0 hosszsg szakaszt, amelynek a vetlete az ellipszoid x, y tengelyeire az
egyenlt skjban az x -tengellyel bezrt szg ismeretben szmthat, nevezetesen x = y 0 C s y = y 0 S . A P pont tvolsga az ellipszis y tengelytl kzvetlenl a P pont z koordintja. A h magassg a P pont legrvidebb tvolsga
az ellipszistl, amely az ellipszisen egy Q = ( z 0 , y 0 ) pontot definil. A Q pontban
148
tekinthetjk az ellipszis rintjt s az arra merleges egyenest, amely utbbi a z tengelyen egy R = (0,c) pontot metsz ki. A QR egyenes az rint normlisa,
hossza N . Az rintnek Q -tl az y tengellyel val metszspontig terjed hossza
N . Az rint normlisa s az y tengely ltal bezrt szg . A viszonyokat a 12.2.
bra mutatja be.
12.2. bra. A trbeli koordintk szmtsnak elve ECEF keret esetn
Az brnak megfelelen a kvetkez sszefggsek rvnyesek:

Ellipszis egyenlete:
Derivlt:
z2
b2
y2
a2
=1 z = b 1
z = b
rint meredeksge:
Normlis egyenlete:
m=
z = z0
a2
2y
2
a
a2
y0
b
a
y2
y2
y 02
a2
1
( y y0 )
m
149
Normlis egyenesnek metszete a z -tengellyel (ahol y = 0 ):
c = z0 +
a2
b
y 02
a2
( y 0 ) z 0 + c =
y0
y2 a2
a2
1 02 = 2 z 0
b
a
b
Hasonl hromszgekbl:
N z0 + c a 2
b2
=
= 2 N = 2 N
z0
N
b
a
b2
z = ( N + h) S = 2 N + h S
Geometribl:
z = N (1 e 2 ) + h S
(12.5)
y 0 = ( N + h)C
Geometribl:
x = ( N + h)C C
(12.6)
y = ( N + h)C S
Meg kvnjuk mg hatrozni az N ( ) fggvnyt, amelyhez nhny azonossgot

s az eddigi eredmnyeket fogjuk felhasznlni.
z 02
Azonossgok:
z0 + c =
y 02 +
a2
b2
a2 b4
b2 a4
b2
z 0 z 02 =
y 02
a2
b4
a4
= 1 y 02 +
a2 b4
2
b2
z 02 = a 2
( z 0 + c) 2
( z 0 + c) 2 = a 2 y 02 +
z 0 + c = NS , y 02 +
a2
b2
a2
( z 0 + c) 2 = a 2
( z 0 + c) 2 = a 2 y 0 = NC ,
b a
N C + (1 e ) N S 2 = N 2 (1 e 2 ) S 2 = a 2
2
b2
a
= 1 e2
N ( ) =
a
1 e 2 S 2
(12.7)
150
A (12.5-7) eredmnyeket a kvetkez algoritmusban foglaljuk ssze.

Konverzi geodetikus koordintkrl derkszg koordintkra:
( , , h ) T
N ( ) =
1 e 2 S 2
x = ( N + h )C C
y = ( N + h )C S
(12.8)
z = N (1 e 2 ) + h S
A GPS informcibl a ksbb megismert mdon derkszg koordintkat lehet kiszmtani, a konverzi geodetikus koordintkra egy rekurzv algoritmussal
vgezhet el a fenti sszefggsek felhasznlsval.
Konverzi derkszg koordintkrl geodetikus koordintkra:
( x , y , z ) T ( , , h ) T
Inicializls:
h := 0, N := a, p := x 2 + y 2 p := ( N + h)C , T = y / x atan2

(12.9a)
Ciklus:
S :=
z
N (1 e 2 ) + h
, T :=
z + e 2 NS ( N + h) S
S
=
= atan

p
( N + h)C C
(12.9b)
p
N
N ( ) := a / 1 e S , h =
C
A tapasztalat azt mutatja, hogy gyorsabb a konvergencia, ha rtkt nem asin,
2
hanem atan fggvnnyel szmtjuk := atan( z + e 2 NS , p ) alapjn.

Az ECEF koordinta-rendszer alapvet fontossg a fld krl kering mestersges
holdak (szatellitek) plyjnak s a hordoz rakta irnytsnak tervezsekor is. A
rakta hajtm ltal kifejtett er/nyomatk mellett a tmegvonzs rrvnye szerint
F g = G
Mm r
r
= GMm 3
2
r
r
r
(12.10)
er is hat a raktra, ahol

GM = 3.98599927 1014 m 3 / s 2 ,
a nehzsgi gyorsuls vektora pedig
(12.11)

g=
GM
r
151
(12.12)
fgg a rakta tmegkzppontjba mutat r vektortl.

A konverzit geodetikus koordintk s ECEF, valamint ECEF s geodetikus koordintk kztt Los Angeles pldjn az albbi MATLAB script program mutatja be.
%frame_LosAngeles
%ECEF position of Los Angeles
clear all
close all
clc
%Geodetic data of Los Angeles
lambda_deg=34+0/60+0.00174/3600;
phi_deg=-(117+20/60+0.84965/3600);
h=251.702;
lambda=lambda_deg*pi/180;
phi=phi_deg*pi/180;
a=6378137;
b=6356752.3;
f=(a-b)/a;
e=sqrt(f*(2-f));
%Direct problem
format long
fprintf('Direct problem: lambda_deg,phi_deg,h -> x,y,z ECEF\n');
lambda_deg
phi_deg
h
N=a/sqrt(1-e^2*sin(lambda)^2)
x=(N+h)*cos(lambda)*cos(phi)
y=(N+h)*cos(lambda)*sin(phi)
z=(N*(1-e^2)+h)*sin(lambda)
format short
%Inverz problem
format long
%Init
h=0;
N=a;
p=sqrt(x^2+y^2);
phi=atan2(y,x);
phi_deg=phi*180/pi;
%Iter
niter=9;
data_iter=NaN*ones(niter,3);
for i=1:niter
S_lambda=z/(N*(1-e^2)+h);
lambda=atan2(z+e^2*N*S_lambda,p);
152
lambda_deg=lambda*180/pi;
N=a/sqrt(1-e^2*S_lambda^2);
h=p/cos(lambda)-N;
data_iter(i,:)=[lambda_deg N h];
end;
%Geodetic data of Los Angeles
fprintf('Iteracios lepesek eredmenyei: lambda_deg,N,h sorrend\n');
data_iter
fprintf('Inverz problem: x,y,z ECEF -> lambda_deg,phi_deg,h\n')
x
y
z
lambda_deg=data_iter(niter,1);
N=data_iter(niter,2);
h=data_iter(niter,3);
N
lambda_deg
phi_deg
h
format short
A futsi eredmnyek a direkt feladat esetn:

Direct problem: lambda_deg,phi_deg,h -> x,y,z ECEF
lambda_deg = 34.00000048333333
phi_deg =
-1.173335693472222e+002
h =
2.517020000000000e+002
N =
6.384823214436118e+006
x =
-2.430601829367448e+006
y =
-4.702442706380628e+006
z =
3.546587344780241e+006
Futsi eredmnyek az inverz feladat esetn:

Iteracios lepesek eredmenyei: lambda_deg,N,h sorrend
data_iter =
1.0e+006 *
0.00003400000754
6.38483779492169
0.00003400000088
6.38482321352208
0.00003400000048
6.38482321437358
0.00003400000048
6.38482321443625
0.00003400000048
6.38482321443612
0.00003400000048
6.38482321443612
0.00003400000048
6.38482321443612
0.00003400000048
6.38482321443612
0.00003400000048
6.38482321443612
0.00023765195628
0.00025173252342
0.00025170199822
0.00025170199987
0.00025170200000
0.00025170200000
0.00025170200000
0.00025170200000
0.00025170200000
Inverz problem: x,y,z ECEF -> lambda_deg,phi_deg,h

x =
y =
-2.430601829367448e+006
-4.702442706380628e+006
13. A Globlis Pozcionl Rendszer (GPS)

z =
3.546587344780241e+006
N =
6.384823214436118e+006
lambda_deg = 34.00000048333332
phi_deg =
-1.173335693472223e+002
h =
2.517020000005141e+002
153
12.3 A NED koordinta-rendszer

A K NED (North-East-Down) koordinta-rendszert gy kapjuk meg, hogy meghatrozzuk a P ponttl a forgsellipszoidig a legkzelebbi Q pontot, amely a NED koordinta-rendszer origja lesz. A Q pontban meghatrozzuk a forgsellipszoid rintskjt, s abban az x NED tengely szak fel, az y NED tengely kelet fel s a z NED
tengely az rintsk normlisa irnyban lefel, a forgsellipszoid belseje fel mutat.
Az alkalmazsokban K NED -et egy adott pillanatban lergztjk, s a tovbbiakban ehhez kpest rtelmezzk a mozg jrmhez rgztett ABC (Aircraft-Body
Coordinate System, vagy ltalban Body Coordinate System) mozgst. A robot koordinta-rendszere a K ABC = K B koordinta-rendszer, amelynek origja tipikusan a
robot tmegkzppontjban van, a tengelyek irnytsa az alkalmazs fggvnyben
megvlaszthat.
A K NED s a K ABC = K B kztti kapcsolatot a robot ( x NED , y NED ,h) T pozcija s az Euler-szgekkel jellemzett ( , , ) T orientcija azonostja, ahol , ,
rendre a roll, pitch, yaw szgek s AK NED , K ABC = Rot ( z , ) Rot ( y, ) Rot ( x, ) . Ez a
koordintarendszer a rvid idtartam irnytsok vizsglatnl alapvet fontossg.
154

A GPS (Global Positioning System) egy a Fld krnyezetben tetszleges idjrsviszonyok kztt alkalmazhat helyzetmeghatroz rendszer, amely megklnbztetett (katonai) felhasznlk szmra pontosabb, ltalnos polgrai felhasznlk
szmra kevsb pontos pozici informcit kpes szolgltatni. A rendszert az USA
hozta ltre, teljes szolgltatsa 1993 ta hozzfrhet.
13.1 A GPS rendszer ttekintse

A GPS rendszer 3 szegmenstpust tartalmaz: r-szegmenst (space), felgyel
szegmenst (control)s felhasznli szegmenst (user).
r-szegmens:
Az r-szegmens 24 szatellit jrmbl (SV, satellit vehicle) ll. A szatellitek 6 fldkrli plyn keringenek, egy plyn 4 szatellit. A keringsi id kb. 12 h (11 h 58
min), a plya kzel kralak, sugara 20200 km.A plyk az egyenlt skjval 55 o os szget zrnak be. A hat plya az egyenlt kerlete mentn 60 o -ra van egymstl, egyenletesen elosztva. Ez biztostja, hogy a fldi felhasznl mindig legalbb 4
szatellitre rlsson. Az SV rdijeleket sugroz, amelyet a felhasznl GPS vevje
vesz s dekdol.
Felgyel szegmens:
A felgyel szegmens tartalmaz 4 fldi antennt (Cape Canaveral, Ascension Island, Diego Garcia, Kwajalein), 1 monitor llomst (Falcon Air Force Base Colorado Springs) s 5 tvoli monitor llomst (Hawaii, Ascension Island, Diego Garcia,
Kwajalein, Cape Canaveral), 1 master felgyel llomst (Falcon Air Force Base
Colorado Springs) s 1 backup master felgyel llomst (Gaithersburg). A fldi
monitor llomsok mrik az SV-tl a master felgyel lloms szmra sugrzott informcit s tovbbtjk a master felgyel llomsnak, amely meghatrozza a keringsi modellt s az rajel-korrekcit a szatellit szmra, s tovbbtja azt a fldi
antennknak, amelyek tovbbsugrozzk azt az SV-knek.
Felhasznl szegmens:
A felhasznli szegmens a vevk antennibl s processzoraibl ll, amelyek mrik
s dekdoljk a szatellitek ltal sugrzott informcit. A felhasznlk szma korltozatlan, mivel az zenetforgalom egyirny. A GPS rendszer n. line-of-sight
(LOS) rendszer, csak azon szatellitek adata vehet, amelyekre a vev akadlytalanul
rlt.
155
Minden GPS szatellit tvolsgkdot s navigcis adatot sugroz code-division

multiple access (CDMA) technikval 2 hordozfrekvencin: L1 (1575.42 MHz) s
L2 (1227.60 MHz). A hordoz frekvencik szrt-sprektum jelekkel vannak
modullva, amelyek a vevk szmra sugrzott informcit tartalmazzk. Minden
szatellit 3 pszeudo-random (PRS) tvolsgadatot kld.
A C/A kd modullja az L1 hordoz fzist. Ennek a kdnak a hossza 1024
chips (chip=bit, de nem hordoz informcit). Minden szatellit szmra van egy ettl
klnbz s erre kvzi-ortogonlis C/A PRN kd. Br a szatellitek ugyanazon a
kt frekvencin sugroznak, a vevk kpesek rkapcsoldni egy meghatrozott szatellitre s klnbsget tenni a szatellitek kztt azltal, hogy korrellltatjk a bell
generlt verzijt a szatellitre jellemz C/A kdnak a vett jellel. Mivel a szatellitek
C/A kdjai kvzi-ortogonlisak, a szatellitek kztti thalls a vteli oldalon kicsi.
A GPS SV azonostsa gyakran az SV PRN kdja alapjn trtnik.
Az n. precz (P) kd modullja mind az L1, mind pedig az L2 hordoz fzist.
A P kd egy nagyon hossz (7 nap) 10.23 MHz PRN kd. Az n. antispoofing (AS)
md zemmdban a P kd t van kdolva Y kdd, minden vteli csatornra egy
szmra megklnbztetett AS modult ignyel, kriptogrfiai kulccsal vdett, s
ezrt a norml vev szmra nem hasznosthat.
A navigcis zenet szintn modullja az L1 C/A kdjelet. A navigcis zenet
egy 50 bit/s jel, amelyet a GPS vev dekdol plya, rajel korrekci s ms rendszer-paramterekk. Ezek az n. ephemeris parameterek hasznlhatk a szatellit pozci, az rajel korrekci s az adott idben rvnyes atmoszfrikus korrekci kiszmtsra a vevben.
A GPS kt szint szolgltatst biztost: standard-pozionlsi szolgltatst (SPS,
standard-positioning service) s precz pozcionlis szolgltatst (PPS, precisepositioning service).
Az SPS pozcionlsi s temezsi szolgltats olyan C/A kdon alapul, amely
hozzfrhet minden GPS felhasznl szmra. Ez a szolgltats az L1 frekvencit
hasznlja, amely tartalmaza a C/A kdot s egy navigcis-adat zenetet, vrhat
pontossga ~ 100 m horizontlisan, 156 m (95%) vertiklisan s az id tekintetben 340 ns (95%) . Differencilis GPS (DGPS) technikk jelentsen javthatnak a
pontossgon.
A PPS sokkal pontosabb pozci, sebessg s id szolgltatst biztost, de csak
az US kormny ltal elfogadott autorizlt felhasznlk vevi szmra hozzfrhet.
A hozzfrst AS s szelektv hozzfrses (SA, selective availability) technikval
biztostja. Az AS implementci a P kdot helyettesti a megklnbztetett Y kddal. Az SA implementci clszer mdon degradlja a szatellit rajelet s az
ephemeris adatokat nemautorizlt felhasznlk szmra. Autorizlt felhasznl vevje rendelkezik az Y kdhoz val hozzfrs kpessgvel, s ezrt el tudja tvoltani az SA effektust. A PPS vrhat pontossga 22 m horizontlisan, 27.7 m
(95%) vertiklisan s az id tekintetben 200 ns (95%) . A teljes PPS szolgltats
1995 tavasza ta ltezik.
156
13.2 A GPS matematikai alapjai

A GPS a berkezsi idbl szmtott tvolsgmrsen (time-of-arrival ranging) alapul. A GPS vev tartalmaz egy bels rt, amelyet a szatellit tvolsg-kdjnak
(C/A, P vagy Y az zemmmdtl fggen) berkezsi idejnek mrsre hasznl
fel. Ebbl az informcibl a GPS vev meghatrozza a jelterjedsi idt a szatellit
adjtl a felhasznl vevjig. Mivel a jel c = 2.99792458 10 8 m / s fnysebessggel terjed, ezrt a tvolsg ennek az idnek s a fnysebessgnek a szorzata. Ez
az rtk egy offszettel (bias) rendelkezik, amelynek oka a szatellit adjnak rajele
s a vev rajele kztti klnbsg, ezrt a szoksos elnevezs pszeudo-tvolsg
(pseudorange). Az 1 ms idre vonatkoztatott idztsi pontatlansg tvolsgra tszmtva ~ 300 km , ezrt az id bias nagy pontossggal kompenzland.
Ha a szatellit helyzete ismert s az rajel hiba elhanyagolhat lenne, akkor a
mrt tvolsg a szatellit s a vev kztt egy gmbt definil, kt ilyen gmb egy
krben metszi egymst, s egy harmadik gmb kt pontot hatroz meg, amelyek kzl a vteli hely a priori ismeretben vagy egy negyedik szatellit tvolsg informcijnak bevonsval a vev pozcija meghatrozhat. Ez a geometriai elv azzal
ekvivalens a matematika nzpontjbl, hogy hrom fggetlen egyenlet kell hrom
ismeretlen meghatrozshoz. A hrom ismeretlen a vev antennjnak 3D pozcija. Ha a vev rajelnek hibjt is tekintjk, akkor a szimultn tvolsgmrsi
eredmnyek ugyanazzal a bias rtkkel vannak eltolva. Ezrt minden szimultn tvolsgmrsi eredmny-egyttes 4 ismeretlent tartalmaz, ezrt legalbb 4 tvolsgmrsi eredmny kell az egyrtelm megolds meghatrozshoz.
A felgyel (control) szegmens monitorozza minden egyes szatellit rajelt s
meghatrozza az offszetet, a drift sebessgt s gyorsulst a GPS idhz kpest.
Ezeket az informcikat peridikusan ttlti a szatellitekre, amely a navigcis informci rsze. A GPS vev felhasznlja a szatellit rajel offszet rtkt a mrt
pszeudo-tvolsgok korrekcijra. Az ezzel kapcsolatos kpleteket ksbb az
ephemeris adatok feldolgozsnl trgyaljuk.
Megjegyezzk mg, hogy amg a szatellitek nagy pontossg atomrkat hasznlnak az idmrsre, addig a GPS vev rajele csak kvarc-kristly pontossg.
Legyen a vev s a szatellit id eltrse t r . Akkor a standard GPS pozcionl
rendszer ngy ismretlen mennyisget tartalmaz, ezek a vev ( x, y, z ) T koordinti
az ECEF koordinta-rendszerben s ct r . Feltesszk, hogy ezek meghatrozshoz
rendelkezsre ll a ngy szatellit ~ pszeudo-tvolsga s ( X (i ) , Y (i ) , Z (i ) ) T
i
koordintja az ECEF rendszerben, i = 1,2,3,4 . A tvolsginformci hibjt a vev

(i )
t r rajel bias, az SV szatellit jrm t SV
rajel bias, a t a(i ) atmoszfrikus kss,
a szatellit jelek korrupcijnak SA (i ) hatsa a szelektv elrhetsgi filozfia kvet-
157
keztben, az ephemeris adatok E (i ) hibja, a tbbirny jelterjeds MP (i ) n.

multiplya (multipath) hibja s a vev kvets (i ) zaja okozza, egyttes hatsuk
(i)
c t r + c t SV
+ c t a( i ) + SA
(i)
+ E
(i)
+ MP
(i)
(i)
(13.1)
A ngy szatellittl szrmaz pszeudo-tvolsg a kvetkez egyenleteket kell,

hogy kielgtse:
~ (i ) = ( X (i ) x) 2 + (Y (i ) y ) 2 + ( Z (i ) z ) 2 + ct r
(i )
+ct SV
+ ct a(i ) + SA (i ) + E (i ) + MP (i ) + (i ) , i = 1,2,3,4
(13.2)
Az egyenletekben elhanyagolhat nagysgrend mrsi hibnak tekintjk a

(i )
(i ) := ct SV
+ ct a(i ) + SA (i ) + E (i ) + MP (i ) + (i ) = (i ) + (i ) , i = 1,2,3,4
(13.3)
mennyisgeket, a megmarad nemlineris egyenleteket pedig az ismeretlen vltozk

szerint linearizljuk a vltozk elz becslse krl s a kapott egyenleteket LS
technikval megoldjuk. Kpezzk a derivltakat az utols becsls helyn:
~ (i )
=
x
( X (i ) x 0 ) 2
~ (i )
=
y
( X (i ) x 0 ) 2
~ (i )
=
z
( X (i ) x 0 ) 2
~ (i )
=1
(ct r )
( X (i ) x 0 )
+ (Y (i ) y 0 ) 2 + ( Z (i ) z 0 ) 2
(Y (i ) y 0 )
+ (Y (i ) y 0 ) 2 + ( Z (i ) z 0 ) 2
( Z (i ) z 0 )
(13.4)
+ (Y (i ) y 0 ) 2 + ( Z (i ) z 0 ) 2
Jellje x := ( x, y, z , ct r ) T az ismeretleneket, s legyen az ismeretlenek becslt

rtke x 0 := ( x 0 , y 0 , z 0 , ct r 0 ) T az iterci megelz lpsben, tovbb vezessk
be a kvetkez jellseket:
158

~ (1)
x
~ ( 2)
x
H = ~ ( 3)

x
~ ( 4)

x
~ (1)
y
~
( 2)
y
~ (3)
y
~ ( 4)
y
~ (1)
z
~
( 2)
z
~ (3)
z
~ ( 4)
z
1
( x0 , y 0 , z 0 ) T ,
x x0
y y0
x =
,
z z0
ct ct
r
r0
(13.5)
~ (1)
~ ( 2)
~
= ~ ( 3)
~ ( 4)
~ (1)
( x 0 , y 0 , z 0 , ct r 0 )
~ ( 2) ( x 0 , y 0 , z 0 , ct r 0 )
~ (3) ( x 0 , y 0 , z 0 , ct r 0 )
~ ( 4) ( x 0 , y 0 , z 0 , ct r 0 )
Akkor a megoldand egyenletrendszer s annak LS (least squares) megoldsa:

~ = Hx x = (H T H ) 1 H T
~ x = x + x
(13.6)
Az itercit addig kell folytatni, amg a becslt rtkek nem stabilizldnak.

Az elhanyagolt (i ) mennyisgek ltal okozott hiba cskkenthet i) PPS implementcival, ii) a hiba modellezsvel, belertve az atmoszfrikus hatsokat is, iii)
differencilis technikval.
13.3 Szatellit ra korrekci

~
Legyen ~ a pszeudo-tvolsg s tt = ~ / c a szatellit s a vev kztti tvitel mrt
~
ideje, tovbb tm az az idpillanat, amelyben a pszeudo-tvolsg meg lett mrve.
Akkor a nominlis t SV id s a valdi t GPS id, amikor a szatellit a jelet elkdte:
~ ~
t SV = tm tt nominlis id,
t = t SV t SV valdi GPS id
(13.7)
A rendszer polinomilis korrekcit hasznl t SV szmra:
t SV = a f 0 + a f 1 (t t oc ) + a f 2 (t t oc ) 2 + t r ,
(13.8)
159
ahol t oc s a korrekcis egytthatk a navigcis zenet rszei, t r relativisztikus

korrekci:
t r = Fe A sin( E k )
(13.9)
Itt F = 4.442807633 10 10 s / m 5 konstans, A a szatellit plyjnak nagyobbik

fltengelye, E k pedig az excentrikus anomlia Kepler-egyenletvel szmthat. A
tapasztalat szerint kielgt pontossg, ha a korrekcis egyenletben t t SV kzeltssel lnk.
A t oc paramter a referencia id az rajel korrekci alkalmazhatsghoz. A
rendszer felttelezi, hogy a t t oc [302400, 302400] , ami egy GPS htnek felel
meg sec-ben. Elfordulhat, hogy egy ht kezdetn vagy vgn t s t oc eltr hetekre vonatkozik, vagyis
t t oc > 302400 . Ekkor a felhasznl felelssge a
t oc := t oc 604800 korrekci elvgzse gy, hogy t t oc visszakerljn az elrt

tartomnyba. A bejv szatellit zenet tartalmaz egy IODC integer adatot, amelynek
monitorozsval a felhasznl megllapthatja, ha j clock modell paramterek vannak rvnyben.
13.4 A szatellit pozci szmtsa

Az ephemeris paramterek rjk le a szatellit plyjt (orbit) egy legalbb 1 h idintervallumban. Az ephemeris paramterek a Kepler-fle becsls plyamodelljnek
pontostsra szolglnak. Az ephemeris paramtereket a szatellit sugrozza s dekdols utn a vev szmra rendelkezsre llnak.
13.4.1 Clock s ephemeris paramterek defincija

A clock s ephemeris paramterek rtelmezst a 13.1. tblzat tartalmazza.
13.1. tblzat. Clock s Ephemeris paramterek rtelmezse
TGD
IODC
t oc
Group delay [s ]
af2
a f1
Second-order correction to satellit clock [ s / s 2 ]

First-order correction to satellit clock [ s / s ]
af0
Constant correction to satellit clock [s ]
Issue of data, clock

Clock data reference time [s ]
160
M0
Mean anomaly at reference time [semicircles]
Mean motion difference from comuted value [semicircles/s]
eccentricity
1/ 2
Square root of the semimajor axis [m1 / 2 ]

Right ascension at reference time [semicircle]
i0
Inclination angle at reference time [semicircle]
&
i&
C ue
C us
C rc
C rs
C ic
C is
t oe
IODE
Argument of perigee [semicircle]

Rate of right ascension [semicircles/s]
Rate of inclination angle [semicircles/s]
Amplitude of the cosine harmonic correction term to the argument of
latitude [rad]
Amplitude of the sine harmonic correction term to the argument of
latitude [rad]
Amplitude of the cosine harmonic correction term to the orbit radius
[m]
Amplitude of the sine harmonic correction term to the orbit radius [m]
Amplitude of the cosine harmonic correction term to the angle of
inclination [rad]
Amplitude of the sine harmonic correction term to the angle of
inclination [rad]
Ephemeris reference time [s]
Issue of data, ephemeris
13.4.2 A szatellit antenna fziscentruma ECEF koordintinak szmtsra szolgl egyenletek

A szatellit ( X (i ) , Y (i ) , Z (i ) ) T ECEF koordintinak szmtsi szablyait a 13.2. tblzat tartalmazza. A A , C rc , C rs paramterek befolysnak rzkenysge az ECEF
pozci pontossgra
~ 1 m / m , ezzel szemben a szgparamterek
10 8 m/semicircle, a szsebessg paramterek 1012 m/(semicircle/s), kvetkezskpp
a szgparamterek szmtsnak pontossga alapveten fontos. Az eddig nem szerepl paramterek kzl = 3.986005 1014 m 3 / s 2 a WGS-84 univerzlis gravitcis lland.
Az IODE integer paramter monitorozsval megllapthat, mdosultak-e az
ephemeris paramterek az elz vtelhez kpest.
161
13.2. tblzat. Az ECEF szatellit pzci szmtsi egyenletei
Reativistic correction term
t SV = a 0 + a1 (t SV t oc ) + a 2 (t SV t oc ) 2
+ t r
t = t SV t SV
Correctionn to satellit clock
A = ( A)2
Corrected message
transmission time
Orbit semimajor axis
n0 = / A 3
rad/s
Computed mean motion
t k = t t oe
n = n 0 + n
rad/s
Time
from
ephemeris
reference epoch
Corrected mean motion
M k = M 0 + tk n
rad
Mean anomaly
E k = M k + e sin( E k )
rad
1 e 2 sin( E ) cos( E ) e
k
k
,
v k = arctan 2
1 e cos( E ) 1 e cos( E )
k
k
k = vk +
rad
Keplers equation for eccentric anomaly

True anomaly
rad
Argument of latitude
u k = C us sin( 2 k ) + C uc cos( 2 k )
rad
rk = C rs sin( 2 k ) + C rc cos( 2 k )
i k = C is sin( 2 k ) + C ic cos( 2 k )
u k = k + u k
rk = A[1 e cos( E k )] + rk
i k = i0 + i k + t k i&
Argument
of
correction
Radius correction
rad
Inclination correction
rad
Corrected argument latitude
Corrected radius
rad
Corrected inclination
x k = rk cos(u k )
x position in orbital plane
y k = rk sin(u k )
y position in orbital plane
k = 0 + (& ie )t k ie t oe
rad
x k = x k cos( k ) y k cos(i k ) sin( k )
Corrected longitude of
ascending node
Satellit x ECEF coordinate
y k = x k sin( k ) + y k cos(i k ) cos( k )
Satellit y ECEF coordinate
z k = y k sin(i k )
Satellit z ECEF coordinate
latitude
162
Vegyk szre, hogy a tblzat 1-7 sorban ll egyenletek itercit ignyelnek E k

meghatrozsra, amely ezen kvl a 7. sor egyenlete miatt egy nemlineris egyenlet
megoldst is ignyli.
13.5 A szmtsok szoftver megvalstsa

Az albbiakban bemutatjuk a GPS matematikai alapfeladat megoldst egy tesztesetben, valamint a szatellit ECEF koordintinak szmtst az ephemeris adatszegmens adataibl.
13.5.1 A vev ECEF GPS koordintit meghatroz szoftver megvalstsa

A 11.3. tblzat mutatja a ngy szatellit felttelezett ECEF koordintit s a vev
ezekkel sszetartoz pszudo-tvolsg rtkeit m-ben.
11.3. tblzat. A ngy szatellit ECEF koordinti s a vev pszeudo-tvolsgai a szatellitektl
SV 2
SV 26
SV 4
SV 7
X
+7766188.44
-25922679.66
-5743774.02
-2786005.69
Y
-21960535.34
-6629461.28
-25828319.92
-15900725.80
Z
+12522838.56
+31864.37
+1692757.72
+21302003.49
~
22228206.42
24096139.11
21729070.63
21259581.09
Az [xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter)
fggvny az itercit valstja meg, a keretprogram pedig frame_SolveRange.
function [xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter)
%Solve Pseudorange Equations for Satposes in ECEF
n=size(Satsxyz,1);
xyzc0=zeros(4,1);
xyzcact=xyzc0;
xyzciter=zeros(niter+1,4);
xyzciter(1,:)=xyzc0';
xyzcact=xyzc0;
for j=1:niter
H=[zeros(4,3) ones(4,1)];
rhoj=zeros(4,1);
for i=1:4
Satixyz=Satsxyz(i,:)';
dixyz=Satixyz-xyzcact(1:3);
di=norm(dixyz,2);
H(i,1:3)=-dixyz'/di;
rhoj(i)=di+xyzcact(4);
163
end;
drhohat=Satsrho-rhoj;
dxyzchat=inv(H'*H)*H'*drhohat;
xyzcact=xyzcact+dxyzchat;
xyzciter(j+1,:)=xyzcact';
end;
xyzc=xyzciter(niter+1,:)';
%frame_SolveRange.m
%Compute estimated rover coordinates
clear all
close all
clc
format long
Satsxyz=[+7766188.44 -21960535.34 +12522838.56; ...
-25922679.66 -6629461.28 +31864.37; ...
-5743774.02 -25828319.92 +1692757.72; ...
-2786005.69 -15900725.80 +21302003.49]
Satsrho=[22228206.42 24096139.11 21729070.63 21259581.09]'
niter=5
[xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter);
fprintf('Results of iteration in x,y,z,c*Delta_tr order\n');
xyzciter
fprintf('Last H matrix\n');
H
fprintf('Receiver coordinates in x,y,z,c*Delta_tr order\n');
xyzc
format short
Futsi eredmnyek:
Results of iteration in x,y,z,c*Delta_tr order
xyzciter =
1.0e+006 *
0
-2.97757147597244
-2.45172853419043
-2.43077221884605
-2.43074509597674
-2.43074509593620
0
-5.63527815933950
-4.73087846098026
-4.70237580233497
-4.70234511363092
-4.70234511359277
0
4.30423450558470
3.57399752045577
3.54660387208444
3.54656870605020
3.54656870599656
0
1.62523980181658
0.31407073267925
0.26474970653356
0.26469112951536
0.26469112943121
164
Last H matrix
H =
Columns 1 through 3
-0.46426691725303
0.98575355484955
0.15435009076044
0.01692128868937
0.78576630371213
0.08086441780387
0.98423412639754
0.53338601476057
-0.40869003596240
0.14748177865320
0.08636685658695
-0.84570268371747
Column 4
1.00000000000000
1.00000000000000
1.00000000000000
1.00000000000000
Receiver coordinates in x,y,z,c*Delta_tr order
xyzc =
1.0e+006 *
-2.43074509593620
-4.70234511359277
3.54656870599656
0.26469112943121
A vev ECEF koordintinak rtkeit az iterci egymst kvet lpseiben kln

is sszefoglaltuk a 13.4. tblzatban.
13.4. tblzat. A vev ECEF koordintinak rtkei az iterci sorn
x ECEF
y ECEF
z ECEF
0
-2977571.48
-2451728.53
-2430772.22
-2430745.10
-2430745.10
0
-5635278.16
-4730878.46
-4702375.80
-4702345.11
-4702345.11
0
4304234.51
3573997.52
3546603.87
3546568.71
3546568.71
ct r
0
1625239.81
314070.73
264749.71
264691.13
264691.13
13.5.2 Az ephemeris adatszegmens kirtkelsnek szoftver megvalstsa

Az albbi MATLAB programrszlet bemutatja egy ephemeris adatszegmens feldolgozsnak pldjn az E k iteratv szmtsra szolgl EE_k=compEk(t,E_k0)
fggvnyt, az ltala az fsolve keretben meghvott y=funEk(x) fggvnyt s az
165
ephemeris adatokbl a szatellit ECEF koordintit meghatroz frame_Ephemeris

MATLAB scriptet s a futsi eredmnyeket. Az ephemeris adatszegmens adatai s a
fontos konstansok rtkei a keretprogramban knnyen megtallhatk a %Received Satellite Information s az %Important Constants commentek utn. Az iterci addig folyik, amg runEk=input('runEk=') krdsre
1 vlasz rkezik.
13.5. tblzat. A szatellit felttelezett ephemeris adatszegmense
t SV
4.03272930e+05
wn %GPS week number
910
t ow %Seconds in GPS week
403230
t gd %group delay in s
2.3283e-09
aodc %clock data issue
409
410400
t oc
a2
0.00000e+00
a1
1.819e-12
a0
3.2977667e-05
AODE %orbit data issue
153
4.3123e-09
n
M0
e
1/ 2
2.24295542
4.27323824e-03
5.15353571e+03
A
t oe
410400
C ic
9.8720193e-08
C rc
282.28125
C is
-3.9115548e-08
C rs
-132.71875
C uc
-6.60121440e-06
C us
5.31412661e-06
2.29116688
i0
&
i&
-0.88396725
0.97477102
-8.025691e-09
-4.23946e-10
166
function EE_k=compEk(t,E_k0)
%Compute Ek
global t_sv w_n t_ow t_gd aodc t_oc a_2 a_1 a_0 AODE D_n M_0 e
global sqrtA t_oe C_ic C_rc C_is C_rs C_uc C_us DOmega omega i_0
global DOmegaDot DiDot
global omega_ie mu c F f_1 f_2 lambda_1 lambda_2 lambda_w lambda_n
global A n_0 Dt_sv t t_k n M_k E_k
A=sqrtA^2;
n_0=sqrt(mu/A^3);
t_k=t-t_oe;
if t_k>302400, t_k=t_k-604800; end;
if t_k<-302400, t_k=t_k+604800; end;
n=n_0+D_n;
M_k=M_0+t_k*n;
option=optimset('TolFun',1e-10);
EE_k=fsolve(@funEk,E_k0,option);
function y=funEk(x)
%Function solve funEk(x)=0
y=M_k+e*sin(x)-x;
%frame_Ephemeris.m
%Frame program for ephemeris computation
clear all
close all
clc

%Received Satellite Information
t_sv=4.03272930e+05;
w_n=910;
%GPS week number
t_ow=403230;
%Seconds in GPS week
t_gd=2.3283e-09;
%group delay
aodc=409;
%clock data issue
t_oc=410400;
a_2=0.00000e+00;
a_1=1.819e-12;
a_0=3.2977667e-05;
AODE=153;
%orbit data issue
D_n=4.3123e-09;
M_0=2.24295542;
e=4.27323824e-03;
sqrtA=5.15353571e+03;
t_oe=410400;
C_ic=9.8720193e-08;
C_rc=282.28125;
C_is=-3.9115548e-08;
C_rs=-132.71875;
C_uc=-6.60121440e-06;
C_us=5.31412661e-06;
DOmega_0=2.29116688;
omega=-0.88396725;
i_0=0.97477102;
DOmegaDot=-8.025691e-09;
DiDot=-4.23946e-10;
%Important Constants
%pi=3.1415926535898
omega_ie=7.2921151467e-05;
mu=3.986005e14;
c=2.99792458e08;
F=-4.442807633e-10;
f_1=1575.42;
f_2=1227.60;
lambda_1=c/f_1;
lambda_2=c/f_2;
lambda_w=c/(f_1-f_2);
lambda_n=c/(f_1+f_2);
%WGS-84 earth rotation rate

%WGS-84 gravitational constant
%speed of light
%MHz L1 carrier frequency
%MHz L2 carrier frequency
%19.0 cm L1 wavelength
%24.4 cm L2 wavelength
%86.2 cm wide-lane wavelength
%10.7 cm narrow-lane wavelength
%
t=t_sv;
runEk=1;
E_k0=0;
while runEk
E_k=compEk(t,E_k0)
Dt_r=F*e*sqrtA*sin(E_k)
Dt_sv=a_0+a_1*(t_sv-t_oc)+a_2*(t_sv-t_oc)^2+Dt_r
t=t_sv-Dt_sv
E_k0=E_k;
runEk=input('runEk=');
end;
167
168
%
[E_k,A,n,t_k,n,M_k]=compEk(t,E_k0);
E_k
A
n
t_k
M_k
v_k=atan2(sqrt(1-e^2)*sin(E_k)/(1-e*cos(E_k)),(cos(E_k)-e)/(1e*cos(E_k)))
phi_k=v_k+omega
du_k=C_us*sin(2*phi_k)+C_uc*cos(2*phi_k)
dr_k=C_rs*sin(2*phi_k)+C_rc*cos(2*phi_k)
di_k=C_is*sin(2*phi_k)+C_ic*cos(2*phi_k)
u_k=phi_k+du_k
r_k=A*(1-e*cos(E_k))+dr_k
i_k=i_0+di_k+t_k*DiDot
x_kprime=r_k*cos(u_k)
y_kprime=r_k*sin(u_k)
Omega_k=DOmega_0+(DOmegaDot-omega_ie)*t_k-omega_ie*t_oe
format long
x_k=x_kprime*cos(Omega_k)-y_kprime*cos(i_k)*sin(Omega_k)
y_k=x_kprime*sin(Omega_k)+y_kprime*cos(i_k)*cos(Omega_k)
z_k=y_kprime*sin(i_k)
format short
Futsi eredmnyek frame_Ephemeris esetn:

A vltozk nevei kvetik az egyenletekben szerepl jellst.
E_k =
1.2073
Dt_r = -9.1449e-009
Dt_sv = 3.2956e-005
t = 4.0327e+005
runEk=1
E_k =
1.2073
Dt_r = -9.1449e-009
Dt_sv = 3.2956e-005
t = 4.0327e+005
runEk=0
E_k =
1.2073
A = 2.6559e+007
n_0 = 1.4587e-004
t_k = -7.1271e+003
n = 1.4587e-004
M_k =
1.2033
v_k =
1.2113
phi_k =
0.3274
du_k = -2.0003e-006
dr_k = 143.0967
di_k = 5.4489e-008
u_k =
0.3274
r_k = 2.6519e+007
i_k =
0.9748
x_kprime = 2.5111e+007
169
y_kprime = 8.5267e+006
Omega_k = -27.1159
x_k =
-5.678411013083118e+006
y_k =
-2.492396293091095e+007
z_k =
7.056518876151539e+006
A 13.6. tblzat sszefoglalja a kirtkels eredmnyeit tblzatos alakban is.

13.6. tblzat. Az ECEF szatellit pozci szmtsi vgeredmnyei ephemeris adatokbl
az iterci vgn
1.2073 az iterci vgn
Ek
-9.1449e-009
t SV = a 0 + a1 (t SV t oc ) + a 2 (t SV t oc ) 2
+ t r
t = t SV t SV
3.2956e-005
4.0327e+005
2.6559e+007
n0 = / A 3
rad/s
1.4587e-004
t k = t t oe
-7.1271e+003
n = n 0 + n
rad/s
1.4587e-004
M k = M 0 + tk n
rad
1.2033
E k = M k + e sin( E k )
rad
1.2073
1 e 2 sin( E ) cos( E ) e
k
k
,
v k = arctan 2
1 e cos( E ) 1 e cos( E )
k
k
k = vk +
rad
1.2113
rad
0.3274
u k = C us sin( 2 k ) + C uc cos( 2 k )
rk = C rs sin( 2 k ) + C rc cos( 2 k )
i k = C is sin( 2 k ) + C ic cos( 2 k )
u k = k + u k
rk = A[1 e cos( E k )] + rk
i k = i0 + i k + t k i&
rad
-2.0003e-006
143.0967
rad
5.4489e-008
rad
0.3274
2.6519e+007
rad
0.9748
x k = rk cos(u k )
2.5111e+007
y k = rk sin(u k )
8.5267e+006
k = 0 + (& ie )t k ie t oe
rad
-27.1159
A = ( A)
170
x k = x k cos( k ) y k cos(i k ) sin( k )
-5678411.01
y k = x k sin( k ) + y k cos(i k ) cos( k )
-24923962.93
z k = y k sin(i k )
7056518.87
13.6 Differencilis GPS (DGPS) a pozci trben

A pszeudo-tvolsgot (13.2-6) szerint a + zaj is terheli, amelyben normja
jelentsen nagyobb normjnl, tovbb nem fgg a vevtl, hanem csakis a
szatellitektl. Ezrt ha rendelkezsre ll egy bzislloms, amelynek az ECEF koordinti ismertek, s amely ugyanazt a 4 szatellitet ltja, mint a sajt jrm (rover),
valamint kpes sajt mrsi eredmnyeibl szmtott korrekcis adatok
sztsugrzsra a krnyezetben lv jrmveknek, akkor mint ltni fogjuk,
eliminlhat a sajt jrmben, s ezltal a rover pozcimeghatrozsnak a pontossga jelentsen feljavul. Azonos szatellitek vtele azonban felttelezi, hogy a sajt jrm tvolsga a bzisllomstl nem nagyobb kb. 50 km-nl.
Megjegyzend, hogy a bzissllomst kivlthatja a sajt jrmre szerelt msodik
antenna a hozztartoz msodik vevvel, ugyanis feltehet, hogy a kt antenna elhelyezkedse ismert a sajt jrmvn (2-antenns DGPS).
A DGPS mkdsi elvt a (13.6) egyenletbl vezetjk le. A bzisllomst s a
sajt jrmvet 0 s r indexszel klnbztetjk meg.
~ = Hx + +
0
0
0
(13.10)
~
r = Hx r + + r
Mivel 0 , r normja jelentsen kisebb, mint normja, ezrt csak kompenzlsra koncentrlunk. (13.10)-bl s a bzislloms helyzetnek pontos ismeretbl kvetkezik, hogy
~ = x + (HT H ) 1 HT ( + ) x + (HT H ) 1 HT
(HT H ) 1 HT
0
0
0
0
x 0 = x 0 + x 0 x 0 = 0
~ (HT H ) 1 HT
(HT H ) 1 HT
(13.11)
~ = x + (H T H ) 1 H T ( + ) x + (H T H ) 1 H T
(H H) H
r
r
r
r
~ (H T H ) 1 H T
x (H T H ) 1 H T
T
~ (H T H ) 1 H T ~
= (H H ) H
0
r
T
(13.12)
(13.13)
Clszer teht bevezetni egy x corr korrekcis tagot, amellyel a vev ECEF pozcijnak a becslsbl eltntethet a dominns (H T H ) 1 H T tag hatsa, miltal
a vev pozci-meghatrozsnak pontossga jelentsen javul:
14. LLAPOTBECSLS KPFELDOLGOZS S IMU BEVONSVAL
171
x corr = (H T H ) 1 H T ~
0
(13.14a)
~ + x
x r := (H T H ) 1 H T
r
corr
x r := x r 0 + x r
(13.14b)
(13.14c)
Egyttal az is kvetkezik a fentiekbl, hogy a bzisllomsnak a jrmvek szmra a x corr zenetet kell forgalmaznia.
172
14. LLAPOTBECSLS KPFELDOLGOZS S

IMU BEVONSVAL
14.1 Az rzkelk fizikai alapjai
Inercilis mregysg (IMU, Inertial Measurement Unit) fejlett verzija kpes a 3D
gyorsuls s a 3D szgsebessg mrsre. Egyik korszer kivitele a Crossbow
MNAV 100CA egysg, amely MEMS technolgival kszlt.
A MEMS szgsebessgrzkel tartalmaz egy kismret, rezgst vgz rudacskt, amelynek oszcillcis frekvencija nemnulla szgsebessg esetn a Coriolis hats kvetkeztben megvltozik. A szgsebessg rzkel kimenjele kzelthet az
y gyro = k gyro + gyro (T ) + gyro
(14.1)
sszefggssel, ahol y gyro a V-ban mrt kimenet, k gyro az ersts, gyro (T ) a

hmrskletfgg bias s gyro nulla vrhatrtk Gauss-zaj.
A MEMS gyorsulsrzkel egy kis lapocskt tartalmaz, amely hozz van
erstve egy torzis rudacskhoz. A lapocka forog s gyorsuls hatsra megvltozik a kapacitsa a krnyezet falhoz kpest, amelyre fel van erstve. A kapacitsvltozs arnyos az inerciarendszerbeli gyorsulssal. A gyorsuls rzkel kimenjele kzelthet az
y acc = k acc a + acc (T ) + acc
(14.2)
sszefggssel, ahol y acc a V-ban mrt kimenet, k acc az ersts, acc (T ) a

hmrskletfgg bias s acc nulla vrhatrtk Gauss-zaj.
A gyorsulsrzkel egy specifikus ert mr, mely az inerciarendszerben hat,
amelyet azonban a helikopterhez rgztett, azzal egytt mozg (nem inercia) koordinta-rendszerben fejezi ki. Ha a a gyorsuls az inerciarendszerben, v a sebessg
s a szgsebessg a helikopterhez rgztett koordinta-rendszerben, akkor a
mozg koordinta-rendszerekben rvnyes derivlsi szably szerint
ax

1
a y = v& + v Fgravity .
m
a
z
(14.3)
173
A tovbbiakban gyelni kell arra, hogy az egyes vektorok milyen koordintarendszerben keletkeznek, s a szksges koordinta transzformcikat ki kell dolgozni s alkalmazni kell.
A vizsglt problma egy ngyrotoros beltri helikopter (UAV)
rzkelrendszert mutatja be, ezrt a beltri jelleg kvetkeztben pozci/orientci
rzkelsre GPS technika nem alkalmazhat (beltrben rendszerint nem vehet a
szatellitek jele). Ezrt pozci s orientci rzkelsre marker-bzis kpfeldolgoz
rendszer alkalmazhat. A helikopterre vilgt ledek vannak szerelve, amelyek
markerknt szolglnak, s amelyek mozgst a szoba mennyezetre szerelt kamera
detektlja, s a mozgs sztereo (motion stereo) elmletre alapozva meghatrozza a
helikopter pozcijt s orientcijt.
Az IMU a helikopterre van szerelve, sajt processzora van, amely CAN buszon
keresztk kommunikl a helikopter begyazott szablyozjval, amely egy
Freescale MPC555 processzorral van megvalstva. Az IMU kb. 100 Hz mintavteli
frekvencival tudja mrni a 3D gyorsulst s szggyorsulst. A kpfeldolgoz rendszer egy host szmtgpen fut, 10 frame/sec gyakorisggal kld pozci s orientci informcit a begyazott szablyoz szmra. A nem mrhet llapotvltozk
becslse a mrt jelekre van alapozva, az llapotbecslst kiterjesztett Kalman-szrk
vgzik.
14.2 A helikopter kinematikai modellje

Multikomponens robotrendszerek lersnl fontos szerepet jtszanak a koordinta-rendszerek. Jellje K W = K H 0 a helikopter koordinta-rendszert a kezdeti
ll helyzetben (world vagy base frame), K H a helikopter tmegkzppontjban a
helikopterhez rgztett s azzal egytt mozg keretet, tovbb K S az IMU szenzor
kerett, amely a helikopterre van szerelve. Egyszersg kedvrt a ltrendszer kerete legyen azonos K W -vel. A grf a kvetkez:
TH , S
TW , H
K W = K H 0
K H
K S
(14.4)
A K W keretet j kzeltsel inercia keretnek tekinthetjk a rvididej szablyozsok szmra. A helikopter pozcijt s orientcijt K W = K H 0 -hoz kpest jellemezhetjk a = ( x, y , z ) T pozcivektorral s a = ( , , ) T orientcival,
ahol , , a navigciban hasznlt Euler-szgek (a robotikban RPY-szgek)
s AKW , K H = A( , , ) az orientcis mtrix:
174

C C
A = S C
S
C S S S C
S S S + C C
C S
C S C + S S
S S C C S
C C
(14.5)
A szgsebessg felrhat = & alakban, ahol & = && + && s

0
1
= 0 C
0 S
S
C S
C C
(14.6)
14.3 llapotbecsls IMU s kpfeldolgozs bevonsval

Az llapotbecslst ktszint kiterjesztett Kalman-szrvel realizljuk. Az llapotbecsls sorn az IMU s a marker-bzis kpfeldolgozs ltal szolgltatott mrsi
eredmnyeket hasznljuk fel. Az IMU mrsi frekvencija 100 Hz (mintavteli ideje
10 ms), a kpfeldolgozs 10 frame/sec gyakorisggal szolgltat mrsi eredmnyeket (mintavteli ideje 100 ms). Az eltr mintavteli dkre az llapotbecsls
sorn tekintettel kell lenni.
14.3.1 Az orientci s szgsebessg becslse (EKF1)

Fogjuk ssze az IMU ltal mrt szgsebessg komponenseket a S K S vektorban, amely a korrekt S ,0 rtken kvl tartalmazza mg az ismeretlen bias
rtkt s a Gauss-zajt, s transzformljuk ezeket K H -ba AS -sel (a zaj eljele nem
lnyeges):
Itt
KH
S = S , 0 + S ,b + S , n
(14.7a)
AS S = AS S ,0 + AS S ,b + AS s , n
(14.7b)
:= AS S ,0 = AS S AS S ,b + AS s ,n
(14.7c)
a szgsebessg vektor, amelynek szoksos alakja a jrm-
navigciban
= ( P, Q, R) T , amelybl
kiszmthatk, felhasznlvn hogy
1 T S
C
=: F ( , ) = 0
0 S / C
az
Euler-szgek
T C
S
C / C
derivltjai
(14.8)
175
&

& = F ( , ) == FAS S ,b + FAS S + FAS S ,n
&

& S ,b = S ,b, n
(14.9a)

= +

m n
(14.9b)
Itt S mrst az IMU, ( , , ) Tm mrst pedig a marker-bzis kpfeldolgoz rendszer vgzi.

Jellje T az IMU mintavteli idejt, akkor a folytonosidej nemlineris
llapot-egyenlet Euler-approximcival diszkrtidej nemlineris alakra
hozhat:
x1 = ( , , ) T , x 2 = S ,b , x = ( x1T , x 2T ) T ,
u = S , w1 = S ,n , w2 = S ,b,n , w = ( w1T , w2T ) T
x1,k +1 = x1,k TFk ASk x 2,k + TFk ASk u k + TFk ASk w1, k
x 2,k +1 = x 2, k + Tw2,k
(14.10)
y k = x1,k + z k
x k +1 = f ( x k , u k , wk )
y k = g (xk , z k )
Tegyk fel, hogy w s z korrellatlanok, s vezessk be a kvetkez jellseket:
R w, k = E[ wk wkT ], R z ,k = E[ z k z kT ]
Ak 1 =
f ( x k 1 , u k 1 ,0)
f ( x k 1 , u k 1 ,0)
, B w,k 1 =
, R w,k 1 := B w, k 1 R w,k 1 B wT ,k 1
x
w
g ( x k ,0)
g ( x k ,0)
Ck =
, C z ,k =
, R z ,k := C z ,k R z ,k C zT,k
x
z
(14.11)
176
A kiterjesztett Kalman-szrnek (EKF1) a kvetkez szmtsokat kell elvgeznie:

1. Prediction:
x k = f ( x k 1 , u k 1 ,0)
M k = Ak 1 k 1 AkT1 + B w,k 1 R w,k 1 B wT ,k 1
2. Time update:
S k = C k M k C kT + C z , k R z ,k C zT, k
G k = M k C kT S k1
x k = x k + G k ( y k g ( x k ,0) )
k = M k G k S k G kT
Mivel az IMU mintavteli ideje 10 ms, mg a kpfeldolgozs 100 ms, ezrt ha
nem rkezett orientci adat, akkor a time update mdosul: x k := x k + 0 , mikzben
a tbbi szmts elvgrsre kerl.
14.3.2 A sebessg s pozci becslse (EKF2)

Az IMU 3D gyorsuls mrsi eredmnyei a kvetkez alakban modellezhetk:
a S = a S ,0 + a S ,b + a S , n AS1 A( , , ) 1 g
a S ,0 =
AS1
(a + p S
(14.12a)
+ ( p S ) )
(14.12b)
1
a m := AS a S p S ( p S ) + A( , , ) g
(14.12c)
ahol a szggyorsuls (nem mrt).

Jellje v K H a helikopter sebessgt, akkor a differencils mozg koordintarendszerbeli alakjnak felhasznlsval rhat, hogy
v& = v AS a S ,b + a m + AS a S ,n
a& S ,b = a S ,b,n
(14.13a)
p& = A( , , )v + v p ,n
pm = p + pn
(14.13b)
A folytonosidej nemlineris modell knnyen trhat diszkrtidejv az Eulerapproximci felhasznlsval, s a kvetkez alakot veszi fel:
177
x = (v T , a ST ,b , p T ) T , u = a m , w = (a ST , n , a ST ,b, n , v Tp , n ) T
y = pm , z = pn
v k +1 = v k + T ( k v k AS , k a S ,b,k + a m,k ) + TAS ,k a S ,n, k
a S ,b,k +1 = a S ,b,k + Ta S ,b,n,k
p k +1 = p k + TAk v k + Tv p ,n, k
(14.14)
p m, k = p k + p n, k
x k +1 = f ( x k , u k , wk )
y k = g (xk , z k )
Ezutn mr a msodik kiterjesztett Kalman-szr (EKF2) egyszeren kpezhet

az EKF1 mintjra. A a m szmtshoz szksges szggyorsuls numerikus derivlssal kpezhet rtkbl a bais levonsa utn, a tbbi jel kzl az IMU
mri az a S gyorsulst s a kpfeldolgoz rendszer a p m pozcit. Az eltr mrsi
mintavteli idket a time update sorn lehet figyelembe venni, hasonlan EKF1-hez.
14.1. bra. A ktszint Kalman-szr struktrja
A ktszint Kalman-szr struktrjt a 14.1. bra mutatja be. Az brn a

pozcit s az orientct a marker-bzis kpfeldolgoz rendszer mri, az
szgsebessget s a && gyorsulst az IMU mri. Az indexek kzl m (measured) a
jelek mrt rtkre, e (estimated) pedig a becslt rtkre utal. Megjegyezzk, hogy
EKF2 felhasznlja az EKF1 eredmnyeit, ezrt EKF2 az EKF1 utn indthat.
178
Ezen kvl fontos rendszertechnikai szempont, hogy az EKF-en alapul llapotbecsls stabilitst az elmlet ltalban nem garantlja. Problmk esetn a T mintavteli id cskkentse javasolhat, amely az Euler-approximci pontossgt is
javtja. A ngyrotoros helikopter (UAV) begyazott irnytsa sorn a ktszint kiterjesztett Kalman-szrvel stabilitsi problmk nem lptek fel.
Az llapotbecsls az irnytsi algoritmus rsze. Az irnytsi trvny n.
visszalpses (backstepping) stabilizlsi technikt hasznl (lsd pl. Lantos: Irnytsi rendszerek elmlete s tervezse II. Akadmiai Kiad, 2003).
Az llapotbecslst s az irnytsi trvnyt Freescale MPC 555 processzor realizlja vals idben. A begyazott szablyoz MATLAB/Simulink krnyezetben fejleszthet s a MATLAB, Simulink, Real Time Workshop, MPC555 Target
Compiler segtsgvel kifordthat a target processzorra (gyors prototpus tervezs).
A begyazott szablyoz repls eltti tesztelse hasonl technikval vgezhet
a dSPACE DS1103 eszkz bevonsval, ahol a DS1103 szimullja a helikopter dinamikt, a beavatkoz szerveket s a szenzor rendszert (hardware-in-the-loop test).
A kt komponens kztt az adatforgalom a valsgnak megfelelen CAN
buszon keresztl trtnhet a tesztels sorn.

MobilisRobotokNavigacioja PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

MobilisRobotokNavigacioja PDF

Uploaded by

Copyright:

Available Formats

139

MOBILIS ROBOTOK NAVIGCIJA

11.1 Aktulis Kalman-szr idben vltoz diszkrtidej

visszacsatolsban szmthatv vlik (mert csak az elz u k 1 kell hozz), de mr

Ex(0) = x 0 , E[( x(0) x 0 )( x(0) x 0 ) T ] = 0 0 (pozitv szemidefinit) (11.2b)

Ez k = 0 , E[ z k z lT ] = R z , k kl , R z , k > 0 (pozitv definit)

Lantos: MOBILIS ROBOTOK NAVIGCIJA

Keressk azt a lineris szrt, amely az u k s y k +1 mrsi eredmnyekbl az

algoritmus alapjn az x k llapot optimlis x k becslst adja abban az rtelemben,

Az infimum az M nsn szimmetrikus mtrixok (kvadratikus alak rvn definilt)

Kpezzk a becslt llapotot s a fut hibt k + 1 esetn:

A hiba vrhat rtkre vonatkoz (11.4a) felttel szerint

11. KITERJESZTETT KALMAN-SZR

k +1 = ( I G k +1C k +1 ) M k +1 ( I G k +1C k +1 ) T + G k +1 R z ,k +1G kT+1 .

alakba, ahonnan kvetkezik az infimum albbi felttele opertor alakban:

Tetszleges X esetn teljesl, hogy

Az infimum felttele ezrt

Lantos: MOBILIS ROBOTOK NAVIGCIJA

Helyettestsk vissza az optimlis G k +1 megoldst a (11.83) egyenletbe:

k +1 = ( I G k +1C k +1 ) M k +1 ( I G k +1C k +1 ) M k +1C kT+1G kT+1 + G k +1 R z ,k +1G kT+1 =

k +1 = M k +1 M k +1C kT+1 (C k +1 M k +1C kT+1 + R z , k +1 ) 1 C k +1 M k +1 .

Bontsuk fel a becslt llapot kpzst a mrsi idpontok kztt elvgezhet

ahol M k +1 szintn meghatrozhat a mrsi idpontok kztt. Msrszt a becslt

11. KITERJESZTETT KALMAN-SZR

Az "aktulis" Kalman-szr algoritmusa:

Frissts mrsi idpontok kztt (time update between measurements):

Mrsi eredmny frisstse (measurement update):

Megjegyzs: Ha az llapotegyenletben a rendszerzaj Bv ,k v k , a mrsi zaj pedig

R z , k := C z ,k R z ,k C zT,k helyettestseket, mert pldul

Lantos: MOBILIS ROBOTOK NAVIGCIJA

11.2 Kiterjesztett Kalman-szr (EKF)

Vegyk szre, hogy

ismert jeleknek tekinthetk.

12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK

Foglaljuk ssze az eddigieket. Tekintsk a nemlineris rendszert a [(k 1)T , kT ]

Lantos: MOBILIS ROBOTOK NAVIGCIJA

12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK

12.1. bra. A navigcinl hasznlt koordinta-rendszerek

12.1 Az ECI inercia rendszer

12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK

12.2 Az ECEF koordinta-rendszer

(1 + 365.25) cycles 2 rad / cycle

A forgsellipszoid egy ellipszisnek a z -tengely krli forgatsval keletkezik. Az

Az ECEF koordinta-rendszerben egy P pont az origbl a pontba mutat

Lantos: MOBILIS ROBOTOK NAVIGCIJA

12.2. bra. A trbeli koordintk szmtsnak elve ECEF keret esetn

Az brnak megfelelen a kvetkez sszefggsek rvnyesek:

12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK

Normlis egyenesnek metszete a z -tengellyel (ahol y = 0 ):

Meg kvnjuk mg hatrozni az N ( ) fggvnyt, amelyhez nhny azonossgot

Lantos: MOBILIS ROBOTOK NAVIGCIJA

A (12.5-7) eredmnyeket a kvetkez algoritmusban foglaljuk ssze.

hanem atan fggvnnyel szmtjuk := atan( z + e 2 NS , p ) alapjn.

er is hat a raktra, ahol

12. A NAVIGCINL HASZNLT KOORDINTA-RENDSZEREK

fgg a rakta tmegkzppontjba mutat r vektortl.

Lantos: MOBILIS ROBOTOK NAVIGCIJA

A futsi eredmnyek a direkt feladat esetn:

Futsi eredmnyek az inverz feladat esetn:

Inverz problem: x,y,z ECEF -> lambda_deg,phi_deg,h

13. A Globlis Pozcionl Rendszer (GPS)

12.3 A NED koordinta-rendszer

Lantos: MOBILIS ROBOTOK NAVIGCIJA