ejemplos filtros kalman control

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



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 ].




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.




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)

ejemplos filtros kalman control

Documents