This action might not be possible to undo. Are you sure you want to continue?
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)
