Laborator TEFO Lucrarea nr. 7 FILTRUL KALMAN este un nstrument matematc puternc care joacă un rol mportant în grafca pe computer când vrem să reprezentăm lumea reală în sstemele de calcul. De asemenea, fltrul Kalman este un bun estmator posbl pentru o clasă largă de probleme. 1. Modele în spaţul stărlor Modelele în spaţul stărlor sunt, în esenţă, o convenţe de notaţe pentru problemele de estmare ş control, dezvoltate pentru prelucrăr matematce ma uşoare. Se consderă un proces descrs prntr-o ecuaţe cu dferenţe de ordn n (smlară une ecuaţ dferenţale) de forma y = a y + + a y + u (1) unde { } + 1, n1, n+ 1, u este un proces de tp zgomot alb, având funcţa de autocorelaţe (, ) E u u = R = Qδ (2) j u j unde E reprezntă operatorul de medere statstcă, ar valorle nţale { y y y }, 1,, n+ 1 sunt varable aleatoare de mede zero cu o matrce de covaranţă de dmensun n n cunoscută P = E( y j, y), j, {, n 1}. (3) Presupunând zgomotul statstc ndepent de procesul care urmează a f estmat, rezultă: E ( u yj ) E ( u ) E ( yj ), = =, pentru n+ 1 j ş, (4) ceea ce asgură că E( u, y j) =, pentru j. (5) Ecuaţa cu dferenţe (1) poate f rescrsă ca y+ 1 a a1 an2 an 1 y 1 y 1 y 1 [ x+ 1] = y 1 = 1 y 2 + u (6) y n+ 2 1 y n+ 1 [ A] [ x ] [ G] care conduce la modelul în spaţul stărlor: [ x+ 1] = [ A][ x] + [ G] u (7) y = [ 1 ][ x] (8) sau în forma ma generală [ x+ 1] = [ A][ x] + [ G] u, (9) y = H x, (1) [ ][ ] 1
unde [ H ] reprezntă matrcea care descre depenţa lnară a eşr de starea sstemulu. Ecuaţa (9) reprezntă depenţa stăr [ x + ] atât de starea anteroară [ ] 1 x, cât ş de un proces de zgomot u. Ecuaţa (1) descre depedenţa dntre observaţle starea nternă [ x ]. Ecuaţle (9) ş (1) sunt adesea numte modelul de proces, respectv modelul de măsurare. Descrerea în spaţul stărlor a sstemulu descrs de ecuaţle (9) ş (1) este dată în Fgura 1, unde se observă ntrarea u, starea [ x ] la momentele ş +1, precum ş eşrea y a sstemulu. y ş u [ x + 1] [ x ] [G] z -1 [H ] y [A] Fgura 1. Descrerea în spaţul stărlor a sstemulu descrs de ecuaţle (9) ş (1) 2. Problema proectăr observatorulu Exstă o problemă generală legată de domenul teore sstemelor lnare numtă problema proectăr observatorulu. Problema de bază este de a determna (estma) stărle nterne ale unu sstem lnar, având acces numa la eşrle sstemulu. Acest lucru este înrudt cu problema cute negre unde este acces la unele semnale vennd dn cute (eşrle), dar nu se poate observa drect ceea ce este în nteror. Multe abordăr la această problemă de bază sunt tpc bazate pe modelul în spaţul stărlor prezentat în secţunea anteroară. Tpc exstă un model de proces care modelează transformarea stăr procesulu. Acesta poate f, de obce, reprezentat ca o ecuaţe cu dferenţe stocastcă lnară smlară cu ecuaţa (9): [ x] = [ A][ x 1] + [ B][ u] + [ w], (11) unde [ A ] ş [ B ] descru depenţa stăr curente de starea anteroară, respectv de ntrarea curentă. În plus exstă o formă a modelulu de măsurare care descre relaţa între starea procesulu ş măsurărle efectuate. Acesta poate f, de obce, reprezentat cu o ecuațe smlară ecuaţe (1): z = H x + v (12) Zgomotul de proces [ ] [ ] [ ][ ] [ ] w ş zgomotul de măsurare [ ] y cu [ ] v sunt varable aleatoare. z pentru a specfca faptul că Dn (1) ş (12) se observă înlocurea varable măsurărle nu sunt neapărat stărle specfcate, c pot f orce combnaţe lnară a acestora. 2
Zgomotul de măsurare ş de proces Zgomotul de măsurare apare, de exemplu, în cazul măsurătorlor cu ajutorul unu senzor ş datortă mperfecţunlor aparatelor de măsură. În funcţe de raportul semnal utl-zgomot trebue să se ţnă cont în estmare de nformaţa obţnută dn măsurare. Deoarece nc transformarea prn care se obţne starea sstemulu nu este perfectă trebue să se ţnă cont în estmaţ stăr de un zgomot de proces. 3. În contnuare se descre fltrul Kalman unde măsurătorle ş starea sunt consderate la momente dscrete în tmp. 3.1 Procesul care urmează a f estmat consderă problema generală a încercăr de a estma starea n [ x] a unu proces controlat în tmp dscret care este guvernat de ecuaţa cu dferenţe stocastcă lnară [ x] = [ A][ x 1] + [ B][ u] + [ w], (13) m z care este descrsă de ecuața dntr-o măsurare [ ] [ z] [ H][ x] [ v] Vector de varable aleatoare [ w ] ş [ ] = +. (14) v reprezntă zgomotul de proces, respectv, zgomotul de măsurare. Se presupune că zgomotul de proces w ş cel de măsurare v sunt ndepente, de tp zgomot alb, ş cu dstrbuţ de probabltate normale p w N, Q (15) ([ ]) ( [ ]) ([ ]) (, [ ]) p v N R (16) unde [ Q ] ş [ R ] sunt matrcele de covaranţă a zgomotulu de proces, respectv a zgomotulu de măsurare, presupuse a f constante. Matrcea [ A ], de dmensun n n, în ecuaţa cu dferenţe (13), leagă starea la momentul de tmp anteror 1 de starea la momentul curent, în absenţa ntrăr de comandă [ u ] ş a zgomotulu de proces [ w ]. Se face observaţa că, în practcă, matrcea [ A ] s-ar putea schmba cu fecare moment de tmp, dar ac se presupune că B, de dmensun n l, leagă ntrarea de control (opţonală) este constantă. Matrcea [ ] l [ u] de starea [ x ]. Matrcea [ H ], de dmensun m n leagă starea [ x ] de măsurarea [ z ]. În practcă matrcea [ ], în ecuaţa de măsurare (14), H s-ar putea schmba cu fecare moment de tmp, dar ac se presupune că este constantă. 3
3.2 Orgnle computaţonale ale fltrulu Se defneşte n x (se observă super mnusul ) ca fnd estmatul stăr a pror la momentul, dată fnd cunoştnţa procesulu anteror la momentul, ş n x estmatul stăr a posteror la momentul dată fnd măsurarea [ z ]. Se pot defn atunc vector de eroare a estmaţlor a pror ş a posteror ca fnd e [ ] = x x, ş (17) [ e ] [ x ] x = (18) Matrcea de covaranță a eror estmatulu a pror este atunc P { } T = E e e, (19) ar matrcea de covaranță a eror estmatulu a posteror este [ P ] {[ ][ ] T = E e e } (2) În obţnerea ecuaţlor pentru fltrul Kalman, scopul nţal este găsrea une ecuaţ care calculează un estmat al stăr a posteror x ca o combnaţe lnară dntre un estmat a pror x o predcţe a măsurăr [ H] x, aşa cum este arătat în ecuaţa (21): [ ] [ ] [ ] x = x + K z H x (21) Dferenţa [ z ] [ ] H x, în ecuaţa (21), este numtă novaţa măsurăr, sau z ş măsurarea prezsă ş o dferenţă ponderată între măsurarea actuală [ z ] ş rezduul. Rezduul reflectă dferenţa între măsurarea actuală [ ]. [ H] x Matrcea [ K ] de dmensun n m în ecuaţa (21) este numtă câştgul sau factorul de amestec mnmzând urma matrce de covaranță a eror a posteror dn (2). Această mnmzare poate f realzată astfel: ma întâ se substtue ecuaţa (21) în defnţa de ma sus pentru [ e ] (relaţa (18)), apo se înlocueşte relaţa obţnută în ecuaţa (2), se realzează mederle ndcate, se dervează urma matrce rezultate în raport cu elementele matrce [ K ], se egalează rezultatul cu zero, ş apo se rezolvă ecuaţa în [ K ]. O formă a lu [ K ] rezultat care mnmzează urma matrce dn (2) este dată prn ( ) 1 [ ] T T = [ ] [ ] [ ] + [ ] K P H H P H R (22) 4
Dn (22) rezultă că atunc când matrcea de covaranță a eror de măsurare [ R ] se aprope de matrcea nulă, câştgul [ K ], care multplcă rezduul, a valor dn ce în ce ma mar. Anume, lm [ K ] [ ] 1 H [ R] [ ] = (23) Pe de altă parte, când matrcea de covaranță a eror estmatulu a pror P se aprope de matrcea nulă, câştgul [ K ] multplcă rezduul cu valor dn ce în ce ma mc. Anume, lm K = (24) P [ ] Se poate conchde că atunc când matrcea de covaranță a eror de măsurare [ R ] se aprope de matrcea nulă, măsurarea actuală este dn ce în ce ma credblă, în tmp ce măsurarea prezsă [ H] x este dn ce în ce ma puţn credblă, ar când matrcea de covaranță a eror estmatulu a pror P se aprope de matrcea nulă măsurarea actuală [ z ] este dn ce în ce ma puţn credblă, în tmp ce măsurarea prezsă [ H] x este dn ce în ce ma mult credblă. 3.3 Algortmul fltrulu Kalman dscret estmează un proces prn utlzarea une forme de control cu reacţe: fltrul estmează starea procesulu la un anumt moment de tmp ş apo obţne reacţa în forma măsurărlor (zgomotoase). Astfel, ecuaţle fltrulu Kalman se împart în două grupe: ecuaţ de actualzare în tmp ş ecuaţ de actualzare a măsurăr. Ecuaţle de actualzare în tmp sunt responsable de determnarea estmaţlor stăr curente ş a matrce de covaranţă a eror pentru a obţne estmaţ a pror pentru următorul moment de tmp. Ecuaţle de actualzare a măsurăr sunt responsable de reacţe adcă de ncorporarea une no măsurăr în estmatul a pror pentru a obţne un estmat a posteror îmbunătăţt. Ecuaţle de actualzare în tmp pot f de asemenea gândte ca ecuaţ predctor, în tmp ce ecuaţle de actualzare a măsurăr pot f gândte ca ecuaţ corector. Într-adevăr algortmul de estmare fnal seamănă cu cel al unu algortm de tp predctorcorector pentru rezolvarea problemelor numerce aşa cum este arătat în Fgura 2. Ecuaţle specfce pentru actualzărle în tmp ș ale măsurăr sunt prezentate ma jos în Tabelele 1 ş 2. Actualzare în tmp ( Predcţe ) Actualzare a măsurăr ( Corecţe ) Fgura 2. Cclul fltrulu Kalman dscret 5
Tabelul 1: Ecuaţle de actualzare în tmp ale fltrulu Kalman dscret x [ ] = A x1 + [ B][ u 1] (25) T P = [ A][ P 1][ A] + [ Q] (26) Observăm cum ecuaţle de actualzare în tmp dn Tabelul 1 determnă estmaţ stăr a pror ş a matrce de covaranţă a eror a pror la momentul în funcțe de cele de la momentul de tmp 1. Tabelul 2: Ecuaţle de actualzare a măsurăr ale fltrulu Kalman dscret T T [ K] P = [ H] [ H] P [ H] + [ R] (27) ( ) 1 [ ] [ ] [ ] x = x + K z H x (28) P = I K H P (29) [ ] ([ ] [ ][ ]) Prma sarcnă în tmpul actualzăr măsurăr este de a calcula câştgul Kalman, z, ş apo de a [ K ]. Următorul pas este de a măsura de fapt procesul pentru a obţne [ ] genera un estmat al stăr a posteror ncorporând măsurarea ca în (28). Pasul fnal este de a obţne estmatul matrce de covaranţă a eror a posteror prn (29). După fecare pereche de actualzare în tmp ş de măsurare, procesul este repetat cu estmaţ a posteror anteror pentru a face predcţa pentru no estmaţ a pror. Această natură recursvă este una dn caracterstcele foarte atractve ale fltrulu Kalman făcând mplementărle practce mult ma fezable decât (de exemplu) o mplementare a unu fltru Wener, care este proectat pentru a opera pe toate datele drect pentru fecare estmat., în schmb, condţonează recursv estmatul curent de toate măsurărle trecute. Fgura 3 lustrează complet funcţonarea fltrulu, combnând dagrama dn Fgura 2 cu ecuaţle dn Tabelele 1 ş 2. În Fgura 4 este dată dagrama bloc a sstemulu, în poteza absenţe ntrăr de control, modelul de măsurare ş structura fltrulu Kalman dscret. 3.4 Parametr fltrulu ş reglare În mplementărle reale ale fltrulu, matrcea de covaranță a zgomotulu de R este de obce măsurată înante de funcţonarea fltrulu. Măsurarea măsurare [ ] matrce de covaranţă a eror de măsurare [ R ] este posblă, deoarece se poate măsura procesul în tmpul funcţonăr fltrulu luând câteva eşantoane de măsurare. Determnarea matrce de covaranţă a zgomotulu de proces [ Q ] este în general ma dfclă când, de fapt, nu exstă posbltatea de a observa drect procesul care se estmează. Uneor un model de proces relatv smplu poate produce rezultate acceptable dacă se ntroduce destulă ncerttudne în proces prn selecţa lu [ Q ]. În acest caz se speră că măsurărle procesulu sunt fable. 6
Actualzare în tmp ( Predcţe ) Actualzare a măsurăr ( Corecţe ) 1) Predcța stăr a pror 1) Calculul câştgulu Kalman x [ ] T T = A x1 + [ B][ u 1] [ K ] [ ] ([ ] [ ] [ ]) P = H H P H R + 2) Predcța matrce de 2) Actualzarea estmatulu cu măsurarea [ z ] covaranță a eror a pror T P = [ A][ P 1][ A] + [ Q] [ ] [ ] [ ] x = x + K z H x 3) Actualzarea matrce de covaranță a eror [ P] = ([ I] [ K][ H] ) P Fgura 3. O descrere completă a funcţonăr fltrulu Kalman SISTEM DISCRET MĂSURARE FILTRU KALMAN DISCRET v w + + x + z + e + Σ H Σ Σ K Σ + - + x 1 A z -1 H x A x 1 x z -1 Fgura 4. Dagrama bloc a sstemulu, modelul de măsurare ş structura fltrulu Kalman dscret În orce caz, dacă exstă sau nu o bază raţonală pentru alegerea parametrlor, adesea poate f obţnută o performanţă superoară (statstc vorbnd) prn reglarea parametrlor fltrulu [ Q ] ş [ R ]. Reglarea este de obce realzată înantea estmăr, de regulă cu ajutorul altu fltru Kalman într-un proces numt, în general, dentfcare de sstem. În condţle în care [ Q ] ş [ R ] sunt constante, atât matrcea de covaranță a eror [ P ], cât ş câştgul Kalman [ K ] se vor stablza rapd ş apo vor rămâne constante (vez ecuaţle de actualzare ale fltrulu în Fgura 3). Dacă acesta este cazul, aceşt parametr pot f pre-calculaţ, fe prn rularea fltrulu premergător, fe, de P. exemplu, prn determnarea valor de stare stablă a lu [ ] 7
4. Exemple de estmare folosnd o fltrare Kalman 3.1 Estmarea nvelulu de curent contnuu Se presupune exemplul estmăr unu nvel de tensune contnuă. Se presupune că exstă posbltatea de face măsurăr ale tensun contnue, dar măsurărle sunt perturbate de un zgomot de măsurare alb cu valoarea devaţe standard de.1 volţ. În acest exemplu, procesul este guvernat prn ecuaţa cu dferenţe lnară x = x + w, (3) 1 1 cu o măsurare z care este z = x + v (31) Astfel în acest caz matrcele devn scalar. Starea neschmbându-se de la un pas la altul rezultă A = 1 ş neexstând control asupra ntrăr, rezultă u =. Măsurarea cu zgomot este drect a stăr, aşa că H = 1. Ecuaţle de actualzare în tmp sunt x = x1, P = P 1 + Q, ar ecuaţle de actualzare a măsurăr sunt ( ) 1 P K = P P + R = P + R ( ) = + x x K z x ( 1 ) P = K P 5 Se presupune o varanţă a procesulu foarte mcă, de exemplu Q = 1. S-ar putea seta Q =, dar presupunând o valoare mcă, dar dfertă de zero, rezultă ma multă flexbltate în reglarea fltrulu după cum se va demonstra în contnuare. Se presupune că dn experenţă se şte că valoarea adevărată a tensun contnue are o dstrbuţe de probabltate normală standard, aşa că se începe cu presupunerea că tensunea este. Cu alte cuvnte, înante de începere se setează x =. Smlar este nevoe de alegerea une valor nţale pentru P 1, anume P. Dacă este absolut sgur că estmarea stăr nţale x = a fost corectă, se va seta P =. Totuş dată fnd ncerttudnea în estmatul nţal x, alegerea P = ar cauza faptul ca fltrul să nţalzeze ş apo totdeauna să creadă tot tmpul că x =. Ca urmare, alegerea alternatvă nu este crtcă. S-ar putea alege orce P ş fltrul eventual ar converge. Fltrul va starta cu P = 1. Valoarea tensun contnue s-a consderat x = 1. S-au smulat 5 de măsurăr dstncte z care au avut eror dstrbute normal în jurul lu zero cu o devaţe standard de.1. Programul pentru generarea tensunlor contnue, cu zgomot ş estmate este dat în contnuare. 8
% P_Kalman_ex1 clear; clc; % parametr ntal n_ter = 5; sz = n_ter; % dmensunea vectorlor x = 1; % valoarea adevarata a tensun contnue z = x +.1*randn(1,sz); % observat (normale raportat la x, sgma=.1) Q = 1e-5; % varanta procesulu xhat = zeros(1,sz); % estmatul a posteror a lu x P = zeros(1,sz); % estmatul varante eror a posteror xhatmnus = zeros(1,sz); % estmatul a pror a lu x Pmnus = zeros(1,sz); % estmatul varante eror a pror K = zeros(1,sz); % castgul sau factorul de amestec R =.1; % estmatul varante masurar, a se schmba pentru a vedea efectul % ghcrea ntala xhat(1) =.; P(1) = 1.; for =2:n_ter % actualzare n tmp xhatmnus() = xhat(-1); Pmnus() = P(-1)+Q; % actualzare a masurar K() = Pmnus()/( Pmnus()+R ); xhat() = xhatmnus()+k()*(z()-xhatmnus()); P() = (1-K())*Pmnus(); vald_ter = 1:n_ter; fgure(1) clf; plot(z,'-m*'); hold on; plot(xhat,'-bv','marerfacecolor','b'); hold on; plot(vald_ter,x*ones(1,sz),'-g^','marerfacecolor','g'); grd on; axs([1 n_ter mn(z) max(z)]); leg('masurar','starea estmata','valoare adevarata'); xlabel('iterate') ylabel('tensune') 9
vald_ter(1)=[]; fgure(2) clf; plot(vald_ter,pmnus(vald_ter),'- bv','marerfacecolor','b') grd on; axs([1 n_ter.1]); xlabel('iterate ()') ylabel('(tensune)^2 - P_') În prma smulare s-a fxat varanţa măsurăr la R = (.1) 2 =.1. Rezultatele sunt date în Fgura 5. 1.1 masurar starea estmata valoare adevarata 1.5 1 Tensune.95.9.85 5 1 15 2 25 3 35 4 45 5 Iterate Fgura 5. Smularea cu R = (.1) 2 =.1 Când s-a consderat alegerea lu P de ma sus, s-a menţonat că alegerea nu a fost crtcă cât tmp P deoarece fltrul eventual ar converge. În Fgura 6 s-a reprezentat valoarea lu P în funcţe de teraţe. Dn alegerea nţală de 1, la a 5-a teraţe valoarea s-a stablt la aproxmatv.3 (Volţ 2 ). În Fgurle 7 ş 8 de ma jos se poate vedea ceea ce se întâmplă când R este crescut sau scăzut cu un factor de 1. În Fgura 7 s-a consderat că varanţa măsurăr a fost de 1 de or ma mare (adcă R = 1) aşa că răspunsul fltrulu a fost ma lent în a crede măsurărle, estmărle prezse devennd ma credble. Rezultatul conduce la o varanță redusă a semnalulu estmat. 1
.1.9.8.7.6 (Tensune) 2 - P.5.4.3.2.1 5 1 15 2 25 3 35 4 45 5 Iterate () Fgura 6. După 5 de teraţ, covaranţa eror P, aleasă nțal ca fnd egală cu 1, s-a stablt la aproxmatv.3 (Volţ 2 ) 1.15 masurar starea estmata valoare adevarata 1.1 1.5 Tensune 1.95.9.85 5 1 15 2 25 3 35 4 45 5 Iterate Fgura 7. Smularea cu R = 1. Fltrul este ma lent în răspuns la măsurăr, rezultând într-o varanţă redusă a estmatulu. În Fgura 8 s-a consderat că varanţa măsurăr a fost de 1 de or ma mcă (adcă R =.1) aşa că răspunsul fltrulu a fost ma rapd în a crede măsurărle 11
zgomotoase, acestea devennd ma credble. Prn urmare varanța semnalulu estmat este ma mare. Deş estmarea une tensun contnue este relatv drectă, aceasta demonstrează clar funcţonarea fltrulu Kalman. În Fgura 7, în partcular, este evdent că în fltrarea Kalman estmatul apare consderabl ma neted decât măsurărle cu zgomot. 1.2 masurar starea estmata valoare adevarata 1.15 1.1 1.5 Tensune 1.95.9.85.8 5 1 15 2 25 3 35 4 45 5 Iterate Fgura 8. Smularea cu R =.1. Fltrul răspunde rapd la măsurăr, crescând varanţa estmatulu. 3.2 Sstem de urmărre prn radar a deplasăr unu vehcul În acest sstem radarul emte mpulsur, ar semnalele returnate sunt procesate prn fltrul Kalman pentru a determna deplasarea unu vehcul (presupusă cu vteză aproxmatv constantă) într-un plan de coordonate xoy. Se presupune că vehculul are vteză aproxmatv constantă astfel încât ecuațle de modfcare a vtezelor pe cele două axe, x [ ] v [ ] = v [ 1] + w [ ] unde [ ] x x x x vy[ ] = vy[ 1] + wy[ ] w ș w [ ] y v ș v [ ] y, sunt:, sunt zgomotele care modelelază schmbarea vtezelor la momentul, presupuse de tp Gaussan cu mede nulă. Presupunând pasul de deplasare între două momente de tmp succesve ca fnd egal cu T, ecuațle de modfcare a deplsasărlor pe cele două axe sunt: rx[ ] = rx[ 1] + vx[ 1] T ry[ ] = ry[ 1] + vy[ 1] T Astfel vectorul de stare este 12
[ ] [ ] [ ] [ ] rx ry [ x ] = vx vy ar ecuața de stare este [ x] = [ A] [ x 1] + [ w], unde 1 T 1 T [ A] = w 1 1, [ ] = wx [ ] wy [ ] Observațle sunt valorle deplasărlor pe cele două axe perturbate de zgomot Gaussan de mede nulă: zx[ ] = rx[ 1] + nx[ ] zy[ ] = ry[ 1] + ny[ ] Defnnd vectorul de observațe [ z ] [ ] [ ] T = zx zy, ecuaţa de observaţe în tmp dscret este [ z] = [ H][ x] + [ n] unde 1 [ H ] = 1, [ n ] [ ] [ ] T = nx ny. Covaranța zgomotulu de stare este [ Q] =, 2 σ w 2 σ w 2 unde σ w este varanța zgomotelor wx [ ] ș wy [ ], ar covaranța zgomotulu de măsurare este 2 σ n [ R] = 2, σ n unde n. σ este varanța zgomotelor n [ ] ș [ ] 2 n x Programul Matlab care dă rezultatele pentru covaranţele erorlor deplasărlor, câştgul Kalman ş evoluţle deplasăr în sstemul de coordonate xoy este dat în contnuare. Este arătată convergenţa elementelor de pe dagonală ale matrce de covaranţă a eror, valorle câştgurlor Kalman selectate ş de asemenea evoluţa 2 4 2 deplasăr. Valorle numerce foloste sunt: T = 1, σ w = 1, σ n =.1, P = 1 [ I4], ar ecuațle deplsărlor pe cele două axe sunt: y 13
[ ] [ ] rx = 1.2 ry = 5+.2 astfel încât vtezele de deplasare nșale sunt vx [ ] =.2 vy [ ] =.2 % P_Kalman_ex2 clear; clc; % parametr ntal n_ter = 5; sz = n_ter; T = 1; x(:,:,1) = [1; -5; -.2;.2]; % starea ntala A = [1 T ; 1 T; 1 ; 1]; % matrcea care leaga starea curenta de cea anteroara % varanta zgomotulu de proces (vtezele vx s vy) sgma_w2 = 1e-4; wx = sqrt(sgma_w2)*randn(1,sz); % wy = sqrt(sgma_w2)*randn(1,sz); H = [1 ; 1 ]; % matrcea care leaga secventa de observatoe de starea curenta % varantele zgomotelor de masurare (pentru rx s ry) sgma_n2 =.1; nx = sqrt(sgma_n2)*randn(1,sz); ny = sqrt(sgma_n2)*randn(1,sz); z(:,:,1) = H*x(:,:,1) + [nx(1); ny(1)]; % observata ntala % starle s observatle la momentele urmatoare de tmp for =2:sz x(:,:,) = A*x(:,:,-1) + [;;wx(-1);wy(-1)]; z(:,:,) = H*x(:,:,) + [nx(); nx()]; Q = [ ; ; sgma_w2 ; 14
sgma_w2]; % matrcea de covaranta a zgomotulu de proces xhatmnus = zeros(4,1,sz); % estmatul a pror a lu x Pmnus = zeros(4,4,sz); % estmatul eror a pror K(:,:,1) = zeros(4,2); % castgul sau factorul de amestec ntal R = [sgma_n2 ; sgma_n2]; % matrcea de covaranta a zgomotulu de masurare % ghcrea ntala xhat(:,:,1) = [5; 5; ; ]; % estmatul a posteror a lu x P(:,:,1) = 1*eye(4); % estmatul eror a posteror for =2:n_ter % actualzare n tmp xhatmnus(:,:,) = A*xhat(:,:,-1); Pmnus(:,:,) = A*P(:,:,-1)*A'+Q; % actualzare a masurar K(:,:,) = Pmnus(:,:,)*H'*nv( H*Pmnus(:,:,)*H'+R ); xhat(:,:,) = xhatmnus(:,:,)+k(:,:,)*(z(:,:,)- H*xhatmnus(:,:,)); P(:,:,) = (eye(4)-k(:,:,)*h)*pmnus(:,:,); vald_ter = [1:n_ter]-1; fgure(1); clf; for =1:n_ter P_rx()=P(1,1,); plot(vald_ter,p_rx,'--o'); xlabel('iterate'); ylabel('covaranta deplasar r_x'); grd on; axs([mn(vald_ter) max(vald_ter) P_rx(2)]) fgure(2); clf; for =1:n_ter P_ry()=P(2,2,); plot(vald_ter,p_ry,'--o'); xlabel('iterate'); 15
ylabel('covaranta deplasar r_y'); grd on; axs([mn(vald_ter) max(vald_ter) P_ry(2)]) fgure(3); clf; for =1:n_ter P_vx()=P(3,3,); plot(vald_ter,p_vx,'--o'); xlabel('iterate'); ylabel('covaranta vteze de deplasare v_x'); grd on; axs([mn(vald_ter) max(vald_ter) 1*mn(P_vx)]) fgure(4); clf; for =1:n_ter P_vy()=P(4,4,); plot(vald_ter,p_vy,'--o'); xlabel('iterate'); ylabel('covaranta vteze de deplasare v_y'); grd on; axs([mn(vald_ter) max(vald_ter) 1*mn(P_vy)]) % fgurle pentru castgurle Kalman fgure(5); clf; for =1:n_ter K_depl()=K(1,1,); plot(vald_ter,k_depl,'--o'); xlabel('iterate'); ylabel('castgul Kalman al deplasar r_x'); grd on; fgure(6); clf; for =1:n_ter K_vt_depl()=K(2,2,); plot(vald_ter,k_vt_depl,'--o'); 16
xlabel('iterate'); ylabel('castgul Kalman al deplasar r_y'); grd on; vald_ter = [1:n_ter-1]; fgure(7); clf; for =1:n_ter rxhat_depl()=xhat(1,1,); rx_depl()=x(1,1,); zx_depl()=z(1,1,); ryhat_depl()=xhat(2,1,); ry_depl()=x(2,1,); zy_depl()=z(2,1,); plot(rxhat_depl,ryhat_depl,'-o',rx_depl,ry_depl,'- go',zx_depl,zy_depl,'-rx'); ttle('evoluata deplasar'); leg('deplasare adevarata','deplasare estmata','masurar'); xlabel('r_x'); ylabel('r_y'); grd on; 5. Aplcaţ propuse 1. Modfcaț programul dn Secțunea 3.1 astfel încât să generaț câte M = 1 de măsurăr la fecare terațe, dn maxm 3 posble, a fltrăr Kalman. Pentru fecare terațe calculaț valoarea mede ș varanța valorlor estmate x dn cele M realzăr. Afșaț, în funcțe de numărul terațe, pe un grafc valoarea mede a valorlor estmate ș valoarea adevărată, ar pe alt grafc varanța valorlor estmate ș lmta nferoară Cramer-Rao pentru estmatul nedeplasat al nvelulu de curent contnuu. Pentru o ma bună vedere afșaț grafcele doar pentru următoarele valor lu : 1, 2, 3,..., 3. Modfcaț valoarea varanțe estmate a zgomotulu de măsurare R ș a varanțe estmate a zgomotulu de proces Q ș observaț cum varază varanța valorlor estmate x comparatv cu lmta nferoară Cramer-Rao. 2. O secvenţă aleatoare în tmp dscret este dată de x = 1 Ax + + w unde A =.5, x este o varablă aleatoare cu mede ş varanţă 1, ar w este zgomot alb de mede ş varanţă 1. Ecuaţa de observaţe este dată de z = x + v cu v zgomot alb de mede ş varanţă 1. Termen x, gaussan. w ş v sunt toţ de tp Să se realzeze un program care să dea secvenţa estmată x prn fltrare Kalman. Se vor consdera dferte valor pentru varanţa estmată a măsurăr. 17
3. Să se realzeze acelaş program ca la aplcaţa 1 pentru cazul în care A = 1, zgomotul w are varanţa 3, ar zgomotul v are varanţa 2. Restul parametrlor, precum ş ecuaţle rămân aceleaş. 4. Un canal cu fadng multcale acțonează asupra semnalulu transms pe acesta ca un fltru cu răspuns fnt la mpuls având coefcenț varabl în tmp conform ecuațe: p1, y [ ] = h [ ] u [ ] = unde u [ ] este semnalul transms pe canal, [ ] ar h [ ] = h[, ] h[ 1, ], h[ p1] T y este semnalul recepțonat de pe canal, este vectorul coefcențlor canalulu la momentul. Se presupune că aceșt coefcenț se modfcă lent în tmp, conform ecuațe de stare h [ ] = [ A] h [ 1] + w [ ], [ A ] fnd o matrce cunoscută ș w [ ] vectorul de zgomot de stare, presupus Gaussan de mede nulă, cu matrce de covaranță [ Q ]. Ieșrea canalulu măsurată la momentul este descrsă de ecuața de măsurare: z [ ] = y [ ] + v [ ], v este zgomotul de măsurare, presupus Gaussan de mede nulă ș varanță unde [ ] 2 σ v. Să se estmeze prn fltrare Kalman coefcenț canalulu. Pentru mplementarea în.99 2 2 4 Matlab se va consdera că p = 2, [ A] =.999, [ Q] = σ w [ I2], cu σ w = 1, 2 σ v =.1, ar semnalul de ntrare în canal se consderă un semnal perodc cu peroada 1 de forma (pe o peroadă):, < 5, u [ ] = 1, 5 < 1 18