PRIMENA UNSCENTED TRANSFORMACIJE U ESTIMACIJI STANJA NELINEARNIH

STOHASTIČKIH SISTEMA Igor Jovandić, Branko Kovačević, Elektrotehnički fakultet u Beogradu

Sadržaj – U ovom radu predstavljena je unscented transformacija – aproksimativni metod za izračunavanje momenata raspodele slučajnog vektora generisanog propagacijom kroz nelinearnu funkciju. Pokazano je da se transformacija može primeniti za rešavanje problema estimacije stanja nelinearnog sistema i dato je poređenje takvog algoritma sa proširenim Kalmanovim filtrom. 1. UVOD Procena stanja nelinearnih stohastičkih sistema predstavlja problem koji se često sreće u praktičnim primenama rezultata teorije estimacije. Iako postoji veliki broj algoritama estimacije koji imaju za cilj procenu stanja nelinearnog sistema, po kriterijumu opšte primenljivosti treba izdvojiti prošireni Kalmanov filtar (EKF), kao numerički jednostavan algoritam, konceptualno sličan poznatom Kalmanovom filtru. EKF je suboptimalan metod estimacije stanja nelinearnih sistema zasnovan na konceptu linearizacije. U ovom radu predstavljen je aproksimativni metod za izračunavanje momenata raspodele slučajnog vektora generisanog propagacijom kroz nelinearnu funkciju, koji se zove unscented transformacija (UT). Efikasnost UT metoda ilustrovana je na primeru konverzije koordinata iz polarnog u Dekartov koordinatni sistem. Pokazano je kako se UT može iskoristiti u cilju rešavanja problema estimacije stanja nelinearnog sistema, što je ilustrovano simulacijom na primeru reentrance problema. 2. ESTIMACIJA STANJA NELINEARNIH SISTEMA Razmatra se problem estimacije vektora stanja nelinearnog stohastičkog sistema opisanog sledećim diskretnim modelom u prostoru stanja:

)())(,()1( kwkxkfkx +=+ (1) )())(,()( kvkxkhky += (2)

gde je x – n-dimenzioni vektor stanja sistema, y – m-dimenzioni vektor izlaza sistema, w – n-dimenzioni vektor šuma procesa, a v – m-dimenzioni vektor šuma merenja. Ako se uslovna funkcija gustine verovatnoće (nadalje fgv) vektora

stanja u k-tom trenutku odabiranja označi sa ,

gde označava skup raspoloživih merenja zaključno sa merenjem u k-tom trenutku, tada je procena vektora stanja koja minimizira srednju kvadratnu grešku estimacije (MMSE – Minimum Mean Square Error) upravo uslovno očekivanje uslovne fgv [1]:

( ( ) | )kp x k Y

kk yyyY ,...,, 21=

∫Ω

== )()|)(()(|)()(ˆ kdxYkxpkxYkxEkx kk (3)

gde označava skup svih mogućih vrednoti vektora stanja. Ω Ako je uslovna fgv vektora stanja u k-1-vom trenutku poznata, tada se, shodno Bajesovom pravilu, na osnovu modela dinamike (jednačina (1)) i modela merenja (jednačina (2)) može izvršiti proračun uslovne fgv vektora stanja u k-tom trenutku. Takav rekurzivni postupak propagacije uslovne fgv vektora stanja opisan je sledećim skupom jednačina [1]:

( ) ( ) ( )1 1( ) | ( ) | ( 1) ( 1) | ( 1)k kp x k Y p x k x k p x k Y dx kΩ

− −= − −∫ − (4)

( ) ( ) ( )1( ) | ( )( ) | ( ) |

( )k kp y k x k

p x k Y p x k Ykη

−= (5)

gde je ( )1( ) ( ) | kk p y k Yη −= faktor normiranja koji obezbeđuje da fgv definisana jednačinom (5) ima jedinični integral po prostoru Ω . Jednačine (4) i (5) predstavljaju uzajamno rekurzivni sistem funkcionalnih jednačina, veoma pogodan za formiranje procene vektora stanja. Jednačina (4) zove se jednačina predikcije, a jednačina (5) jednačina korekcije (popravke). Ove jednačine predstavljaju polazište svakog rekurzivnog metoda estimacije stanja nelinearnog

sistema. Iako uslovna fgv vektora stanja predstavlja potpun statistički opis slučajnog procesa

( ( ) | )kp x k Y( )x k u

k-tom trenutku odabiranja, njeno izračunavanje opisanim rekurzivnim metodom vrlo je teško (a često i analitički nemoguće) za slučaj proizvoljnog nelinearnog sistema i proizvoljne tipove raspodele šumova w i v. Međutim, u slučaju linearnog sistema i Gausovskih raspodela šumova procesa i merenja, postoji jednostavno, egzaktno rešenje rekurzivne propagacije uslovne fgv vektora stanja i zove se Kalmanov filtar. Pod navedenim pretpostavkama, uslovna fgv vektora stanja data jednačinama (4) i (5) ostaje Gausovska. Pošto je Gausova raspodela u potpunosti opisana poznavanjem prva dva momenta (očekivanje i kovarijaciona matrica) koraci predikcije i korekcije svode se na propagaciju uslovnog očekivanja i kovarijacione matrice. Ako je sistem opisan sledećim linearnim modelom:

( 1) ( ) ( )x k Ax k Gw+ = + k

)

(6) ( ) ( ) ( )y k Hx k v k= + (7)

gde relevantne veličine imaju istu interpretaciju kao i u jednačinama (1) i (2), tada je rešenje problema rekuzivne estimacije vektora stanja dato sledećim jednačinama [1]:

ˆ ˆ( 1 | ) ( |x k k Ax k k+ = (8)

( 1 | ) ( | ) TP k k AP k k A GQGΤ+ = + (9)

( ) 1

( ) ( 1 | ) ( 1 | )T TK k P k k H HP k k H R−

= + + + (10) ˆ( 1) ( 1) ( 1 |r k y k Hx k k+ = + − + )

) (11)

ˆ ˆ( 1 | 1) ( 1 | ) ( ) ( 1x k k x k k K k r k+ + = + + + (12)

( )( 1 | 1) ( ) ( 1 |P k k I K k H P k k+ + = − + ) (13) gde je i . Vrednosti P i K determinističke su funkcije vremena i mogu se apriori izračunati za zadate početne uslove

cov ( )Q w k= cov ( )R v k=

0x i . 0P Polazeći od navedenog rezultata rekurzivne estimacije stanja linearnog stohastičkog sistema, može se forumlisati suboptimalan algoritam za rešavanje problema estimacije stanja nelinearnog sistema, koji se zove prošireni Kalmanov filtar (EKF – Extended Kalman filter). Algoritam EKF-a bazira se na linearizaciji modela dinamike (jednačina (1)) i modela merenja (jednačina (2)) u okolini poslednje raspoložive procene vektora stanja [1]. Konkretno, ako je u k-

Zbornik radova 50. Konferencije za ETRAN, Beograd, 6-8. juna 2006, tom I Proc. 50th ETRAN Conference, Belgrade, June 6-8, 2006, Vol. I

235

tom trenutku odabiranja na raspolaganju procena ˆ( | )x k k i kovarijaciona matrica greške te procene , tada su predikcija vektora stanja i kovarijaciona matrica greške predikcije dati sledećim jednačinama:

( | )P k k

(ˆ ˆ( 1 | ) , ( | ))x k k f k x k k+ = (14)

( 1 | ) ( | ) TP k k AP k k A Q+ = + (15)

gde je ˆ( | )x k k

fA

x

∂=∂

Jakobijan funkcije ( )f ⋅ izračunat u

tački ˆ( | )x x k k= . Korak korekcije opisan je jednačinama (10), (12) i (13), pri čemu je :

( )ˆ( 1) ( 1) , ( 1 | )r k y k h k x k k+ = + − + (16)

a ˆ( 1| )x k k

hH

x +

∂=∂

– Jakobijan funkcije izračunat u tački ( )h ⋅

ˆ( 1 | )x x k k= + . EKF algoritam predstavlja samo aproksimaciju MMSE optimalnog rešenja, koje podrazumeva egzaktno izračunavanje uslovnog očekivanja. Naime, optimalna vrednost predikcije iznosi:

( ) ˆ ( 1 | ) ( 1) | , ( ) |OPT

k kx k k E x k Y E f k x k Y+ = + = (17) Pošto u opštem slučaju važi nejednakost

( ) ( ), ( ) | , ( ) |k kE f k x k Y f k E x k Y≠ (18) jasno je da se aproksimacijom realizovanom u EKF algoritmu, u komparaciji sa optimalnim rešenjem, uvodi greška predikcije. Pomenuta aproksimacija zove se linearizacija, jer se svodi na jednostavno odbacivanje članova višeg reda u Tejlorovom razvoju stvarnog uslovnog očekivanja [2]. Opisanim postupkom linearizacije greške se unose i u predikciju merenja (a time i u korekciju stanja), pošto i za model merenja važi nejednakost analogna nejednakosti (18). Isti komentar važi i za kovarijacione matrice grešaka predikcije i korekcije. Algoritam unscented transformacije opisan u ovom radu može se primeniti za rešavanje problema estimacije vektora stanja nelinearnog sistema, a pošto rezultuje greškama manjim nego metod linearizacije, generiše kvalitetnije procene vektora stanja u odnosu na EKF. 3. UNSCENTED TRANSFORMACIJA I UNSCENTED KALMANOV FILTAR Unscented transformacija (nadalje UT) predstavlja aproksimativni metod za izračunavanje momenata raspodele slučajnog vektora generisanog propagacijom kroz nelinearnu funkciju. Ako je poznato očekivanje X slučajnog vektora X i njegova kovarijaciona matrica i ako je slučajan vektor Y definisan sledećom jednačinom

XP

( )Y g X= (19) gde je ( )g ⋅ poznata nelinearna funkcija, postavlja se problem

izračunavanja očekivanja Y i kovarijacione matrice slučajnog vektora Y. Metod linearizacije aproksimira tražene veličine sledećim izrazima [2,3]:

YP

( )LINY g X= (20)

( ) ( )LIN TY g X gP J X P J X= ⋅ ⋅ (21)

gde je ( )gJ X Jakobijan funkcije ( )g ⋅ u tački X . Nasuprot metodu linearizacije, unscented transfromacija ne

aproksimira nelinearnu funkciju, već raspodelu slučajnog vektora X i to konačnim skupom deterministički odabranih tačaka (tzv. sigma tačke) u prostoru vektora X. Skup sigma

tačaka ( )(1) (2), ,..., UTNX X X bira se tako da budu

zadovoljeni sledeći uslovi:

( ) ( )

1

UTNi i

iW X X

==∑ ; ( )( )

1

( ) ( ) ( )UTN

Xi

Ti i iW X X X X P=

− − =∑ (22)

Skalarne veličine predstavljaju težinske faktore svake od sigma tačaka. Sigma tačke biraju se na deterministički način, ali tako da odslikavaju prva dva momenta raspodele slučajnog vektora X, u smislu iskazanom uslovima (22). Svaka sigma tačka preslikava se u prostor vektora Y putem nelinearne funkcije

( )iW

( )g ⋅ :

( )( ) ( )i iY g X= , (23) 1, 2,..., UTi = NKao rezultat dobija se skup preslikanih sigma tačaka

( )(1) (2), ,..., UTNY Y Y . Procene očekivanja i kovarijacione matrice slučajnog vektora Y formiraju se na bazi statistika skupa preslikanih sigma tačaka:

( ) ( )

1

UT

UT

Ni i

iY W Y

== ∑ (24)

( )(( ) ( ) ( )

1

UT TUTY UT

Ni i i

iP W Y Y Y Y

== − −∑ )UT (25)

Težinski koeficijenti mogu biti proizvoljnog znaka, ali moraju zadovoljavati uslov normiranja

( )iW

( )(1) (2) .. 1UTNW W W+ + + = . Jedan izbor sigma tačaka koji zadovoljava uslove (22) je simetričan skup od tačaka, gde je n dimenzija vektora X. Simetričan skup sigma tačaka dat je sledećim jednačinama:

2UTN n=

( )( )X

i

iX X nP= + , ( ) 1

2iW

n= , (26)

( )( )X

i n

iX X nP+ = − , ( ) 1

2i nW

n+ = , (27)

gde je 1, 2,...,i n= , a oznaka ( X inP ) predstavlja i-tu

kolonu matričnog korena kovarijacione matrice skalirane brojem dimenzija vektora X. Matrični koren može se izračunati numerički jednostavnim i stabilnim metodom Cholesky dekompozicije.

XP

Sledeći primer demonstrira efikasnost algoritma unscented transformacije. Razmatra se problem konverzije polarnih u Dekartove koordinate. Slučajne promenljive i r θ predstavljaju polarne koordinate objekta u ravni, a slučajne promenljive x i y njegove Dekartove koordinate. Koordinatni počeci oba sistema se poklapaju; sva rastojanja iskazana su u metrima, a uglovi u radijanima. Veza navedenih slučajnih promenljivih data je sledećom relacijom:

( ) [ cos sin ]TD PX g X r rθ θ= = (28)

gde su [ TD ]X x y= i [ T

PX r ]θ= . Očekivanje

slučajnog vektora PX je [1 / 2 TPX π= ] , a njegova

kovarijaciona matrica . Potrebno je 3 1(10 10 )PP diag − −=

236

izračunati očekivanje i kovarijacionu matricu slučajnog vektora DX . Primenom metoda linearizacije dobija se:

( )( ) [1 / 2] [0 1]TLIN TD PX g X g π= = = (29)

1 3( ) ( ) 10 10( LIN TD g P gP PP J X P J X diag − −= ⋅ ⋅ = ) (30)

Procene očekivanja i kovarijacione matrice po UT metodu su: [0 0.9508]UT T

DX = ; (31) (0.0935 0.0034)UTDP diag=

Pošto je izabran simetričan skup sigma tačaka, svi težinski faktori su jednaki i iznose 1/ . Da bi se proverila valjanost ovih procena, izvršena je MonteCarlo simulacija na skupu od semplova. Generisani su semplovi nezavisnih Gausovih slučajnih promenljivih i

( )iW 4

410MCN =r θ , u skladu

sa očekivanjem PX i kovarijacionom matricom PP , pa su izračunate realizacije slučajnih promeljivih x i y, na osnovu relacije (28). Standardne procene očekivanja i kovarijacione matrice na dobijenom uzorku su:

3[10 0.95]MC TDX −= ; (32) (0.0923 0.0056)MC

DP diag=Na slici 1 krstićima su prikazana očekivanja procenjena metodom linearizacije, metodom UT i MonteCarlo simulacijom, kao i dobijene 1σ konture.

-0.1 -0.05 0 0.05 0.1 0.150.8

0.85

0.9

0.95

1

1.05

1.1

x [m]

y [m

]

LIN

MC UT

Slika 1. Procene očekivanja i 1σ konture dobijene

linearizacijom, UT-om i MC simulacijom

Metod linearizacije očigledno daje pomerenu procenu očekivanja, a procena varijanse slučajne promenljive y iznosi , dok je stvarna vrednost više od 5 puta veća. Ove greške direktna su posledica implicitnog odsecanja članova višeg reda u Tejlorovim razvojima očekivanja i kovarijacione matrice. Unscented transformacija je metod koji pri formiranju procene očekivanja i kovarijacione matrice uključuje i informaciju sadržanu u članovima višeg reda, uz zadržavanje istog reda numeričke kompleksnosti kao i metod linearizacije [2]. Posledice su gotovo nepomerena procena očekivanja vektora

20.001[ ]m

DX i značajno bolja procena kovarijacione matrice. Opisani algoritam unscented transformacije direktno je primenljiv u rešavanju problema estimacije vektora stanja nelinearnog sistema opisanog jednačinama (1) i (2). Takav metod estimacije poznat je pod nazivom unscented Kalmanov filtar (UKF) i definisan je sledećim skupom koraka [4]:

1. Predikcija stanja Podrazumeva se da su na raspolaganju procena vektora stanja ˆ( | )x k k i kovarijaciona matrica greške te procene .

Na osnovu raspoloživih veličina treba formirati skup sigma tačaka

( | )P k k

(1) (2 ): ,..., nA A AS X X , gde su:

( )( ) ˆ( | ) ( | )iA i

X x k k nP k k= + , ( ) 1

2iW

n= (33)

( )( ) ˆ( | ) ( | )i nA i

X x k k nP k k+ = − , ( ) 1

2i nW

n+ = (34)

gde je n – dimenzija vektora stanja. Propagacijom kroz nelinearnu funkciju modela dinamike (jednačina 1) dobija se skup preslikanih sigma tačaka , gde su: (1) (2 ): ,..., n

B B BS X X

( )( ) ( ),i iB AX f k X= , (35) 1, 2,..., 2i = n

Shodno algoritmu UT, predikcija vektora stanja i kovarijaciona matrica greške predikcije su:

2( )( )

1

ˆ( 1 | )n

iiB

i

x k k W X=

+ = ∑ (36)

( )( )2

( ) ( ) ( )

1ˆ ˆ( 1| ) ( 1| ) ( 1| )

n Ti i iB B

iP k k W X x k k X x k k

=Q+ = − + − +∑ + (37)

2. Predikcija merenja Propagacijom sigma tačaka skupa kroz nelinearnu funkciju merenja formira se skup sigma tačaka

BS

(1) (2 ): ,..., nC C CS Y Y , gde su:

( )( ) ( ),i iBCY h k X= , (38) 1, 2,..., 2i = n

R

Predikcija merenja i kovarijaciona matrica inovacije su: 2

( )( )

1

ˆ( 1)n

iiC

i

y k W Y=

+ = ∑ (39)

( ) ( )( )2

( ) ( )

1ˆ ˆ( 1| ) ( 1| )i

C

n Ti iC

iS W Y y k k Y y k k

== − + + +⋅ −∑ (40)

3. Korekcija Kroskovarijaciona matrica predikcije i merenja iznosi:

( ) (2

( ) ( )( )

1

ˆ ˆ( 1 | ) ( 1 | )n Ti ii

XY B Ci

P W X x k k Y y k k=

= − + − +∑ ) (41)

a Kalmanova matrica pojačanja je 1XYK P S −= . Korekcije

procene stanja i kovarijacione matrice greške procene date su sledećim jednačinama:

( )ˆ ˆ ˆ( 1 | 1) ( 1 | ) ( 1) ( 1)x k k x k k K y k y k+ + = + + + − + (42)

( 1 | 1) ( 1 | ) TP k k P k k K S K+ + = + − ⋅ ⋅ (43) 4. EKSPERIMENT I REZULTATI Dva metoda nelinearne estimacije opisana u ovom radu, EKF i UKF primenjena su u cilju rešavanja problema praćenja objekta koji ulazi u atmosferu (reentrance problem). Kako na objekat pored gravitacione sile deluje i rezistivna sila usled kretanja kroz vazduh (drag force), ubrzanje objekta je h d g= − , gde h označava visinu objekta, g - gravitaciono

ubrzanje. Ubrzanje d iznosi 2 / 2d hρ β= , gde su ρ -

gustina vazduha, - brzina objekta, a h β - balistički koeficijent. Ako se usvoji jednostavan eksponencijalan model atmosfere, promena gustine vazduha sa visinom iznosi

237

0( ) exp( / )h hρ ρ= − kr , gde je 0ρ - gustina vazduha na nivou mora, a - konstanta koja određuje brzinu opadanja gustine sa visinom. Na raspolaganju je samo merenje rastojanja objekta od senzora (slika 2), pri čemu su parametri geometrije problema (D i H) poznati.

kr

H

D

h

d

gz

Slika 2. Ilustracija reentrance problema

Smatra se da balistički koeficijent β nije u dovoljnoj meri poznata veličina, pa se mora estimirati. Opisani problem veoma je nelinearan i praktično je nemoguće izvršiti prihvatljivu estimaciju relevantnih veličina na bazi jednog linearnog modela [5]. Jednačine nelinearnog kontinualnog modela u prostoru stanja su:

21 2

0 1 22

33

0exp( / )

02

0

xx

x kr xx

xg

x w

ρ

⎡⎡ ⎤ ⎡ ⎤⎢

−⎢ ⎥ ⎢ ⎥⎢= −⎢ ⎥ ⎢ ⎥⎢⎢ ⎥ ⎢ ⎥⎢ ⎣ ⎦⎣ ⎦

⎢⎣

⎤⎥⎥ +⎥⎥⎥⎦

(44)

2 2( )z D h H= + − + v (45)

pri čemu su komponente vektora stanja 1x h= , 2x h= i

3x β= . Šum procesa označen je sa w, a šum merenja sa v. U cilju estimacije vektora stanja sistema, formiran je diskretan model usvajanjem periode odabiranja od . Merenja su zašumljena belim Gausovim šumom, nulte srednje vrednosti, varijanse . Stvarna vrednost

početnog uslova je . Filtri su inicijalizovani sa stvarnom vrednošću visine i brzine, ali sa početnom procenom balističkog koeficijenta

0.5[ ]T = s

2m5

2 2500[ ]vR σ= =4 2[6 10 3 10 10 ]T⋅ − ⋅

40

ˆ 7.5 10β = ⋅ . Kovarijaciona matrica šuma procesa ista je u oba slučaja i iznosi . Izvršeno je usrednjavanje grešaka procene i varijansi procena koordinata stanja na skupu od 50 MonteCarlo simulacija, a rezultati su prikazani slikama 3 i 4.

4(0 0 10 )Q diag=

0 20 40 60 80 100 120 140 160 180 200-140

-120

-100

-80

-60

-40

-20

0

20Srednja MC greska visine

vreme [s]

gres

ka p

roce

ne v

isin

e [m

]

EKFUKF

Slika 3. Greška procene visine

0 20 40 60 80 100 120 140 160 180 2000

20

40

60

80

100

120

140

160

180Varijansa procene visine

vreme [s]

varij

ansa

[m2 ]

EKFUKF

Slika 4. Varijansa procene visine

5. ZAKLJUČAK Rezultati simulacija pokazuju da se performanse dva analizirana algoritma (EKF i UKF) minimalno razlikuju. Osnovna razlika leži u načinu na koji se formira predikcija procene: metod EKF bazira se na parcijalnim izvodima funkcije modela dinamike, dok metod UKF aproksimira raspodelu vektora stanja. To je razlog zbog koga se pre ulaska u stacionarno stanje (približno prva polovina simulacije) procene razlikuju. U stacionarnom stanju uticaj parcijalnih izvoda na formiranje procene isčezava, te su i performanse oba algoritma približno jednake. Međutim, važno je primetiti da je za formiranje procene po metodu EKF neophodno raspolagati Jakobijanima funkcija modela, dok su kod metoda UKF Jakobijani nepotrebni. Ako se, pored rečenog uzme u obzir i približno jednaka numerička složenost realizacije oba algoritma, jasno je da metod UKF može predstavljati efikasnu alternativu klasičnom metodu EKF u slučajevima složenih modela procesa, kada izračunavanje Jakobijana ne predstavlja jednostavan zadatak. LITERATURA [1] A.H. Jazwinski, Stochastic Processes and Filtering Theory, San Diego, CA, Academic Press, 1970. [2] S.J. Julier, J.K. Uhlmann, A General Method for Approximating Nonlinear Transformations of Probability Distributions, Tech. Report, RRG, Univ. of Oxford, 1996. [3] S.J. Julier, J.K. Uhlmann, Unscented Filtering and Nonlinear Estimation, Proceedings of the IEEE, vol. 92, no. 3, March 2004. [4] S. Haykin, Kalman Filtering and Neural Networks, John Wiley and Sons Inc., NY, 2001. [5] M. Athans, R.P. Wishner, A. Bertolini, Suboptimal state estimation for continuous-time nonlinear systems from discrete noisy measurements, IEEE Trans. Automat. Control, vol. AC-13, pp. 504-518, Oct. 1968. Abstract – The unscented transformation – an approximate method for calculation of distribution moments of a random vector generated by propagation through a nonlinear function, is presented in this paper. It is shown how the transformation can be used in solving the nonlinear estimation problem, and a comparison to extended Kalman filter is given via simulation example.

THE UNSCENTED TRANSFORMATION APPLICATION TO NONLINEAR ESTIMATION

Igor Jovandić, Branko Kovačević

238