Post on 07-Jul-2018
8/19/2019 Ejemplos Filtros Kalman Control
http://slidepdf.com/reader/full/ejemplos-filtros-kalman-control 1/4
INGENIERIA EN AUTOMATIZACION Y CONTROL I NDUSTRIAL
Control Automatico II
Ejemplo Resuelto de Filtro de Kalman
UNIVERSIDAD NACIONAL 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
−2 −4
x +
0
1
u +
1
−1
v
y = 1 0 x + w.
(1)
donde el t ermino de ruido v(t ) tiene media cero y covarianza V = 0.09. El ruido de medici´ on se asume de
media cero y covarianza W = 0.25. El objetivo es 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 −0.5T
, con covarianza
de este estado inicial P0 = I 2×2.
Para describir completamente el filtro de Kalman, recurrimos a las ecuaciones
˙ x(t ) = A ˆ x(t ) + Bu(t ) + L(t ) ( y(t )−C ˆ x(t ))
L(t ) = P(t )C T W −1
P(t ) = AP(t ) + P(t ) AT −P(t )C T W −1CP(t )−GVGT
(2)
Para resolver num´ ericamente la ecuaci ´ on diferencial, usamos
[t,p]=ode45(’Ej_Kal’,[0 10],[0.1 0 0.1]);
donde el archivo Ej_Kal contiene la siguiente funci´ on:
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 4 5 6 7 8 9 10−0.02
0
0.02
0.04
0.06
0.08
0.1
t [s]
p22
p11
p12
= p21
0 1 2 3 4 5 6 7 8 9 10−0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
t [s]
l1
l2
Figura 1: Elementos de la matriz de covarianza P(t ) de la solucion numerica de la ecuacion diferencial
matricial de Riccati
Como resolver la ecuaci ´ on de Riccati es dif ıcil, consideramos la ganancia de Kalman en estado esta-
cionario. Los valores de P(t ) de la Figura 1 sugieren una buena aproximaci´ on. Para poder confirmar la
8/19/2019 Ejemplos Filtros Kalman Control
http://slidepdf.com/reader/full/ejemplos-filtros-kalman-control 2/4
Control Automatico II Ejemplo Resuelto de Filtro de Kalman Pagina 2 de 4
existencia de una ´ unica matriz P definida positiva soluci´ on de la ecuaci´ on algebraica de Riccati, debemos
primero verificar que ( AT ,C T ) sea estabilizable y / AT ,T T ) sea detectable, donde GVGT = T T T . Como se
verifican ambas condiciones, la soluci ´ on de la ecuaci ´ on algebraica de Riccati es
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 1 2 3 4 5 6 7 8 9 10−0.5
−0.4
−0.3
−0.2
−0.1
0
0.1
0.2
0.3
0.4
0.5
t [s]
Estado Real
Estado Estimado
Figura 2: Simulacion 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 =
0.8146405 0.0817367
−0.0817367 0.8146405
xk +
0.0021886
0.0452456
uk +
0.0430570
−0.0474342
vk
y = 1 0 xk + wk ,
(4)
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 (predicci ´ on)
˜ 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 A
T + GVGT ]C T +W −1
, conS 0 = E [ x0 xT 0 ].
8/19/2019 Ejemplos Filtros Kalman Control
http://slidepdf.com/reader/full/ejemplos-filtros-kalman-control 3/4
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 )( 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 GVGT ][ 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 condiciones iniciales para el filtro de kalman
xh=[0.5;-0.5]; % xh(0)
xp=xh; % xp(0)
S=eye(2,2); % 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.
8/19/2019 Ejemplos Filtros Kalman Control
http://slidepdf.com/reader/full/ejemplos-filtros-kalman-control 4/4
Control Automatico II Ejemplo Resuelto de Filtro de Kalman Pagina 4 de 4
0 1 2 3 4 5 6 7 8 9 10
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
x2(t )
x1(t ) ˆ x1(t )
ˆ x2(t )
t [s]
Figura 3: Estados verdaderos y estados estimados para el sistema (4)