A Kalman filtering simulation

Transcription

1 A Klmn filering simulion The performnce of Klmn filering hs been esed on he bsis of wo differen dynmicl models, ssuming eiher moion wih consn elociy or wih consn ccelerion. The former is epeced o beer predic he rjecory when he moion is long srigh line, while he ler should work beer in cse of winding ph. Model A: consn elociy Consider one-dimensionl cse. A moion wih consn elociy is ruled by he following lw: ) cons ( where () is he posiion of he body ime, is he sring posiion of he sysem nd he consn elociy. Considering discree sysem, he dynmicl model becomes where is he ime spn beween wo epochs. For he ske of simpliciy, ssume so h In oher words, defining he se ecor of he discree sysem s X he rnsiion mri resuls T so h X T X Gien he iniil se X (which howeer hs o be modelled s rndom rible), posiions nd elociies eery ime re sricly deermined by his dynmicl model. In order o inroduce higher leel of fleibiliy, i is ssume h he elociy cn slighly chnge from one epoch o noher. This is obined by dding, epoch by epoch, whie noise o he elociy. All in ll, he dynmics of he sysem cn be represened s follows X X T X ε X ε

2 where X is he men lue of he iniil se-ecor nd he model error ε cn be sochsiclly described s { } E ε, { } ε E ε ε ' δ C ' Noe h for > he noise cs only on he elociy, i.e. σ ε ε C > while for een he posiion hs o rndomly modelled, i.e. C σ σ ε ε As for he obserions y, ypiclly only posiions re ilble (bsed on GPS/GLONASS sysem), i.e. y ν or in mri noion Y H X ν where he design mri H (from he se o he oupu of he sysem) is gien by H nd he obserion noise ν cn be sochsiclly described s { } ν E ν, E { ν ν } δ C ' ' Since he obserion process is no reled o he eoluion of he sysem, he wo error ypes cn be considered independen, i.e. E { ε ν } ' I could be ineresing o underline h he sysem dynmics cn be epressed only in erms of posiions, in fc ε From he firs equion, i holds

3 nd similrly Using he second equion of he sysem, he dynmics cn be wrien s ε The preious model cn be esily generlized o he wo-dimensionl cse (e.g. moion on plne), wih se ecor X,,,, rnsiion mri T nd design mri H Model B: consn ccelerion Consider one-dimensionl cse. Repeing he sme resoning of he preious model, bu ssuming now h he ccelerion is consn (pr from n dded whie noise), he sysem dynmics cn be modelled s follows ε or in mri noion ε ε X X T X X where he se ecor is defined s

4 X nd he rnsiion mri is gien by T Only posiions re supposed o be mesured, i.e. Y H X ν wih design mri H Agin i is possible o epress he dynmics in erms of posiions only. Wih some lgebr, i holds 3 ε ε 3 ( ) ε In he wo-dimensionl cse (e.g. moion on plne), he se ecor becomes X,,,,,,

5 while he rnsiion mri T nd he design mri H re respeciely gien by T H Emple The body is cully moing long srigh line wih consn elociy m/s, m/s. The noise of he posiion obserions hs sndrd deiion of m (see Fig..). In he cse of model A he error dded o he elociy hs sndrd deiion of. m/s, while in he cse of model B he error dded o he ccelerion hs sndrd deiion of. m/s. I is cler h using he Klmn filer bsed on model A he esimed rjecory is closer o srigh line since higher leel of regulriy is imposed (see Fig..). Consequenly, fer rnsiion ime of bou 5 seconds, he posiion errors deried from he model A (rms.6 m) re smller hn hose deried from he model B (rms.6 m) (see Fig..3) Fig.: Idel rjecory (in blck) nd posiion obserions (in grey) smpling re of sec.

6 Fig.: Esimed rjecory using Klmn filer wih he dynmicl model A (in blck) nd he dynmicl model B (in grey) error ime [s] Fig.3: Absolue lue of he posiion errors using Klmn filer wih he dynmicl model A (in blck) nd he dynmicl model B (in grey).

7 Emple The body is now moing long circulr rjecory wih rdius R m nd consn ngulr elociy ω.5 rd/s. This mens h in seconds (obserion ime) he body coers bou 3 lps of he circui. The noise of he posiion obserions hs sndrd deiion of.5 m (see Fig..). In he cse of model A he error dded o he elociy hs sndrd deiion of. m/s, while in he cse of model B he error dded o he ccelerion hs sndrd deiion of. m/s. In his emple, fer rnsiion ime due o he fc h he sring poin is no on he circulr ph, he Klmn filer bsed on he model B is ble o follow he curiliner rjecory (where he elociy chnges), while he soluion wih he model A is oo rigid, predicing circulr rjecory wih lrger rdius (see Fig..). This resuls in posiion error wih sysemic bis. The error rms for he model A is of he order of 5 m gins n error rms of. m for he model B (see Fig..3) Fig.: Idel rjecory (in blck) nd posiion obserions (in grey) smpling re of sec.

8 Fig.: Esimed rjecory using Klmn filer wih he dynmicl model A (in blck, solid line) nd he dynmicl model B (in grey, solid line). True rjecory in blck dsh line error ime [s] Fig.3: Absolue lue of he posiion errors using Klmn filer wih he dynmicl model A (in blck) nd he dynmicl model B (in grey).

9 Emple 3 The body is moing long he ph shown in Fig. 3.. The obserion noise hs sndrd deiion of.5 m. In he cse of model A he error dded o he elociy hs sndrd deiion of.5 m/s, while in he cse of model B he error dded o he ccelerion hs sndrd deiion of.5 m/s. This emple emphsizes he pros nd cons of he wo models (see Fig. 3.). In he srigh sreches he soluion A is more regulr, bu in he curiliner secions i runs wy from he rue ph; i needs some ddiionl ime o correc he rjecory when he rod reurns o be srigh. On he oher hnd, he soluion B is more nerous eerywhere, bu i is cpble o follow he rue rjecory een in he curiliner secions. As consequence, fer he iniil rnsiion ime, he error leel become sble in he cse of model B, while i oscilles in he cse of model A, depending on he fc h he body is coering srigh or curiliner secion (noe h wo of he four srigh lines re no long enough o llow he body o come bck on he righ rjecory). The error rms for he model A is. m, while for he model B i is bou. m (see Fig. 3.3). Emple 4 The Emple 3 is repeed long he sme ph nd wih he sme obserions, bu now he error dded o he elociy in he model A hs sndrd deiion of.5 m/s. In oher words, higher model errors re cceped. The corresponding soluion becomes much more recie, following eery chnge of direcion. On he oher hnd, he min dnge of model A is definiiely los, since he rjecory hs he sme regulriy of he one compued by using he model B. Therefore he wo soluions re ery similr (see Fig. 4.), boh wih n error rms of. m (see Fig. 4.) Fig 3.: Idel rjecory (in blck) nd posiion obserions (in grey) smpling re of sec.

10 Fig 3.: Esimed rjecory using Klmn filer wih he dynmicl model A (in blck, solid line) nd he dynmicl model B (in grey, solid line). True rjecory in blck dsh line error ime [s] Fig 3.3: Absolue lue of he posiion errors using Klmn filer wih he dynmicl model A (in blck) nd he dynmicl model B (in grey).

11 Fig 4.: Esimed rjecory using Klmn filer wih he dynmicl model A (in blck, solid line) nd he dynmicl model B (in grey, solid line). True rjecory in blck dsh line error [s] ime [s] Fig 4.: Absolue lue of he posiion errors using Klmn filer wih he dynmicl model A (in blck) nd he dynmicl model B (in grey).

12 Emple 5 In his eperimen he body is kep sill he sme locion. Is posiion is mesured eery second wih n obserion noise of.5 m (see Fig. 5.). When Klmn filering is pplied, he esimed posiion howeer chnges in ime nd he resuling rjecory winds round he rue locion boh in he cse of model A nd of model B (see Fig 5.). Noe h, due o he rndomness of he iniil se, he esimed rjecory cn sr fr from he rue posiion nd een moe in he wrong direcion; howeer, fer rnsiion ime, i ends o come bck owrds he rue locion Fig 5.: Body locion (blck cross) nd posiion obserions (in grey) smpling re of sec Fig 5.: Esimed rjecory using Klmn filer. The blck cross indices he rue posiion of he body