Navigációs célú jelfeldolgozás

Mesterséges Intelligencia labor

Navigációs célú jelfeldolgozás Mérési útmutató

Összeállította: Kis László ,doktorandusz [email protected]

Prohászka Zoltán ,tanársegéd [email protected]

BME Irányítástechnika és Informatika Tanszék 2010

Tartalomjegyzék 1. Bevezetés

2

2. Elméleti alapismeretek 2.1. GPS működése . . . . . . . . . . . . . 2.2. Inerciális mérőegységek működése . . . 2.3. Állapotbecslés Kalman-szűrővel . . . . 2.4. A méréshez használt rendszermodellek

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

4 4 6 8 9

3. A mérés menete 13 3.1. Mérés során használt adatformátumok . . . . . . . . . . . . . . . 13 3.2. Mérési feladatok . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

1

1. fejezet

Bevezetés Egy jármű mozgásállapotának irányításához elengedhetetlen az irányítani kívánt fizikai mennyiségek mérése. Egy szabályzási kör pontossága és megbízhatósága sohasem lehet jobb a kimenet mérésének pontosságánál és megbízhatóságánál. Járműnavigáció esetében a jármű helyzetét kell minél pontosabban meghatározni. Az ehhez alkalmazható szenzorok kiválásztásánál probléma, hogy mindegyik szenzor más tekintetben pontos illetve pontatlan. Kézenfekvő megoldás, hogy több szenzort alkalmazunk ugyanazoknak a menynyiségeknek a mérésére, és valamilyen módszerrel megpróbáljuk kombinálni az egyes érzékelők előnyeit. Tipikus példa egy ilyen megoldásra a robotikában az inkrementális enkoderek és kalibrációs potmétereket használó algoritmus. Ez a felállás tipikusnak tekinthető, miszerint egyik szenzor differenciális jellegű, azaz a kimenet DC összetevője teljesen bizonytalan, míg az AC összetevője igen pontos, míg egy másik szenzor kimenete akkora zajjal terhelt, hogy az csak hosszú időn át tartó átlagolással tüntethető el. A szituációt tovább bonyolítja, ha a mérendő mennyiség nem változhat meg tetszőleges módon, pl. egy pozíció jellegű mennyiség, ami nem ugorhat, illetve deriváltjának sem lehet bizonyos mértéknél nagyobb meredeksége. Egy irányított dinamikai rendszer esetében visszamérhetjük a beavatkozó jel tényleges mértékét is, és ismerhetjük a rendszert érő egyéb hatások modelljét. Ez esetben célszerű a rendszer dinamikus jellemzőit beépíteni a szenzorok kimenetét kombináló algoritmusba. Például egy maximálisan 100 N-t erőt kifejteni képes hajtással hajtott tömeg egész másképpen mozog, ha súrlódása elhanyagolható és 350 kg tömegű, és megint másként, ha a hajtott tömeg 10 kg, és a fellépő súrlódási erők nagyságrendje 50 N. Lineáris dinamikus rendszerek esetében létezik optimális algoritmus a fent vázolt problémák megoldására, ez a Kalman-szűrő (KF), ami megalkotójáról, Kalman Rudolfról kapta a nevét. Nemlineáris rendszerek adott állapot környékén linearizálhatóak, vagy kezelhetőek kiterjesztett Kalman-szűrővel (EKF), ami képes nemlineáris összefüggések kezelésére is. A (lineáris) Kalman-szűrő egy lineáris állapotbecslési algoritmus (állapot-visszacsatolt szabályozók esetében is ilyeneket szokás használni), ahol a mért és ismeretlen mennyiségek zajmodelljé2

nek ismeretében Kalman módszerével határozhatóak meg az optimális lineáris állapotbecslés mátrixai. Vegyük észre, hogy egy tetszőleges spektrumú zaj modellezhető úgy, hogy fehér zajt eresztünk át a zajmodellnek megfelelő dinamikus lineáris rendszeren. A mérés keretén belül egy síkban mozgó autó pozícióját és orientációját próbáljuk minél pontosabban meghatározni GPS és inerciális szenzorok jelei alapján. A számításnál figyelembe vesszük, hogy az autó leghátsó differenciálműve csak az autó hossztengelye irányában mozdulhat el (ha a hátsó kerekek rendesen tapadnak és kellően fel vannak fújva). Az inerciális szenzor 3D gyorsulás és szögsebesség méréseket szolgáltat a saját koordináta-rendszerében. A következőkben részletesen bemutatjuk a szükséges elméleti hátteret, és egzakt specifikációt adunk az elvégzendő feladatokról. Kérem, hogy amennyiben hibát talál a mérési útmutatóban, akkor azt jelezze a [email protected] címen!

3

2. fejezet

Elméleti alapismeretek 2.1.

GPS működése

A GPS rendszer egy 32 műholdból álló pozícionáló rendszer. A műholdak a Föld felülete felett egyenletesen vannak elosztva, így biztosítva a napi 24 órás elérhetőséget. A pozíció meghatározás rádiófrekvencia segítségével, távolságmérés alapján történik. Az alap GPS algoritmus a következő egyenlet megoldásán alapul: Ri =

p

(Xi − x)2 + (Yi − y)2 + (Zi − z)2 + Cerr

(2.1)

ahol Ri az i-ik műhold vevőtől mért távolsága, Xi , Yi , Zi az i-ik műhold pozíciója, Cerr a vevő órahibájából származó mérési hiba és x, y, z a vevő pozíciója. A műhold pozícióját a vevő által gyűjtött adatokból ki lehet számítani. A műholdtól mért távolságot(Ri ) a GPS-vevő a rádiójelre szuperponált kód fázisának analíziséből számítja. A maradék négy paramétert(x,y,z,Cerr ) kell meghatározni, ezért egy időpillanatban legalább négy egyenlet felírására van szükség, ami miatt a pozíció meghatározásához legalább négy műhold szükséges. A hagyományos GPS vevők a pozíció számítást az irányítási algoritmusok igényeihez képest lassan, tipikusan 1-20Hz-es sebességgel képesek elvégezni. Az itt bemutatott algoritmus néhány tíz méter erejéig pontos csak. A pontatlanságnak több forrása van. Egyrészt a műhold pozícióinak meghatározásában van néhány méteres pontatlanság. Másrészt a rádióhullámoknak keresztül kell haladni a Föld légkörén, ahol a terjedési sebességük ismeretlen mértékben lecsökken. Megjegyzendő azonban, hogy míg az abszolút pozíció pontatlansága jelentős (tipikusan 50-100m), a két mérési időpont közötti relatív elmozdulás már sokkal kisebb hibával rendelkezik (1 perc alatt a hiba kisebb, mint 1 méter). A GPS rendszer pozíció adatai az un. Earth Centered Earth Fixed (ECEF) koordináta-rendszerben vannak értelmezve. Ennek a koordináta-rendszernek a középpontja a Föld geometriai középpontja. A Z tengely megegyezik a Föld 4

forgástengelyével és a pozitív irány észak fele van értelmezve. Az X tengely merőleges a Z tengelyre és a Föld 0-ik hosszúsági körének síkjában van. Az ECEF koordinátákból számíthatóak a térképészetben szokásosan használt szélességi, hosszúsági és magassági adatok. Kétfajta szélességi, hosszúsági, magassági információ létezik. Az első a geocentrikus nézőpontot alkalmazza. A szélességi érték: atan2(ZECEF , XECEF ) és a hosszúsági: atan2(YECEF , XECEF ). A második értelmezés a szélesebb körben használt geodetikus értelmezés (WGS84 konvenció). Ez figyelembe veszi a Föld lapultságát. Ezek az adatok szerepelnek a térképeken is. A NED (North-East-Down) koordináta-rendszer definíciójából következik, hogy az itt értelmezett szögekből közvetlenül számítható az ECEF-NED transzformáció. A NED koordináta-rendszer Z tengelye jó közelítéssel a gravitációs vektorral egy irányba mutat. Az X tengely pedig mindig Észak felé mutat. A 2.1.ábra a koordináta-rendszereket összefoglalja össze. A NED keret origója a jármű (vevő) és a forgásellipszoid között a legrövidebb távolságot adó pont a forgásellipszoidon. A North-East sík a forgásellipszoid érintősíkja, Down a forgásellipszoid belsejébe mutató merőleges.

2.1. ábra. ECEF és NED koordináta-rendszerek A forgatás az ECEF és a NED koordináta-rendszerek között a következő képlettel írható le:

5

RECEF,N ED



−Sφ Cλ =  −Sλ −Cφ Cλ

−Sφ Sλ Cλ −Cφ Sλ

 Cφ 0  −Sφ

(2.2)

További információ megtalálható [3]-ban.

2.2.

Inerciális mérőegységek működése

Az inerciális mérőegységek kategóriájába a gyorsulás és szögsebességmérő egységek tartoznak. Ezeknek az eszközöknek a közös tulajdonsága, hogy differenciális működésűek, a mozgásállapotnak csak két időpont közötti relatív változását képesek megmérni. Minden esetben a vizsgált objektum koordináta-rendszeréhez rögzített adatokat adják, emiatt nem lehetséges velük a világkoordináta-rendszerben hosszú távon is helyes információt szolgáltatni. Az inerciális szenzorok legnagyobb előnye, hogy általában sokkal gyorsabbak, mint az abszolút információt szolgáltató szenzorok (pl. GPS). További előnyük, hogy rendelkezésre állásukat nem befolyásolja a környezet, ezért a gyakorlatban bármikor elérhetők. Gyorsulásmérő Gyorsulásmérők számos formában előfordulnak a gyakorlatban. A mérési tartományuk terjedhet az 10-20 m/s2 -től akár a néhány ezer m/s2 -is. Méretükben is jelentős változatosságot mutatnak. Léteznek nagy méretű, robusztus kialakítású gyorsulásmérők, de a leggyakrabban a néhány mikrométer nagyságú un. MEMS(Micro Electro Mechanical System) struktúrában kialakított gyorsulásmérő chippekkel találkozunk. Bármilyen legyen is a fizikai kialakítása a gyorsulásmérőnek, a mérési elv mindig ugyanoda vezethető vissza. Az alapelv az, hogy van egy egy-dimenzióban elmozdulni képes m tömegű test, amely a mozgás irányában fel van függesztve egy rugóra. Ha a merőegység a szabad dimenzió irányában gyorsulni kezd, akkor a rugó vagy megnyúl, vagy összenyomódik. A visszaadott érték a rugó nyúltságának a nagysága, mely lineáris kapcsolatban van a szenzort ért gyorsulással. A szenzor szabad dimenziójának irányát nevezzük a szenzor tengelyének. A gyorsulásmérő mérési elvéből következik az, hogy a gravitációs gyorsulást nem méri. Ezért a gravitációs gyorsulás a jelfeldolgozás során külön figyelembe kell venni. Háromdimenziós mérőegység kialakítása úgy képzelhető el, hogy három gyorsulásmérőt elhelyezünk úgy, hogy a szabad dimenziók egymásra merőlegesek legyenek. A továbbiakban csak a MEMS gyorsulásmérőkkel foglalkozunk. Érdemes megvizsgálni, hogy a szenzor által mért adat milyen jellegű hibákat tartalmazhat. A szisztematikus hibaforrások két csoportba oszthatóak. Az első csoport az állandó jellegű hibák. Ezek tipikusan a szenzorok mechanikai felépítéséből 6

származnak, és gyakorlatilag a szenzor teljes élettartama alatt állandónak tekinthetők. A háromdimenziós gyorsulásmérő esetében egy ilyen hibaforrás lehet, hogy az egyes szenzorok tengelyei nem tökéletesen merőlegesek egymásra. Mérési eredmények alapján a szenzor erősítése is tekinthető állandó jellegű hibának. A hibaforrások második csoportja a változó hibák. Ezek nem tekinthetők állandónak még egy rövid ideig sem, szerencsére egyéb mérhető mennyiségektől való függésük állandó. Ezen hibák tipikusan ofszet (bias) jellegű hibaként fordulnak elő, melynek a forrása lehet a szenzor tápfeszültségének vagy hőmérsékletének változása. Meg kell említeni még a méréseket terhelő zajt is, mint további nem szisztematikus hibaforrást. Ezeket tipikusan a zaj modelljével jellemezzük és nulla várható értékűnek tekintjük. Szögsebességmérő Hasonlóan a gyorsulásmérőhöz a szögsebességmérő is sok formában fordul elő a gyakorlatban. Leggyakrabban két változattal lehet találkozni. Az első a MEMS kialakítású gyorsulásmérő, melynek jellemzője, hogy olcsó, kis méretű és viszonylag pontatlan. A másik típus az optikai szögsebességmérő, mely jellemzően nagyon drága, a MEMS-hez képest nagy méretű és rendkívül pontos. A továbbiakban itt is a MEMS szenzorokkal foglalkozunk. Mechanikai kialakításban számos változattal találkozunk, de az alapelv itt is mindig ugyanarra vezethető vissza. Adott egy m tömegű test, melyet a sík egy irányában ismert frekvenciával rezegtetünk. Egy síkban rezgő tömeg centripetális és Coriolis jellegű viselkedése, egy ugyanebben a síkban körmozgást végző, pontszerű tömeg viselkedésével analóg módon kezelhető. Ahhoz, hogy a rezgés síkja konstans szögsebességgel forogjon, a szögsebességgel arányos nyomatékra van szükség. Ezt a nyomatékot egy torziós rugó segítségével elfordulássá alakítjuk és ezt méri a szenzor. A szögsebességmérő esetében is célszerű végigtekinteni a hibaforrásokat. Itt is beszélhetünk állandó és változó hibaforrásokról. Az egyes tengelyek nem merőleges volta és az erősítés hiba itt is állandónak tekinthető. Egy érdekes hibaforrás a szögsebességmérővel kapcsolatban a következő. Ha a szögsebességmérőt különböző orientációba állítjuk, akkor a gyorsulás okozta erő is különböző irányból fog hatni rá. Vagyis ennek az erőnek az m tömegű test elmozdulási irányába mutató komponense is különböző lesz. Ez azt jelenti, hogy különböző orientációkban azonos szögsebesség mellett a mérőtest különböző mértékben mozdul el, ami egy ofszet jellegű hibát eredményez. Ez a hibaforrás kezelhető akkor, ha pontosan ismerjük a gyorsulás irányát. Mivel a szükséges kompenzálás nagysága állandónak tekinthető, így a gyorsulás hatása a szögsebességmérőre is az állandó jellegű hibák közé sorolandó. A változó hibaforrások közé itt is a tápfeszültség és a hőmérsékletváltozás okozta ofszet hibákat soroljuk. A hibaforrások kompenzálására kalibrációs algoritmusokat használunk. Ezek az algoritmusok képesek méréssel meghatározni az egyes hibaforrások nagyságát és a hibák eltüntetésére is adnak módszereket. A mérés során használt 7

kalibrációs algoritmus leírása megtalálható [2].

2.3.

Állapotbecslés Kalman-szűrővel

A vizsgált lineáris rendszer és az állapotbecslő kapcsolata a 2.2. ábrán látható.

u q

 x ˆ

-

x˙ = Ax + Bu y = Cx

y q

-

 x ˆ˙ = F x ˆ + Gy + Hu

2.2. ábra. A rendszer és az állapotbecslő Az állapotbecslő algoritmusának meghatározására számos módszer ismert. Amennyiben ismert zajmodelleket tételezünk fel, akkor az optimális megoldást a Kalman-szűrő adja (a becslési hiba várható értéke nulla, kovariancia mátrixa a legkisebb). A Kalman-szűrő felírható folytonos és diszkrét időben is. Mivel a mérés során a mérési adatok diszkrét mérési időpontokban állnak rendelkezésre, ezért itt a diszkrétidejű Kalman-szűrő kerül bemutatásra. Feltételezzük, hogy a k-ik időpontban adott a diszkrét idejű rendszer modelljének Ak , Bk , Ck mátrixai. A mérés során használt rendszermodellek a következő fejezetben kerülnek bemutatásra. A diszkrétidejű Kalman-szűrő a következő rendszerre értelmezhető: xk+1 = Ak xk + Bk uk + vk yk = Ck xk + zk

(2.3) (2.4)

ahol vk ∈ N (0, Rv ) és zk ∈ N (0, Rz ) fehérzajok, ahol az N (µ, Σ) a µ várható értékű, Σ szórású normális eloszlás. Rv és Rz korrelációs mátrixok definíció szerint a következőképpen értelmezettek: Rv = E(vv T ) T

Rz = E(zz )

(2.5) (2.6) (2.7)

ahol az E(x) jelöli az x valószínűségi változó várható értékének képzését. Amennyiben a mátrixok adottak, akkor a Kalman-szűrő algoritmusa két jól elkülöníthető lépésben értelmezhető, az algoritmus levezetése megtalálható [1]-ben:

8

• Predikció: (2.8)

x ¯k = Ak x ˆk−1 + Buk Mk =

Ak Σk−1 ATk

(2.9)

+ Rv k

Gk = Mk CkT Ck Mk CkT + Rz k Σk = Mk − Gk Ck Mk


−1

(2.10) (2.11) (2.12)

• Frissítés: x ˆk = x ¯k + Gk (yk − Ck x ¯k )

(2.13)

Az algoritmusban használt Σk a következőképpen van definiálva: Σk = E((xk − x ˆk )(xk − x ˆk )T ))

(2.14)

Az algoritmus változói közül x0 és Σ0 inicializálandó. A (2.14) képlet alapján k = 0 helyettesítéssel Σ0 becsülhető. A gyakorlatban a Σ0 = Rv 0 közelítéssel lehet élni. Az x0 a kezdeti állapot, amit a vizsgált rendszer analízise alapján lehet meghatározni. A Kalman-szűrő alkalmazható szenzorfúzió megvalósítására is. Ez abban az esetben szükséges, ha a mérési információink két különböző forrásból állnak rendelkezésre, akár különböző mintavételezési idővel. Legegyszerűbb megvalósítás az, ha a rendszermodellt úgy vesszük fel, hogy az egyik szenzor információja legyen az u bemenő jel és a második szenzor adata pedig az y kimenő jel. További elvárás, hogy az y jel mintavételezési frekvenciája ne legyen nagyobb, mint az u jel mintavételi frekvenciája. Ekkor a szenzorfúzió megvalósítható a korábban bemutatott Kalman-szűrő algoritmussal, azzal a kiegészítéssel, hogy ha nincs új információ az y csatornán, akkor (2.13) helyett x ˆk = x ¯k alkalmazandó, azaz formálisan Gk = 0.

2.4.

A méréshez használt rendszermodellek

A rendszermodellekben használt koordináta-rendszerek a 2.3. ábrán láthatók. Orientációs modell A mérés során az egyszerűség kedvéért azzal a feltételezéssel élünk, hogy az autó vízszintes terepen haladt. Vagyis az X és Y tengely körüli orientáció nullának tekinthető. A Z tengely körüli elfordulást jelöljük ψ-vel. Ekkor az orientáció és a mért szögsebesség között a következő összefüggés áll fenn: ψ˙ = ωZ − ωb,Z + ωn,Z ω˙ b,Z = ωb,n,Z 9

(2.15) (2.16)

X Észak

X

X

p s KS Y

KA p

Y

Y

KW(NED)

Kelet

2.3. ábra. koordináta-rendszerek, Kw a világkoordináta-rendszer(NED), KA a járműhöz kapcsolt keret és KS az a koordináta-rendszer, melyben az IMU a mérési eredményeket adja vissza ahol ω a mért szögsebességet jelöli, a b index a szenzor biasére, míg az n index a szenzor zajára utal. A zaj előjele a nulla várható érték miatt tetszőleges lehet. Definiáljuk a rendszer állapotvektorait a következő formában:   x1 x1 = ψ, x2 = ωb,Z , x = x2 u = ωZ w1 = ωn,Z , w2 = ωb,n,Z , v =



w1 w2



Írjuk át az így kapott folytonosidejű rendszert diszkrét időbe elsőrendű Euler közelítéssel, akkor megkapjuk a rendszer a kívánt (2.3) alakú formában.

xk+1 =



1 0

−T 1







   T ωn,Z,k ωZ,k + ωb,n,Z,k T       T T 1 −T xk + uk + wk = 0 T 0 1 {z } | {z } | {z } |

ψk ωb k

+

T 0



A

B

(2.17)

vk

Az összefüggés a valódi és a mért orientáció között: ψm = ψ + ψn

10

(2.18)

Az y = ψm és z = ψn választással: yk = [1 0] xk + zk | {z }

(2.19)

C

Megjegyzés A (2.15)-ban úgy definiáltuk az orientációt, mint a szögsebesség integrálját. Ez azt jelenti, hogy az orientáció a [−∞; ∞] tartományra képződik le. A GPS mérések alapján az orientációt a [0◦ ; 360◦ ) tartományban tudjuk meghatározni. A két érték különbségét a Kalman-szűrő a (2.13)-ben képzi. A két tartomány közti ellentmondást kezelni kell ahhoz, hogy ne kapjunk hibás eredményt. Több lehetőség kínálkozik: 1. A GPS adatokat transzformáljuk a [−∞; ∞) tartományra. Ehhez azonban detektálni kell, hogy mikor és milyen irányba lépünk ki a tartományból. Ez a feltétel a gyakorlatban zajos adatoknál csak nagy nehézségekkel valósítható meg. 2. Az integrált szögsebességet kényszerítjük a [0◦ ; 360◦ ) tartományra a Kalman-szűrő belsejében. Ehhez x ¯ állandó vizsgálatára van szükség. A mérés során a problémát a két módszer egyikével kezelni kell. Pozíció modell A pozíció becslése során is feltételezzük a vízszintes mozgást, vagyis csak az XY síkot vizsgáljuk. További egyszerűsítés érdekéden feltételezzük, hogy az IMU koordináta-rendszere megegyezik az autó koordináta-rendszerével. Ugyanis ellenkező esetben az autó gyorsulására hatással lenne az autó szöggyorsulása is. Ezáltal elkerüljük, a szöggyorsulás becslésének nehézségeit. A pozíció és a gyorsulás között a következő összefüggés írható fel: ν˙ = Rot(ψ)am − ab + an a˙ b = ab,n p˙ = ν + νn pm = p + pn

(2.20) (2.21) (2.22) (2.23)

ahol am a mért gyorsulás, ν az autó sebességvektora az inerciarendszer (közelítőleg a NED) bázisában kifejezve, p az autó becsült pozíciója, pm az autó GPS által mért pozíciója a b és n indexek továbbra is a bias-ra és a zajra utalnak. Rot(ψ) a Z tengely körüli forgatás mátrixa:   Cψ −Sψ Rot(ψ) = (2.24) Sψ Cψ

11

Az állapotválasztás legyen a következő:     an ν x =  ab  , w =  ab,n  , u = Rot(ψ)am , y = pm , z = pn p νn Euler közelítést használva a diszkrétidejű modell:     I −T · I 0 T ·I I 0  xk +  0  uk + T wk xk+1 =  0 |{z} T ·I 0 I 0 vk | | {z } {z } A

(2.25)

B

yk = [0 0 I] xk + zk | {z } C

ahol I az 2x2-es egységmátrix.

12

(2.26)

3. fejezet

A mérés menete A mérési feladatok a mérés során használt adatok specifikációja után következnek.

3.1.

Mérés során használt adatformátumok

A kiindulási adatok a start_data.mat fájlban találhatók. Ez tartalmazza a GPS és IMU mérési adatait. A két adathalmaz két külön tömbben található. A tömbök minden egyes oszlopa egy-egy különböző mérési időponthoz tartozik. A GPS adat 4 sort tartalmaz. Az első sorban a GPS idő található. A másodiktól a negyedik sorokban a mért pozíció (X,Y,Z) ECEF koordináta-rendszerben. Az IMU adathalmaz 7 sorból áll. Az első sor a GPS időhöz szinkronizált idő. Figyelem! A szinkronizáció másodpercenként történik, két szinkronizáció között órajelcsúszás léphet fel. A másodiktól a negyedik sorig a gyorsulásmérő adatai vannak eltárolva (X,Y,Z sorrendben). A mértékegysége m/s2 -ben értendő. Az utolsó három sor a szögsebességmérő adatait (ϕ, ϑ, ψ sorrendben) tartalmazza, mértékegysége f ok/s. Az inerciális adatok szenzor koordináta-rendszerben vannak megadva. A mérés során azzal a közelítéssel élünk, hogy a szenzor koordináta-rendszer megegyezik az autó koordináta-rendszerével. A mérés során segítségként rendelkezésre áll a Kalman-szűrőt megvalósító függvény, melynek a paraméterezése a következő: [x,S]=kalman_filter(A,B,C,Rv,Rz,u,y,x_0,S_0,new_value): • A,B,C: A becsülendő diszkrét idejű rendszer mátrixai. • Rv: Az állapotzaj kovariancia mátrixa. • Rz: A mérési zaj kovariancia mátrixa. • u: A rendszer bemenetén mért adatok. • y: A rendszer kimenetén mért adatok. • x_0: Az állapotbecslő kezdő állapota. • S_0: A kezdőállapot zajának kovariancia mátrixa.

13

• new_value: Új kimeneti adatot jelző tömb (ahol 1-es áll, ott érkezett új adat) • S: Az utoljára számított Σ mátrix. • x: A becsült állapotokat tartalmazó tömb. A függvénnyel kapcsolatos elvárások: Az u, y, new_value változók tartalmazzanak azonos számú oszlopot. Megjegyzés: A függvény egy végrehajtása alatt azonos A,B,C,Rv,Rz mátrixokkal számol. Viszont a visszaadott értékek között szerepel az aktuális Σ, ezért lehetőség van a függvény rekurzív hívására és a hívások között a rendszermátrixok és kovarianciamátrixok megváltoztatására.

3.2.

Mérési feladatok

1. feladat - GPS adatok transzformációja NED koordináta-rendszerbe Transzformálja az ECEF koordináta-rendszerben adott mért pozíció adatokat NED koordináta-rendszerbe. Segítség: a mérés a 18.9682◦ hosszúsági, 47.4512◦ szélességi fokon történt. Ábrázolja a kapott eredményeket. 2. feladat - Sebesség, orientáció, gyorsulás számítás A pozíció információból számítsa ki minden időpillanatra a sebesség és a gyorsulás vektorát. A sebesség vektorból számítsa ki és ábrázolja a sebesség nagyságát és az északi iránnyal bezárt szögét. Az orientáció változzon a jobb-forgásszabály szerint. A számítások során elég csak két dimenzióban, a NED koordinátarendszer X-Y síkjában dolgozni. 3. feladat - IMU, GPS szinkronizáció Szinkronizálja a GPS és IMU adatait az időinformációk alapján. Vigyázat, a mérések nem feltétlenül azonos időpillanatban kezdődtek. Hozzon létre egy új vektort, ami megegyezik a szinkronizált adathalmaz hosszával és 1-est tartalmaz, ha az adott pillanatban új adat érkezett a GPS-től, különben legyen 0. 4. feladat - Kalman-szűrő készítése orientáció becslésre Hajtson végre Kalman-szűrést az orientáció információra. Használja a GPS adatokból számított orientációt és a szögsebességmérő adatainak Z komponensét. Ábrázolja az eredményeket. 5. feladat - Kalman-szűrő készítése pozíció becslésre Az orientáció információ alapján határozza meg minden időpillanatra a forgatást NED koordináta-rendszer és az autó koordináta-rendszere között. Transzformálja a gyorsulást NED koordináta-rendszerbe. A GPS pozíció és a transzformált gyorsulás alapján végezzen Kalman-szűrést a pozíció meghatározásához. Ábrázolja az eredményeket. 14

Irodalomjegyzék [1] Lantos Béla, Irányítási Rendszerek Elmélete és Tervezése II., Akadémiai Kiadó, 2003. ISBN 963 05 7922 7 [2] Kis László, Lantos Béla, Szenzorfúzió alkalmazása beltéri autonóm négyrotoros helikopteren Hadmérnök 4. évf. 4. szám pp. 191-204. (2009)(http://mycite.omikk.bme.hu/doc/81115.pdf) [3] Grewal,Weill,Andrews, Global Positioning Systems, Inertial Navigation and Integration, Wiley, 2007, ISBN 978-0-470-04190-1

15