´ I NGENIER´A 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 filtro 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 = I2×2 . Para describir completamente el filtro 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)

4

3.5
0.08

3

0.06

2.5

2
0.04

1.5

0.02

p

22 11

1

p 0 p
12

0.5
=p

l1

21

0 l2

−0.02

0

1

2

3

4

5 t [s]

6

7

8

9

10

−0.5

0

1

2

3

4

5 t [s]

6

7

8

9

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 dif´cil, 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 confirmar la o 

´ Control Automatico II

Ejemplo Resuelto de Filtro de Kalman

´ Pagina 2 de 4

existencia de una unica matriz P definida 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 verificar que (A verifican 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

−0.1

−0.2

−0.3 Estado Real −0.4 Estado Estimado

−0.5

0

1

2

3

4

5 t [s]

6

7

8

9

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 per´odo 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 filtro de Kalman discreto para estimar la evoluci´ n del estado del sistema (4) cuando se o le aplica la entrada u = sen kT , con per´odo de muestreo T = 0.05s, y sobre el intervalo kT ∈ [0, 10]s. ı Resumimos los pasos a seguir para programar el filtro 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 dif´ciles 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 filtro o de Kalman inestacionario. Puede verse como las variables ruidosas son filtradas 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

0

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)