´ Y C ONTROL I NDUSTRIAL I NGENIER´IA I A EN A UTOMATIZACION ´ Control Autom atico 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
= + + +. = −4
x˙ y
−2
2 x −4
1 0 x
0 u 1
1 v −1
(1)
w
donde el t ermino ´ ´ de ruido v(t ) tiene media cero y covarianza V = 0 .09. El ruido ruido de medici medici´ on ´ se asume de media cero y covarianza W = 0 .25. El objeti objetivo vo es dise dise˜ nar ˜ un filtro de Kalman continuo para estimar las
variables de estado de (1) . Considermos el estado inicial de la planta x(0) = 0.5 de este estado inicial P0 = I 2×2 . Para describir completamente el filtro de Kalman, recurrimos a las ecuaciones
−0
T
.5 , con covarianza
)) x˙ˆ(t ) = A xˆ(t ) + Bu(t ) + L(t ) ( y(t ) − C xˆ(t )) L(t ) = P(t )C T W −1
(2)
P˙ (t ) = AP(t ) + P(t ) AT − P(t )C T W −1CP(t ) − GVG GV GT Para resolver num´ numericamente la ecuacion ´ ´ diferencial, usamos [t,p]=ode [t,p]=ode45(’E 45(’Ej_Ka j_Kal’,[0 l’,[0 10],[0.1 10],[0.1 0 0.1]); 0.1]);
donde el archivo Ej_Kal contiene la siguiente funci´ funcion: ´ function dp=Ej_Kal(t,p) dp = ze zeros(3,1);
% un un ve vector co columna
A=[-4 A=[-4 2;-2 2;-2 -4]; -4]; B=[0;1 B=[0;1]; ]; C=[1,0 C=[1,0]; ]; G=[1;G=[1;-1]; 1]; V=0.09 V=0.09; ; W=0.02 W=0.025; 5; P=[p(1),p(2);p(2),p(3)]; DP=A DP =A*P *P + P* P*A’ A’ - P* P*C’ C’*i *inv nv(W (W)* )*C* C*P P + G* G*V* V*G’ 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. 4
0.1
3.5 0.08 3
2.5
0.06
2 0.04 1.5
0.02
1
p
22
p
11
0.5
l
1
0 p = p 12
21
0 l
−0.02 0
1
2
3
4
5 t [s]
6
7
8
9
10 10
−0.5 0
1
2
3
4
2
5 t [s]
6
7
8
9
10 10
Figura Figura 1: Elemen Elementos tos de la matriz matriz de covaria covarianza nza P(t ) de la solucion o´ n num´ numerica e´ rica de la ecuaci on o´ n diferencial matricial de Riccati
Como resolver la ecuaci´ ecuacion consideramos la ganancia de Kalman en estado esta´ de Riccati es dif ´ ´ıcil, cionari cionario. o. Los valor valores es de P(t ) de la Figura 1 sugieren una buena aproximaci´ aproximacion. ´ Para poder poder confirm confirmar ar la
´ Control Autom atico II
´ Pagina 2 de 4
Ejemplo Resuelto de Filtro de Kalman
existencia de una unica matriz P definida positiva soluci´ on de la ecuaci´ on algebraica de Riccati, debemos ´ T T T T primero verificar que ( A , C ) sea estabilizable y / A , T ) sea detectable, donde GVGT = T T T . Como se verifican ambas condiciones, la soluci´ on de la ecuaci´ on algebraica de Riccati es
0.0066 P = −0.0088
.0088
−0
0.0153
,
0.2653 L = −0.3519
cuya ganancia de Kalman es
(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 Estado Estimado
−0.4
−0.5
0
1
2
3
4
5 t [s]
6
7
8
9
10
Figura 2: Simulaci´on a lazo abierto. Estado real y estimado
Ejemplo 2 (Filtro de Kalman discreto) Consideremos el sistema en tiempo discreto, el sistema (4) es la discretizaci´ on exacta del sistema 1, con per ´ıodo de muestreo T = 0.05s. xk +1
. = . =
y
0 8146405 0.0817367 0.0021886 xk + u + 0.8146405 0.0452456 k −0 0817367
1
0.0430570 vk −0.0474342
(4)
0 xk + wk ,
donde el t ermino de ruido v tiene media cero y covarianza V = 0.09. El ruido de medici´ on se asume de media ´ cero y covarianza W = 0.25. Construimos un filtro de Kalman discreto para estimar la evoluci´ on del estado del sistema (4) cuando se 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 adiscas, ´ valor esperado y varianza de los ruidos vk , y wk , y la condici´ on inicial x0 . 1 Calculamos la estima a priori del estado (prediccion) ´ x˜ = A xˆ + Buk ,
inicializadaconlaestimainicial x0 = E [ x0 ].
2 Calculamos la ganancia de Kalman Lk +1 = [ AS k AT + GVGT ]C T C [ AS k AT + GVG T ]C T + W
−1
,
conS 0 = E [ x0 xT 0 ].
´ Control Autom atico II
Ejemplo Resuelto de Filtro de Kalman
´ Pagina 3 de 4
3 Calculamos la estima a posteriori , corregida con la salida yk +1 xˆk +1 = ( I − Lk +1C )( A xˆ + Buk ) + Lk +1 . 4 Calculamos la matriz de covarianza para la pr oxima ´ iteraci´ on S k +1 = [ I − Lk +1C ][ AS k AT GVG T ][ I − Lk +1C ]T + Lk +1W LT k +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´ on de los estados del sistema y los estados estimados por el filtro de Kalman inestacionario. Puede verse como las variables ruidosas son filtradas por el estimador, dando versiones ¡¡suavizadas¿¿ de la evoluci´ on de los estados luego de un breve transitorio de aproximadamente 0.5 s.
´ Control Autom atico II
´ Pagina 4 de 4
Ejemplo Resuelto de Filtro de Kalman
0.6
0.4
xˆ2 (t ) x2 (t )
0.2
0
xˆ1 (t )
x1 (t ) −0.2
−0.4
−0.6 0
1
2
3
4
5
6
7
8
9
t [s]
Figura 3: Estados verdaderos y estados estimados para el sistema (4)
10
