Kálmán-szűrés. Korszerű matematikai módszerek a geodéziában

Átírás

1 Kálmánzűré Korzerű matemata módzere a geodézában 4.3..

2 A Kálmánzűré defnícója Olyan algortmu, amely valamely lneár dnamu rendzerben egzat övetezetét tez lehetővé, amely a rejtett Marovmodellhez haonló Bayeféle modell, azonban a rejtett változó állapottere folytono, é mnden rejtett é megfgyelhető változó (gyaran többváltozó normál elozlá. UGYE MINDEN VILÁGOS!?

3 Az aomatu módzer E módzer lényege abban áll, hogy a tételeet defnícóá tezü. A tétel tartalm réze lyenor a defnícó motvácójává vál, az algebrtá tudományu tentélyét növelendő, többnyre ezt elhagyjá egy nem motvált defnícót megérten nem lehet az aomatu módzer[ne] zámtalan előnye van, haonlóan ahhoz, hogy a lopána zámtalan előnye van a tzteége munával zemben (V. I. Arnold, oroz matematu

4 A Kálmánzűrő Rudolf Eml Kalman publálja az elméletet (96 Elő nevezete alalmazáa: Apolló űrhajó fedélzet zámítógépe (Armtrong Navgácó é mérnö alalmazáo Szenzor adato fzója (radar, lézerzenner, amera, INS, GNSS vevő, ootelefono, zámítógépe, játéo, tb. elmaradhatatlan réze őzde előrejelzé, dőor elemzé, függvény appromácó Rudolf Eml Kalman (Kálmán Rudolf (93 magyar zármazá vllamomérnö

5 A publácó R.E. Kalman: A New Approach to Lnear Flterng and Predcton Problem, ranacton of the ASME Journal of Bac Engneerng, 8 (Sere D: 3545

6 Meg lehet érten? a homályona tűnő matemata jelölé ellenére, a Kálmánzűré elgondoláa egézen egyzerű é özvetlen: dőben változó rendzer, amelyne állapotát leíró változóat ( nem (teljeen merjü ezt az állapotot zeretnén mnden lépében a lehető legjobban megmern rendelezün folyamato méréeel (y, melye apcolatba hozható a rendzer állapotával (

7 A Kálmán zűrő jellemző lépéenént zámítá, ca az előző é a jelenleg lépé függvényében reurzív a apcolato lneára (állapot változáa, méréeállapot apcolata ztochaztu (méré hbá, állapot é a hbája valózínűég változó legebb hbáj optmál beclé (ca lneár eetben bzonyítható

8 Reurzív beclé példa egyváltozó rendzer állapotát mérjü egymá után dőpontoban mérée:,, n, a legjobb állapot beclé az átlag: m å n n j méréün van: n, a övetező beclé: m n n n æ ç n n n èn n å å n n ö ø

9 Erőíté, j nformácó a frített beclé orrecóval zámítható: n mn mn n mn K n n n K az erőíté tényező ( nyereég ez dönt el, meora lez a orrecó n µ n ( m az j nformácó ( nnovácó ha nnc j nformácó, nem változ az előző beclé n

10 Reurzív átlag beclé

11 Különböző nformácó optmál felhaználáa egy távolágot ét ülönböző ezözzel mértün (mérőzalag, lézer távmérő mérée:,, zóráo: σ, σ az optmál beclét ereü re ez a lyozott átlag: ˆ w ( w M az optmál w ly?

12 Optmál ly meghatározáa mvel, normál elozlá, a lneár ombnácóju normál elozlá, melyne zóráa ˆ w ( az optmál w ly enne a fejezéne a mnmuma, ahol a w zernt dervált zéru lez w az optmál ly tehát w ˆ w ( opt w opt w opt

13 Optmál beclé é zóráa az optmál beclé re: az optmál beclé zóráa: az optmál beclé mnmalzálja az, től vett lyozott távolágözeget: ˆ ˆ û ù ë é ø ö ç ç è æ ø ö ç ç è æ arg mn ˆ

14 Optmál beclé az optmál beclé re zélőérté: ezt megoldva valóban ˆ ˆ ( ˆ ( û ù ë é ø ö ç ç è æ ø ö ç ç è æ

15 Reurzív optmál beclé Két lépében mérün, előzör et, azután t. Az elő lépében ˆ ˆ A máod lépében frítjü a beclét ˆ ˆ ˆ ˆ ˆ ˆ æ ˆ ( ö ø ç ˆ ˆ è K erőíté (nyereég j nformácó (nnovácó

16 Reurzív optmál beclé a beclé egyenlete ( ˆ ˆ ˆ ˆ K ( K ˆ K ˆ ˆ beclé hbája (zórá K: erőíté merő az egyenlet alaja? n mn K n ( m m n

17 Közvetett mérée M van, ha et nem tudju özvetlenül mérn (rejtett változó, rendzerállapot? Geodézában ez jól mert: özvetett mérée egyenlítée (II. egyenlíté coport, paramétere egyenlíté, GauMarov modell Lneár (özvetítő egyenlet az y méré é rendzerállapot özött (z a méré hba, zaj : y C a méré zajt zéru átlag ( é az S z ovaranca mátr jellemz (E[ ] a várható érté: z z z [ ] ( z z( z z S E

18 Közvetett állapotbeclé együ fel, hogy egyzerre N db. y méréel rendelezün Mndegy méré ülön C é S z apcolatban lehet a rendzer állapotával: y C Eor az optmál beclére a z javítáo lyozott négyzetözege mnmál lez: ˆ arg mn N å ( y C z S z ( y C

19 Közvetett állapotbeclé A orább (távolágméré példában a mérée özvetlenül az állaportra vonatozta, tehát y volt é C. Így am megegyez a orábbval, ha N : å N z S ( ( arg mn ˆ û ù ë é ø ö ç ç è æ ø ö ç ç è æ arg mn ˆ

20 Közvetett állapotbeclé Ha van nformácón a ezdet rendzerállapotról é anna ovaranca mátráról (, ezt a ad dőpont méréént lehet ezeln: Ha a mérée nem egyzerre, hanem egymá után érezne, a ad dőpontban: Enne a megoldáa a tatu Kálmánzűrő û ù ë é å N z S P ( ( (ˆ ˆ (ˆ arg mn ˆ ˆ ˆP [ ] ( ( (ˆ ˆ argmn (ˆ ˆ C y S C y P z

21 Statu Kálmánzűrő példa GPS mérée feldolgozáa rejtett állapot ( paraméter: vevő oordnátája é az órájána gazítatlanága mérée (y: műholdvevő (áltávolágo öze méré rendelezére áll: hagyományo LKN egyenlíté epochánént érező mérée: reurzív egyenlíté, tatu Kálmánzűrő

22 Helymeghatározá egyenlete mért (áltávolágo: PR ( X ( Y y ( Z z Cr.. n jelölée: X, Y, Z az ed műhold mert térbel derézögű oordnátá PR az ed műholdra végzett távolágméré eredménye, y, z a vevő meretlen térbel derézögű oordnátá Cr a vevő órahbájána hatáa n az ézlelt műholda záma

23 Lnearzált méré modell mért (áltávolágo: jelölée:, y, z a vevő előzete térbel derézögű oordnátá ΔX, ΔY, ΔZ az előzete oordnátá javítáa a geometra távolág özelítő értée n Cr Z z Z Y y Y X X PR.. D D D r r r r ( ( ( z Z y Y X r

24 Megoldandó egyenletrendzer û ù ë é D D D û ù ë é û ù ë é Cr Z Y X z Z y Y X z Z y Y X z Z y Y X PR PR PR n n n n n n n n r r r r r r r r r r r r M M M M M jelöljü az A alamátr ed orát a h vetorral, az állapotvetort az vetorral

25 Reurzív egyenlíté aác Bence, P h K P P h l K R h P h h P K,,, ( ( A zámítá özefüggée a Kálmánzűrő egyenlete (lád éőbb: j nformácó (nnovácó K erőíté (nyereég Az terácó utoló lépée után a P mátr a normálmátr nverzévé vál

26 Dnamu rendzer Sozor van az, hogy a rendzer állapota változ, vagy függ az dőtől pl. GNSS mozgávzgálat hálózat pontja mozogna meterége hold helyzete, ebeége változ jármű helyzete, ebeége változ járműforgalom folyamatoan változ együ fel, hogy lneár a változát leíró egyenlet (állapotegyenlet: A Bu A folyamat zajt, w t a Q (vagy ha változ, Q ovaranca mátrzal jellemezzü, u pedg egy mert nput vetor w

27 Dnamu rendzer A rendzer állapota változ a é dőponto özött. Ezért a orább (tatu egyenlet elő tagját meg ell változtatn az előrejelzett állapotváltozána megfelelően: ˆ argmn (ˆ Az előrejelzett zámítható: [ ˆ (ˆ ( ( ] P y C S y C z ~ ~ rendzerállapot az állapot egyenletből ˆ A Bu Ha az u nputot töéleteen merjü, aor az előrejelzé hbája ~ (ˆ A w

28 Optmál beclé ovaranca mátra enne megfelelően: ~ ~ P ˆ AP A Q ˆ Pˆ Az egyenletbe beírva ~ é P ~ értéet é helyére, ˆ argmn ( ~ ~ [ ( ~ ( ( ] P y C S y C A mnmumot mét az zernt parcál derváltat zérual egyenlővé téve apju. Az eredmény: ˆ ~ ( ~ K y C Pˆ ~ ( I K C P z

29 Optmál beclé a züége mátro: A bzonyítához alalmazn ell a övetező mátr nverzó lemmát: ~ S PC K z C P C S S ~ ( ( A B C B A B B A A BCB A

30 Rendzerállapot ovaranca mátra A rendzerállapot Pˆ ovaranca mátra az ˆ állapotbeclé bzonytalanágna mértée. Ezt az ˆ ellpzod mutatja: ˆ P (ˆ ( Pˆ US V SVD felbontá

31 Rendzerállapot onvergenca Ahogy tel az dő, a rendzer állapotána beclée egyre bzonytalanabbá vál ezt az egyre növevő hbaellpzod jelz Ezzel zemben, a mérée cöent az állapot beclé bzonytalanágát ezt a zugorodó hbaellpzod jelz A hbaellpzod tengelyhoza jelz a rendzer állapot bzonytalanágát A beclé onvergencája azt jelent, hogy az ellpzod mnden rányban zugorod

32 Kálmánzűrő egyenlete Állapotegyenlet: A Bu w Átvtel egyenlet: y C z A, B, C mátro; az dőváltozó (nde a rendzer állapota (özvetlenül nem mérhető u a rendzer mert bemenete y a mért menete w é z a zaj w állapot (folyamat zaj zt a méré zaj

33 Kálmánzűrő egyenlete Folyamat zaj ovaranca: Méré zaj ovaranca: Kálmánzűrő algortmua: ( w w w S E ( z z z S E z w z A CP S AP C S A AP P C y K Bu A S CP C AP C K ˆ ( ˆ ( ˆ (

34 Állapot optmál beclée K mátr: Kálmánféle nyereégmátr P mátr: beclé hba ovaranca mátr állapot terjedée orrecó ˆ ( Aˆ Bu K ( y Cˆ

35 K nyereégmátr Ha nagy a méré zaj, S z nagy lez, tehát K c, é így nem adun nagy htelt az y méréenne a övetező állapot becléénél. Vzont ha c a méré zaj, S z c lez, tehát K nagy, é így több htelt adun a méréenne a övetező állapot becléénél. K AP C ( CP C S z

36 P ovaranca terjedé A ovaranca terjedé özefüggée: P AP A S w AP C S z CP A

37 Kálmánzűré

38 Kálmánzűrő alalmazáa objetumöveté, meterége látá dnamu helymeghatározá gazdaágtan, maroöonóma nercál navgácó pályameghatározá radar öveté GNSS zezmológa bezédjavítá dőjárá előrejelzé navgácó rendzere 3D modellezé

39 Példa: járműnavgácó egyene ton haladó jármű állapot vetor: [p, v] helyzet, ebeég nput vetor u: vezérlő gyoruláo ebeég változá: v v u helyzet változá: p p v ½ u állapot átvtel é méré egyenlet v ~ p~ [ ] z y w u û ù ë é û ù ë é /

40 Példa: járműnavgácó helyzet bzonytalanág: méter véletlenzerű gyoruláo zóráa,5 m/

41 Euler zmulácó functon alman(duraton, dt ## functon alman(duraton, dt Kalman flter mulaton ## duraton length of mulaton (econd ## dt tep ze (econd meanoe ; ## poton meaurement noe (meter accelnoe.5; ## acceleraton noe (meter/ec^ a [ dt; ]; ## tranton matr c [ ]; ## meaurement matr [; ]; ## ntal tate vector hat ; ## ntal tate etmate Q accelnoe^ * [dt^4/4 dt^3/; dt^3/ dt^]; ## proce noe covarance P Q; ## ntal etmaton covarance R meanoe^; ## meaurement error covarance ## et up the ze of the nnovaton vector Inn zero(ze(r; po []; ## true poton array pohat []; ## etmated poton array pomea []; ## meaured poton array

42 Euler zmulácó Counter ; for t to duraton tep dt; Counter Counter ; ## Smulate the proce ProceNoe accelnoe * [(dt^/*normal(,; dt*normal(,]; a. ProceNoe; ## Smulate the meaurement MeaNoe meanoe * normal(,; z c. MeaNoe; ## Innovaton Inn z c. hat; ## Covarance of Innovaton c. P. c' R; ## Gan matr K a. P. c'. nv(; ## State etmate hat a. hat K. Inn; ## Covarance of predcton error P a. P. a' Q a. P. c'. nv(. c. P. a'; ## Save ome parameter n vector for plottng later po po_(; pomea pomea_z; pohat pohat_hat(; end; ## Plot the reult

43 Eredménye pro : valód helyzet é : mért helyzet zöld : a Kálmánzűrő által becült helyzet