I NGENIERA EN AUTOMATIZACI ON Y C ONTROL I NDUSTRIAL I Control Automatico II Ejemplo Resuelto de Filtro de Kalman

U NIVERSIDAD N ACIONAL DE Q UILMES 1 de julio de 2004 Pagina 1 de 4

Ejemplo 1 (Filtro de Kalman continuo) Consideremos el sistema continuo dado por x= 4 2 0 1 x+ u+ v 2 4 1 1

(1)

y = 1 0 x + w. donde el t rmino de ruido v(t) tiene media cero y covarianza V = 0.09. El ruido de medici n se asume de e o media cero y covarianza W = 0.25. El objetivo es dise ar un ltro de Kalman continuo para estimar las n T variables de estado de (1). Considermos el estado inicial de la planta x(0) = 0.5 0.5 , con covarianza de este estado inicial P0 = I22 . Para describir completamente el ltro de Kalman, recurrimos a las ecuaciones x(t) = Ax(t) + Bu(t) + L(t) (y(t) Cx(t)) L(t) = P(t)CT W 1 P(t) = AP(t) + P(t)AT P(t)CT W 1CP(t) GV GT Para resolver num ricamente la ecuaci n diferencial, usamos e o [t,p]=ode45(Ej_Kal,[0 10],[0.1 0 0.1]); donde el archivo Ej_Kal contiene la siguiente funci n: o function dp=Ej_Kal(t,p) dp = zeros(3,1); % un vector columna A=[-4 2;-2 -4]; B=[0;1]; C=[1,0]; G=[1;-1]; V=0.09; W=0.025; P=[p(1),p(2);p(2),p(3)]; DP=A*P + P*A - P*C*inv(W)*C*P + G*V*G; dp(1)=DP(1,1); dp(2)=DP(1,2); dp(3)=DP(2,2); y cuyo resultado se observa en la Figura 1.
0.1

(2)

3.5
0.08

0.06

2.5

2
0.04

1.5

0.02

22 11

p 0 p
12

0.5
=p

l1

21

0 l2

0.02

5 t [s]

10

0.5

5 t [s]

10

Figura 1: Elementos de la matriz de covarianza P(t) de la soluci n num rica de la ecuaci n diferencial o e o matricial de Riccati Como resolver la ecuaci n de Riccati es difcil, consideramos la ganancia de Kalman en estado estao cionario. Los valores de P(t) de la Figura 1 sugieren una buena aproximaci n. Para poder conrmar la o

 Control Automatico II

Ejemplo Resuelto de Filtro de Kalman

Pagina 2 de 4

existencia de una unica matriz P denida positiva soluci n de la ecuaci n algebraica de Riccati, debemos o o T ,C T ) sea estabilizable y /AT , T T ) sea detectable, donde GV GT = T T T . Como se primero vericar que (A verican ambas condiciones, la soluci n de la ecuaci n algebraica de Riccati es o o P= 0.0066 0.0088 , 0.0088 0.0153 cuya ganancia de Kalman es L= 0.2653 0.3519 (3)

El resultado de aplicar esta ganancia es el que se observa en la Figura 2.

0.5

0.4

0.3

0.2

0.1

0.1

0.2

0.3 Estado Real 0.4 Estado Estimado

0.5

5 t [s]

10

Figura 2: Simulaci n a lazo abierto. Estado real y estimado o

Ejemplo 2 (Filtro de Kalman discreto) Consideremos el sistema en tiempo discreto, el sistema (4) es la discretizaci n exacta del sistema 1, con perodo de muestreo T = 0.05s. o xk+1 = 0.8146405 0.0817367 0.0021886 0.0430570 x + u + v 0.0817367 0.8146405 k 0.0452456 k 0.0474342 k

(4)

y = 1 0 xk + wk , donde el t rmino de ruido v tiene media cero y covarianza V = 0.09. El ruido de medici n se asume de media e o cero y covarianza W = 0.25. Construimos un ltro de Kalman discreto para estimar la evoluci n del estado del sistema (4) cuando se o le aplica la entrada u = sen kT , con perodo de muestreo T = 0.05s, y sobre el intervalo kT [0, 10]s. Resumimos los pasos a seguir para programar el ltro de Kalman discreto. Partimos del conocimiento de las propiedades est discas, valor esperado y varianza de los ruidos vk , y wk , y la condici n inicial x0 . a o 1 Calculamos la estima a priori del estado (predicci n) o x = Ax + Buk , inicializadaconlaestimainicialx0 = E[x0 ].

2 Calculamos la ganancia de Kalman Lk+1 = [ASk AT + GV GT ]CT C[ASk AT + GV GT ]CT +W

1

T conS0 = E[x0 x0 ].

 Control Automatico II

Ejemplo Resuelto de Filtro de Kalman

Pagina 3 de 4

3 Calculamos la estima a posteriori, corregida con la salida yk+1 xk+1 = (I Lk+1C)(Ax + Buk ) + Lk+1 . 4 Calculamos la matriz de covarianza para la pr xima iteraci n o o
T Sk+1 = [I Lk+1C][ASk AT GV GT ][I Lk+1C]T + Lk+1W Lk+1 .

Este procedimiento no es difciles de programar en M ATLAB, por ejemplo de la siguiente manera:

% Ejemplo filtro de Kalman discreto % Sistema en tiempo continuo Ac=[-4,2;-2,-4]; Bc=[0;1]; Gc=[1;-1]; C=[1,0]; % Discretizacion exacta - Sistema en tiempo discreto T=0.05; % tiempo de muestreo A=expm(Ac*T); B=inv(Ac)*(A-eye(2,2))*Bc; G=inv(Ac)*(A-eye(2,2))*Gc; % Covarianzas de Ruidos V=0.09; % ruido de proceso W=0.025; % ruido de medicion % Condiciones iniciales del sistema (para simular) t=0:T:10; u=sin(t); x0=[0;0]; x=x0; y=C*x0; % Conjetura de xh=[0.5;-0.5]; xp=xh; S=eye(2,2); condiciones iniciales para el filtro de kalman % xh(0) % xp(0) % S0

% Simulacion for k=1:length(t)-1 % sistema x(:,k+1)=A*x(:,k)+B*u(k)+G*sqrt(V)*randn; y(k+1)=C*x(:,k+1)+sqrt(W)*randn; % filtro de Kalman inestacionario xp(:,k+1)=A*xh(:,k)+B*u(k); % estima a priori L=(A*S*A+G*V*G)*C*inv(C*(A*S*A+G*V*G)*C+W); xh(:,k+1)=xp(:,k+1)+L*(y(k+1)-C*xp(:,k+1)); % estima S=(eye(2,2)-L*C)*(A*S*A+G*V*G)*(eye(2,2)-L*C)+L*W*L; end

Corriendo este programa simulamos el sistema discreto y al mismo tiempo vamos calculando la estima del estado. La Figura 3 muestra la evoluci n de los estados del sistema y los estados estimados por el ltro o de Kalman inestacionario. Puede verse como las variables ruidosas son ltradas por el estimador, dando versiones suavizadas de la evoluci n de los estados luego de un breve transitorio de aproximadamente o 0.5 s.

 Control Automatico II

Ejemplo Resuelto de Filtro de Kalman

Pagina 4 de 4

0.6

0.4

x2 (t) x2 (t)

0.2

x1 (t)
0.2

x1 (t)

0.4

0.6 0 1 2 3 4 5 6 7 8 9 10

t[s]

Figura 3: Estados verdaderos y estados estimados para el sistema (4)