LABORATORIO DE CONTROL MECATRONICO III

FILTROS DE KALMAN
I.

Objetivo:
Aplicar los filtros de Kalman como alternativa ptima
ante las seales de ruido que
pueden afectar en
forma muy negativa a los sistemas de control
automtico

II. Marco Terico:

CONCEPTO GENERAL:
El filtro Kalman es un mtodo de estimacin cuyos
parmetros se corrigen en cada iteracin dependiendo del
error de prediccin que se haya cometido en la iteracin
anterior. Es un estimador lineal y ptimo desde el punto
de vista de mnimos cuadrados, que ha ganado aceptacin
en el anlisis de series de tiempo.
El filtro
de
Kalman es
un algoritmo desarrollado
por Rudolf E. Kalman en 1960 que sirve para poder
identificar el estado oculto (no medible) de un sistema
dinmico lineal,
al
igual
que
el observador
de
Luenberger, pero sirve adems cuando el sistema est
sometido a ruido blanco aditivo.1 La diferencia entre
ambos es que en el observador de Luenberger, la ganancia
K de realimentacin del error debe ser elegida "a mano",
mientras que el filtro de Kalman es capaz de escogerla
de forma ptima cuando se conocen las varianzas de los
ruidos que afectan al sistema.
El objetivo del filtro de Kalman es estimar los estados
de una manera ptima, de manera que se minimiza el
ndice del error cuadrtico medio.
El siguieten diagrama de bloques representa el filtro de
Kalman asociado a un proceso determinado:

LABORATORIO DE CONTROL MECATRONICO III

De manera que su evolucin es expresada en espacio de estados

por:

Siendo:
x(k)
y(k)
w(k)
v(k)

Estado
Salida u observacin del sistema.
proceso estocstico asociado a la medida
proceso estocsticas asociado al sistema

A, B, C matrices determinsticas que definen la dinmica del

sistema
El filtro de Kalman propone un mtodo para obtener un
estimador ptimo del estado. Suponemos que x(k) es la
estimacin en el instante k del estado. El filtro de Kalman
buscar obtener ese valor de estimacin de manera que se
minimice el error cuadrtico medio. Definiendo el error como
la diferencia entre el valor real del estado y la estimacin:

El objetivo por tanto ser minimizar:

A la matriz P(n) se la conoce como matriz de covarianza del

error. Dependiendo del valor que tome n en [2], tendremos
distintas representaciones del filtro de Kalman:
Si n = k+1, el filtrado ser de prediccin.
Si n = k, el filtrado ser de alisado.

LABORATORIO DE CONTROL MECATRONICO III

III. PROCEDIMIENTO
Ejemplo 1 (Filtro de Kalman continuo):
Consideremos el sistema continuo dado por:

Donde el termino de ruido v(t) tiene media cero y

covarianza V = 0.09. El ruido de medicin se asume de media
cero y covarianza W = 0.25. El objetivo es disear un
filtro de Kalman continuo para estimar las variables de
estado de (1). Consideramos el estado inicial de la planta
x(0) =[0.5 0.5]T , con covarianza de este estado inicial
P0 = I22.
Para
describir
completamente
el
filtro
de
Kalman,
recurrimos a las ecuaciones deduciendo del diagrama de
bloques del filtro de Kalman:

Para resolver numricamente la ecuacin diferencial, usamos

[t,p]=ode45(Ej_Kal,[0 10],[0.1 0 0.1]);

LABORATORIO DE CONTROL MECATRONICO III

La funcin ode45 est basada en el mtodo de Runge-Kutta
explcito de Matlab. Es capaz de resolver numricamente un
conjunto de ecuaciones diferenciales de primer orden
Con esta funcin podemos determinar los valores finales del
filtro de Kalman
Donde el archivo Ej_Kal contiene la siguiente funcin:
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);

PROGRAMA EN MATLAB

GRAFICA 1
clc;clear all
[t,p]=ode45('Ej_Kal',[0 10],[0.1 0 0.1]);
figure(1)
plot(t,p)

GRAFICA 2

LABORATORIO DE CONTROL MECATRONICO III

Teniendo los Valores de los Pesos de la P, que es la
matriz de covariancia ptima del error del estimador de
estado, podemos calcular la ganancia de Kalman
Para hallar esta ganancia de Kalman, si las seales v y w
no estn correlacionadas, el valor de Kk se determina
mediante la frmula:

Definimos vectores a para obtener los pesos P de la matriz

de covarianza ptima
a=size(p);
P=[p(a(1),1),p(a(1),2);p(a(1),2),p(a(1),3)]
W=[0.025]
C=[1,0]
Teniendo los valores necesarios para hallar la ganancia de
Kalman se realiza un bucle for para que halle todos los
valores de la ganancia de Kalman con los valores de la 1ra
grafica obtenida.

for i=1:a(1)
P=[p(i,1),p(i,2);p(a(1),2),p(i,3)];
LA=P*(C')*(W^-1);
L(1,i)=LA(1);
L(2,i)=LA(2);
end
Finalmente hallamos la transpuesta de esta solucin apra
poder graficarla en funcin del tiempo.
L=L';
figure(2)
plot(t,L)
De donde obtenemos el L ptimo final que sera:

Como se muestra en la figura siguiente:

LABORATORIO DE CONTROL MECATRONICO III

Las grficas anteriores muestran los elementos de la matriz

de covarianza P(t) de la solucin numrica de la ecuacin
diferencial matricial de Riccati
Como
resolver
la
ecuacin
de
Riccati
es
difcil,
consideramos la ganancia de Kalman en estado estacionario.
Los valores de P(t) de la primera grfica
sugieren una
buena aproximacin.

GRAFICA 3
Como resolver la ecuacin de Riccati es difcil,
consideramos la ganancia de Kalman en estado estacionario.
Los valores de P(t) de la Grfica 1 sugieren una buena
aproximacin. Para poder confirmar la existencia de una
nica matriz P definida positiva solucin de la ecuacin
algebraica de Riccati, debemos primero verificar que (AT
,CT ) sea estabilizable y /AT ,TT ) sea detectable, donde
GVGT = TTT . Como se verifican ambas condiciones, la
solucin de la ecuacin algebraica de Riccati es:

%% CALCULAMOS EL LQR DE LA PLANTA

sys1=ss(A,B,C,D);
x0=[0.5;-0.5];
Q=C'*C;
R=1;
K=lqr(A,B,Q,R)
%% HACEMOS UNA PRIMERA REALIMENTACION CON EL LQR
Ar=A-B*K;

LABORATORIO DE CONTROL MECATRONICO III

Br=[1 0;0 0];
Cr=C;
Dr=[0 1];
SYS=ss(Ar,Br,Cr,Dr);

El diagrama de esta realimentacin es el que se observa en

el siguiente diagrama de bloques:

%% GENERACION DE RUIDO CON MEDIA CERO Y COVARIANZA DETERMINADA

t_aum=0:0.01:10; %Tiempo de evaluacion de Sistema
w_aum=mvnrnd(0,W,length(t_aum));
v_aum=mvnrnd(0,V,length(t_aum));
z_aum=[w_aum,v_aum]; %Union de Ruido
Nn=W-V; %Covarianza entre W y V
figure(4)
plot(t_aum,z_aum)

LABORATORIO DE CONTROL MECATRONICO III

%% SISTEMA COMPLETO CON MATRICES G Y H DE INTRODUCCION DE RUIDO

SYSk=ss(A,[B G],C,[D H]);
[KEST,Kk,PP]=kalman(SYSk,V,W); %Hallar parametros de kalman
RLQG=lqgreg(KEST,K);
LAZO=feedback(sys1,RLQG,+1); %Realimentacion con Kalman
[Acc,Bcc,Ccc,Dcc]=ssdata(LAZO); %Separacion de Partes Espacio
Estado

El diagrama de esta realimentacin es el que se observa en

el siguiente diagrama de bloques:

%% VARIACION IDEA 01:11PM VILCA H.

%PARA INSERCION DE RUIDO Y SEAL SENOIDAL DE ENTRADA
Acc=Acc;
bc=[G;0;0];
bcc=[0;0;B];
Bcc=[Bcc,bc,bcc];
Ccc=Ccc;
Dcc=[Dcc 0 0];
LAZO=ss(Acc,Bcc,Ccc,Dcc); %Sistema en Espacio Estados

LABORATORIO DE CONTROL MECATRONICO III

%% CONDICIONES INICIALES
x0_aum=[x0;0;0]; %Condiciones iniciales del valor real
%% GRAFICA DE ESTADOS
u=sin(t_aum); %Seal senoidal U
[YSS,TSS,XSS]=lsim(LAZO,[u' w_aum u'],t_aum,x0_aum);
figure(3)
plot(t_aum,XSS)
legend('X1','X2','X1e','X2e');
XLABEL('t(s)')

Grafica final: ESTADO REAL Y ESTADO ESTIMADO