Welcome to Scribd. Sign in or start your free trial to enjoy unlimited e-books, audiobooks & documents.Find out more
Download
Standard view
Full view
of .
Look up keyword
Like this
12Activity
0 of .
Results for:
No results containing your search query
P. 1
EJEMPLO - Filtro de Kalman

EJEMPLO - Filtro de Kalman

Ratings: (0)|Views: 526|Likes:
Published by saltacarditos

More info:

Published by: saltacarditos on Jun 28, 2011
Copyright:Attribution Non-commercial

Availability:

Read on Scribd mobile: iPhone, iPad and Android.
download as PDF, TXT or read online from Scribd
See more
See less

10/16/2013

pdf

text

original

 
I
NGENIER
´
IA EN
A
UTOMATIZACI
´
ON Y
C
ONTROL
I
NDUSTRIAL
Control Autom´atico IIEjemplo Resuelto de Filtro de Kalman
U
NIVERSIDAD
N
ACIONAL DE
Q
UILMES
1 de julio de 2004P´agina 1 de 4
Ejemplo 1 (Filtro de Kalman continuo)
Consideremos el sistema continuo dado por 
˙
 x
=
4 2
2
4
 x
+
01
u
+
1
1
v y
=
1 0
 x
+
w
.
(1)
donde el t ´ ermino de ruido v
(
)
tiene media cero y covarianza V 
=
0
.
09
. El ruido de medici´ on se asume demedia cero y covarianza W 
=
0
.
25
. El objetivo es dise˜ nar un filtro de Kalman continuo para estimar lasvariables de estado de
(1)
. Considermos el estado inicial de la planta x
(
0
) =
0
.
5
0
.
5
 , con covarianzade este estado inicial P
0
=
2
×
2
.Para describir completamente el filtro de Kalman, recurrimos a las ecuaciones
˙ˆ
 x
(
) =
A
ˆ
 x
(
)+
 Bu
(
)+
 L
(
)(
 y
(
)
ˆ
 x
(
))
 L
(
) =
P
(
)
1
˙
P
(
) =
AP
(
)+
P
(
)
 A
P
(
)
1
CP
(
)
GVG
(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 columnaA=[-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.
012345678910−0.0200.020.040.060.080.1t [s]p
22
 p
11
 p
12
= p
21
 
0 1 2 3 4 5 6 7 8 9 10−0.500.511.522.533.54t [s]
l
1
 l
2
 
Figura 1: Elementos de la matriz de covarianza
P
(
)
de la soluci´on num´erica de la ecuaci´on diferencialmatricial 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
(
)
de la Figura 1 sugieren una buena aproximaci´ on. Para poder confirmar la
 
Control Autom´atico II Ejemplo Resuelto de Filtro de Kalman
P´agina 2 de 4
existencia de una´ unica matriz P definida positiva soluci´ on de la ecuaci´ on algebraica de Riccati, debemos primero verificar que
(
 A
,
)
sea estabilizable y
/
 A
,
)
sea detectable, donde GVG
=
T
. Como severifican 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.
012345678910−0.5−0.4−0.3−0.2−0.100.10.20.30.40.5t [s]Estado RealEstado Estimado
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 ladiscretizaci´ on exacta del sistema 1, con per ´ ıodo de muestreo T 
=
0
.
05
s. x
+
1
=
0
.
8146405 0
.
0817367
0
.
0817367 0
.
8146405
 x
+
0
.
00218860
.
0452456
u
+
0
.
0430570
0
.
0474342
v
 y
=
1 0
 x
+
w
,
(4)
donde el t ´ ermino de ruido v tiene media cero y covarianza
=
0
.
09
. El ruido de medici´ on se asume de mediacero y covarianza
=
0
.
25
.Construimos un filtro de Kalman discreto para estimar la evoluci´ on del estado del sistema
(4)
cuando sele aplica la entrada u
=
sen
kT, con per ´ ıodo de muestreo T 
=
0
.
05
s, y sobre el intervalo kT 
[
0
,
10
]
s. Resumimos los pasos a seguir para programar el filtro de Kalman discreto. Partimos del conocimiento delas propiedades est ´ adiscas, valor esperado y varianza de los ruidos v
 , y w
 , y la condici´ on inicial x
0
.1 Calculamos la estima
a priori
del estado (predicci´ on)
˜
 x
=
A
ˆ
 x
+
 Bu
,
inicializadaconlaestimainicial
 x
0
=
[
 x
0
]
.
2 Calculamos la ganancia de Kalman L
+
1
= [
 AS
 A
+
GVG
]
[
 AS
 A
+
GVG
]
+
1
,
con
S
0
=
[
 x
0
 x
0
]
.
 
Control Autom´atico II Ejemplo Resuelto de Filtro de Kalman
P´agina 3 de 4
3 Calculamos la estima
a posteriori
 , corregida con la salida y
+
1
ˆ
 x
+
1
= (
 I 
 L
+
1
)(
 A
ˆ
 x
+
 Bu
)+
 L
+
1
.
4 Calculamos la matriz de covarianza para la pr ´ oxima iteraci´ on S
+
1
= [
 I 
 L
+
1
][
 AS
 A
GVG
][
 I 
 L
+
1
]
+
 L
+
1
WL
+
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 continuoAc=[-4,2;-2,-4]; Bc=[0;1]; Gc=[1;-1]; C=[1,0];% Discretizacion exacta - Sistema en tiempo discretoT=0.05; % tiempo de muestreoA=expm(Ac*T); B=inv(Ac)*(A-eye(2,2))*Bc;G=inv(Ac)*(A-eye(2,2))*Gc;% Covarianzas de RuidosV=0.09; % ruido de procesoW=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 kalmanxh=[0.5;-0.5]; % xh(0)xp=xh; % xp(0)S=eye(2,2); % S0% Simulacionfor k=1:length(t)-1% sistemax(:,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 inestacionarioxp(:,k+1)=A*xh(:,k)+B*u(k); % estima a prioriL=(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)); % estimaS=(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 estimadel estado. La Figura 3 muestra la evoluci´ on de los estados del sistema y los estados estimados por el filtrode Kalman inestacionario. Puede verse como las variables ruidosas son filtradas por el estimador, dandoversiones ¡¡suavizadas¿¿ de la evoluci´ on de los estados luego de un breve transitorio de aproximadamente0.5 s.

You're Reading a Free Preview

Download
scribd
/*********** DO NOT ALTER ANYTHING BELOW THIS LINE ! ************/ var s_code=s.t();if(s_code)document.write(s_code)//-->