Filtro Kalman

´
I NGENIER´A EN AUTOMATIZACI ON Y C ONTROL I NDUSTRIAL
I
´
Control Automatico II
Ejemplo Resuelto de Filtro de Kalman

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
−4 2
0
1
x+
u+
v
−2 −4
1
−1

x=
˙

(1)

y = 1 0 x + w.
donde el t´ rmino de ruido v(t ) tiene mediacero 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

(2)

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

0.1

3.5
0.08

3

2.5

0.06

2
0.04

1.5

0.02

1

p

22

p

11

0.5

l1

0
p

12

=p

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 sugierenuna buena aproximaci´ n. Para poder confirmar la
o

´
Control Automatico II

´
Pagina 2 de 4

Ejemplo Resuelto de Filtro de Kalman

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

L=

cuya ganancia de Kalman es

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
Estado Estimado

−0.4

−0.5

0

1

2

3

4

5
t [s]

6

7

89

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 seguirpara 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 +...