curs_12_filtrul kalman

8/8/2019 Curs_12_FILTRUL KALMAN

1/11

Curs 12FILTRUL KALMAN

NecesitateEste cunoscut c un observer de stare are ecuaia

r

! &% % % x Ax Bu K y Cx (1)

unde termenul r %K y

x , conine diferena dintre ieirea sistemului (msurat) y i

ieirea observerului, %y = %

x, dedus pe baza estimrii %x. Aceast diferen este

amplificat cu Kr, care reprezint parametrul specific al observerului. El se calculeaz

astfel nct eroarea de estimare, ! %e x x, care satisface ecuaia

ee A K C e&! ; (2)s conduc la o dinamic impus de anulare a erorii. Dinamica de anulare a erorii depinde

de valorile proprii ale matricii eA K C . Deci, prin alocarea polilor sistemului (2) se

impune viteza cu care starea estimat %xconvergespre starea real x. Dac polii alocai

sunt apropiai de axa imaginar, amplificrile incluse n vectorul Ke sunt mici, iar

dinamica de anulare a erorii este lent. Firete, este de dorit ca aceast dinamic s fierapid, ceea ce conduce la impunerea unor poli deprtai de axa imaginar. In acest caz,amplificrile incluse n vectorul Ke vor fi mari. Deoarece amplificarea (vectorial) Keamplific direrena y Cx %, iar ieirea y este conine o component de zgomot, este clarc amplificarea zgomotului poate produce efecte majore n funcionarea observerului.Variabilele de stare estimate pot conine componente de zgomot cu att mai mari, cu ctamplificarea Ke este mai mare. In aceste condiii apare o dilem, privind alocarea polilorobserverului:

- dac polii sunt prea apropiai de axa imaginar, amplificarea Ke este redus inivelul zgomotului ca afecteaz starea estimat este mic. In schimb, dinamicade anulare a erorii de estimare este lent;

-dac polii sunt prea departai de axa imaginar, dinamica de anulare a erorii deestimare este rapid, ns amplificarea Ke rezult mare i nivelul zgomotuluica afecteaz starea estimat este excesiv.

Evident, trebuie cutat un compromis, ceea ce implic rezolvarea iterativ a

problemei de alocare, pn se obine o soluie convenabil.Filtrul Kalman consider un model al sistemului n care este evideniat n mod

explicit zgomotul, iarestimarea striinu se face printr-o procedur de alocare, ciprinminimizarea unui criteriu de performan definit n abordare statistic (printr-o

problem de optimizare).Filtrul Kalman poate fi formulat in dou abordri:pentru timp continuu ipentru

timp discret.

In consecin, trebuie prezentate dou probleme preliminare:- definirea unui sistem n abordare statistic;- definirea i rezolvarea unei probleme de optimizare n abordare statistic (ca

etap preliminar pentru rezolvarea problemei de estimare optimal a strii, n

abordare statistic).


2/11

Filtrul Kalman pentru timp continuu1. Sistemul dinamic n abordare statisticEcuaiile sistemului sunt:

( ) ( ) ( ) ( )t t t t ! x

x

u w& (3)

( ) ( ) ( )t t t! y x v (4)

unde ( ) nt w este vectorul zgomotului de sistem, iar ( ) rt v este vectorul

zgomotului de msur (de ieire). Componentele celor doi vectori reprezint zgomotealbe necorelate, cu distribuie normal (gaussian),avnd mediinule:

_ a( )E t !w 0; _ a( )E t !v 0 (5)

iar matricele de autocorelatie i intercorelaie sunt:

_ a( ). ( ) ( )T wE t t tX H !w w R ; _ a( ). ( ) ( )T vE t t tX H !v v R (6)

_ a( ). ( ) 0TE t t X !w v (7)unde matricele wR i vR sunt diagonale i conin dispersiile

2 , 1,iw

i nW ! i

2, 1,

jvj rW ! , presupuse cunoscute (se presupune c se cunosc intensitile

componentelor de zgomot incluse n ( )tw i ( )tv ).

Observaie. Zgomotul total care se simte la ieire conine 2 componente:- componenta produs de zgomotul de sistem. Aceasta provine din w(t) , fiind

un zgomot colorat, deoarece zgomotul alb w(t) acioneaz ca o variabil deintrare n ecuaia de stare a sistemului i are ca efect la ieire un zgomot

colorat (filtrul de formare a zgomotului colorat este chiar sistemul);- componenta v(t), care este un zgomot alb (de msurare).

De regul, prima component are o pondere sensibil mai mare.

2. Problema optimizrii n abordare statistic (problema LQG).O ptimizarea sistemului n abordare statistic implic utilizarea criteriului de

performan

_ a( ) ( ) ( ) ( )T TJ E t t t t ! x Qx u Ru (8)unde E{.} este simbolul mediei statistice, iar Q i R sunt matrice pozitiv definite, princare se pondereaz calitatea reglrii, respectiv efortul de comand. Menionm c ncazul optimizrii deterministe pe un orizont infinit (problema LQ), n locul medieistatistice se utilizeaz integrala celor doi termeni, care conin variabilele de stare i de

comand.Se poate demonstra c soluia problemei LQG este aceeai ca n cazul problemei

LQ, adic se obine comanda optimal:

( ) ( )t t! u Lx (9)unde

1 T!L R B P (10)n care matricea Peste soluia ecuaiei Riccati


3/11

1 0T T !P BR B P PA A P Q (11)Pentru a putea rezolva problema LQG, este necesar s existe un observer

conceput pentru sistemul (3) i (4). Acesta este filtrul Kalman.

Filtrul KalmanSunt date: sistemul (3) i (4), precum i matricele wR i vR .

Ecuaia filtrului Kalman pentru timp continuu este:

! x Ax Bu L y C x &% % % (12)Se observ c ecuaia coincide cu cea a unui observer determinist , ns matricea deamplificare, notat aici cu L, se calculeaz n alt mod. In loc s se apeleze la o problem

de alocare pentru ecuaia erorii (2), se impune minimizarea criteriului de performan :

_ a _ a( ( ) ( )) ( ( ) ( )) ( ) ( )T TJ E t t t t E t t! ! x x x x e e (13)Dac se dezvolt procedura de minimizare, se obine matricea de amplificare

1T

v

!L PC R (14)unde Peste soluia ecuaiei Riccati1

0T T

v w

! PC R CP PA AP R (15)

Dac se compar ecuaiile (10), (11) cu ecuaiile (14), (15), se constat c matriceaamplificrii L din problemele LQG i cea a filtrului Kalman se poate rezolva prin acelaialgoritm, utiliznd corespondenele din tabelul urmtor:

Problema LQG A Q B R

Filtrul Kalman AT Rw CT Rv

ObservaieFuncia Matlab care calculeaz filtrul Kalman este kalman. Aici, ecuaia

sistemului se consider de forma

( ) ( ) ( ) ( )t t t t ! x Ax Bu Gw& (16)

( ) ( ) ( )+ ( ) ( )t t t t t ! y Cx Du Hw v (17)

ceea ce implic o definire corespunztoare a modelului sistemului fizic.Apelarea funciei kalman se face astfel:

[KEST,L,P] = KALMAN(SYS,QN,RN,NN)

unde KEST este estimatorul Kalman definit ca sistem in spaiul starilor, L este vectorul

amplificrii, P este soluia ecuaiei Riccati, iar QN, RN, NN sunt:

E{wwT}= QN, E{vvT}= RN, E{wvT}= NN,

Estimatorul KEST are intrarea [u;y] i genereaz estimrile y_e i x_e ale lui y ixdin ecuaiile: .

x_e =Ax_e + Bu + L(y - Cx_e - Du)


4/11

y_e=C x_e +Du

Prin, SYS este definit sistemul n spaiul starilorSS(A,[B G],C,[DH]) unde

NN=0 este omis.

Exemplu

S se construiasc n Matlab-Simulinkobserverul de stare pentru un motor de curentcontinuu, pentru care ecuaiile sistemului sunt:

1

2f

dIL U rI K w

dt

dJ K .I k w

dt

[

[[

!

!

n care: r= 1.9 ohm; L = 30 mH; J = 0.1 Kg.m2;

K= 0.6 Nm/A; kf= 0.03. Presupunem c

1

2 10w

W ! ;2

2 1w

W !

Modelul sistemului, conform procedurii de aplicare n funcia Matlab kalman, este

? A

1

2

11 0

10 0

1 0

f

r

wL L Lu ;L

k wK

JJ J

y x v

! ! -

- - -

!

x x w w&

Programulclear all;close all;

global r LK J ks BC p

r=1.9;L=0.03;K=0.6;J=0.1;ks=0.03;

A=[-r/L-K/L;K/J -ks/J]; B=[1/L;0];C=[1 0];D=0;

Q=[10 0;0 1];R=0.01;

G=[1/L 0;0 1/J];

SYS=SS(A,[B G],C,[D]);

[KEST,L,P] = KALMAN(SYS,Q,R,0)

genereaz urmtoarele rezultate: 1)Filtrul Kalmana = x1_e x2_e

x1_e -1058 -20x2_e 85.62 -0.3

b = u1 y1

x1_e 33.33 994.2x2_e 0 -79.62c = x1_e x2_e

y1_e 1 0x1_e 1 0x2_e 0 1

d = u1 y1

y1_e 0 0

sM [

;

U

Fig 1 Motor de c.c.


5/11

x1_e 0 0x2_e 0 0

2) Vectorul amplificrii: LT= [ 994.1670 -79.6180]

3) Matricea P din ecuaia Riccati P = 9.9417 -0.7962

-0.7962 45.0925

Aplicatii1) Se consider sistemul din exemplul prezentat. S se analizeze performanele

sistemului utiliznd un observer clasic (determinist).clear all;close all;global r LK J ks BC p Ke

r=0.9;L=0.03;K=0.6;J=0.1;ks=0.0

03;

h=0.0005;

p=[-20+i*20 -20-i*20];

x=[80; 100];

xe=[0;0];

u0=150;

tf=1.5;n=tf/h;

t=[0:h:tf];

U=u0*ones(1,n);

X=[x];

Xe=[xe];

C=[1 0];

B=[1/L;0];

for i=2:n

u(i)=U(i)+50*sign(sin(8*t(i)));

x=x+h*fp(t(i),x,u(i));

y=x(1);

X=[X x];

xe=xe+h*fpe(t(i),xe,y,u(i));

Xe=[Xe xe];

end;figure(1);

plot(t(1:n),X(1,:),t(1:n),Xe(1,:),

'r');grid;

figure(2);

plot(t(1:n),X(2,:),t(1:n),Xe(2,:),

'r');

grid;axis([0 2 0 300]);

Funcia fp servete la simularea procesului i are formafunction fp=fp(t,x,u)

global r LK J ks BC p

w1=50*randn;

w2=randn;fp=[(u-r*x(1)-K*x(2)+w1)/L ;(K*x(1)-ks*x(2)+w2)/J];

Funcia fpe servete la simularea observerului i are formafunction fpe=fpe(t,xe,y,u)

global r LK J ks BC p Ke

A=[-r/L-K/L;K/J -ks/J];

C=[1 0];D=0;

B=[1/L;0];

Ke=place(A',C',p);Ke=Ke';

fpe=A*xe+B*u+Ke*(y-C*xe);

Dac n programul principal se alocp=[-50+i*50 -50-i*50],

atunci se obin rezultatele prezentate n Fig. 2.a i b. Dac se alocp=[-20+i*20 -20-i*20],

atunci se obin rezultatele din Fig. 3.a i b.Se observ c pe msur ce scade distana polilor alocai n raport cu axa imaginar,

scade viteza de convergen a observerului, dar n acelai timp scade i nivelulzgomotului suprapus peste variabila estimat.


6/11

0 0.5 1 1.5 -100

-50

0

50

100

150

200

0 0 .2 0 .4 0 .6 0 .8 1 1 .2 1 .4 1 .6 1 .8 2 0

50

10 0

15 0

20 0

25 0

30 0

a b

Fig. 2

0 0.5 1 1.5 -100

-50

0

50

100

150

200

0 0 .2 0 .4 0.6 0.8 1 1 .2 1 .4 1.6 1 .8 2 0

50

10 0

15 0

20 0

25 0

30 0

a b

Fig. 3

In cazul filtrului Kalman nu este necesar s se utilizeze diverse variante de alocare

a polilor. In acest caz, funcia fpe careservete la simularea filtrului (observerului

statistic) estefunction fpe=fpe(t,xe,y,u)

global r LK J ks BC p Ke

A=[-r/L-K/L;K/J -ks/J];B=[1/L;0];

C=[1 0];D=0;

Q=[1 0;0 100]*0.1;R=0.001;

G=[1/L;1/J];

SYS=SS(A,[B G],C,[0 0]);

[KEST,Ke,P] = KALMAN(SYS,Q,R,0);

fpe=A*xe+B*u+Ke*(y-C*xe);


7/11

Rezultatele care se obin sunt prezentate n Fig. 4.a i b.

0 0. 5 1 1.5 -100

-50

0

50

100

150

0 0 . 2 0 . 4 0 . 6 0 . 8 1 1 .2 1 . 4 1 . 6 1 .8 2 0

50

10 0

15 0

20 0

25 0

30 0

a b

Fig. 4

Se constat c filtrul Kalman realizeaz o estimare mai lent, ns cu o bun filtrare azgomotului.

ObservaieProiectarea filtrului Kalman poate fi legat de anumite incertitudini, ns ntr-o

msur mai mic dect observerele deterministe (aici, trebuie testate mai multe variantede alocare). Inceritudinile filtrului Kalman sunt legate de iniializarea matricelor

diagonale wR i vR , care conin dispersiile2 , 1,

iwi nW ! i 2 , 1,

jvj rW ! , presupuse

cunoscute (acestea sunt notate prin Q i R n aplicaia Matlab). Totui, sensibilitateafiltrului Kalman n raport cu aceste variabile este redus. Astfel, punnd

Q=[1 0;0 1];R=0.001;

n funcia fpe, ceea ce nseamn modificarea dispersiilor cu un ordin de mrime,

rezultatele obinute sunt cele din fig. 5.a i b.

0 0.5 1 1.5 -100

-50

0

50

100

150

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

50

100

150

200

250

300

a b

Fig. 5

Q=[0.1 0;0 10];R=1; Q=[0.1 0;0 10];R=10;


8/11

Filtre Kalman pentru timp discret

Fie sistemul pentru timp discret

( 1) ( ) ( ) ( )k k k k ! x Ax Bu w (14)( ) ( ) ( )k k k! y Cx v (15)

Ca i n cazul timpului continuu, filtrul Kalman minimizeaz un criteriu care

exprim media statistic a ptratului erorii ( ) ( ) ( )k k k! e x x .

In varianta discret a filtrului Kalman, funcionarea estimatorului de stare sedescompune n dou etape, ilustrate n fig. 3.

y Inprima etap se face evaluarea strii sistemului ntre dou momente de

eantionare,k-1 i k. Aceast etap este numit TU (Time Update). Variabila % 1x k

se noteaz prin % 1 1x k k , care nseamn estimarea strii la momentul k-1, pe baza

msurtorilor din proces efectuate la momentul k-1. In etapa TU se face estimarea

variabilei % 1x k k , adic a strii la momentul k, pe baza ultimei msurtori din proces,

efectuat la momentul anterior,k-1. Aceast estimare se face pe baza ecuaiei de stare a

sistemului (excluznd zgomotul w): 1 ( 1) ( 1)k k k k ! x A x Bu (16)

Fie ( 1)kP matricea de covarian a erorii de estimare la momentul k-1. Prin

determinarea mrimii 1k kx , eroarea de estimare crete i matricea de covarian a

erorii, 1k kP , este

_ a1 ( ) 1 . ( ) 1 Tk k E k k k k k k ! - - P x x x x (17)Inlocuind ( )kx din ecuaia de stare (14) i 1k kx din ecuaia (16),

1k k

P devine: 1 ( 1) ( 1) ( 1) ( 1) ( 1) . ( 1) ( 1) ( 1) ( 1) ( 1)

Tk k E k k k k k k k k k k

! - - P Ax Bu w Ax Bu Ax Bu w Ax Bu

= ( ( 1) ( 1)) ( 1) . ( ( 1) ( 1)) ( 1)T

E k k k k k k ! - -

A x x w A x x w

= _ a. ( 1) ( 1) . ( 1) ( 1) . ( 1) ( 1)T T TE k k k k E k k ! - -

A x x x x A w w

Etapa TU

% %1 1 1x k x k k | % 1x k k 1 4 2 43

%x k k

1 2 3

Etapa

U

k-1k

Fig. 3


9/11

( 1) Tk! AP A Q (18)y Cea de a doua etap - numit etapa MU (Measurement Update) -

face ajustarea estimrii sistemului la pasul k, pe baza achiziiei mrimii de ieire y(k). Eaeste descris prin ecuaia filtrului:

1 ( ) ( ) 1k k k k k k k k ! -

x x L yC

x(19)

n care matricea amplificrii are expresia1

( ) 1 1T Tk k k k k

! -

L P C R CP C (20)

Dup aceast etap, variana erorii se micoreaz, ca urmare a informaiei adusede msurarea y(k) a mrimii de ieire. Prin urmare, este necesar s se ajusteze matricea decovarian a erorii, conform relaiei:

1( ) 1 1 1 1T Tk k k k k k k k k k k

| ! -

P P P P C R CP C CP (21)

Prin prelucrarea acestei ecuaii i prin cuplarea celorlalte relaii, se obine modelul

final al filtrului Kalman pentru timp discret:

? A

1

1 ( 1) ( 1)

1 ( 1)

( ) 1 1

1 ( ) ( ) 1

( ) 1

T

T T

n

k k k k

k k k

k k k k k

k k k k k k k k

k k k k k

! ! ! -

! - !

x A x Bu

P AP A Q

L P C R CP C

x x L y C x

P I L C P

(22)

Dac cele 2 etape sunt aplicate n ordinea prezentat, se obine un filtru Kalman.Dac se inverseaz cele 2 etape (mai nti MU, apoi TU), se obine unpredictor

Kalman. In etapa MU se calculeaz

1 ( ) ( ) 1k k k k k k k k ! - x x L y C x (23)

i apoi urmeaz etapa TU, care furnizeaz predicia 1 ( ) ( )k k k k ! x A x Bu (24)

sau, nlocuind ( )k k k|x x prin expresia (23),

1 1 ( ) ( ) 1 ( )k k k k k k k k k ! - x A AL y Cx Bu (25)

n care1

( ) 1 1T Tk k k k k

! -

L P C R CP C (26)

Dup predicie, matricea de covarian are expresia

1 1 ( ) 1T

k k k k k k k ! - P A P L CP A Q (27)


10/11

AplicatiePredictor Kalman pentru motorul de cc

% Predictor Kalman

clear all;close all;

% Caracteristicile motorului

de cc

r=1.9;L=0.03;K=0.6;J=0.1;ks=0

.003;

A=[-r/L-K/L;K/J -ks/J];

C=[1 0];D=0;

B=[1/L;0];

% Discretizarea modelului de

stare

Te=0.001;

[Ad,Bd]=c2d(A,B,Te)

sys=ss(Ad,Bd,C,D,Te);

% zgomotul

n=1000;w=3*randn(n,1);

v=2*randn(n,1);

Q=std(w).^2;

R=std(v).^2;

% Semnalul de intrare

t=0:Te:(n-1)*Te;

u=100*sin(2*pi*t);

for i=1:n,

if i


11/11

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-100

-50

0

50

100

150

200Semnalul de intrare

Timp

u(t)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-60

-40

-20

0

20

40

60

80

100Variabla de stare prezisa xe1(t)

Timp

xe

1(t)

0 0 .1 0.2 0 .3 0 .4 0 .5 0 .6 0 .7 0 .8 0 .9 1-100

-5 0

0

50

10 0

15 0Variabla de s tare prezisa xe2(t )

Timp

xe2(t)

0 0 .002 0 .004 0 .006 0 .008 0 .01 0 .012 0 .014 0 .016 0 .018 0 .02 9.4

9.6

9.8

10

10.2

10.4

10.6

10.8

11

11.2

11.4Varianta erorii de predictie

Timp

P11(t)

curs_12_filtrul kalman

Documents