Está en la página 1de 13

UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO

FACULTAD DE INGENIERIA MECANICA P.A.2019


DAIA

Laboratorio No 8 Diseño del Filtro de Kalmann - Bucy

Sea el siguiente sistema continuo, donde W y V son señales de ruido que afectan al
vector de estados y a la salida respectivamente:

X  AX  BU  W Wnx1
(1)
Y  CX  V Vpx1

El mismo puede verse representado en el diagrama siguiente

Utilizando el principio de dualidad y la teoría de control óptimo podemos diseñar un


observador, denominado Filtro de Kalmann-Bucy, que minimice el efecto del ruido en
las variables de estado estimadas.
El sistema dual de (1) es:
Z  AT Z  C T S (2)
y el funcional costo a minimizar es:

J   (Z T Qo Z  S T Ro S ).dt
0

Donde Qo y Ro son matrices simétricas definidas positivas que sirven de pesos para el
ruido en los estados (W) y en la salida (V) respectivamente. Puede demostrarse que
dichas matrices Qo y Ro corresponden a las matrices de covarianza de las señales de
ruido W y V (ruidos blancos).
La realimentación obtenida a través de Ricatti para este sistema dual es:
S  K 0 .Z
Las matrices L y Ao del observador completo a construir son entonces
L  K 0T A0  A  LC

Profesora del Curso MT227 – R.M.G 1


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

CALCULO DEL CONTROL ÓPTIMO

Para la resolución de la ecuación de Ricatti, MATLAB posee una función que calcula la
matriz P (solución de la ecuación), la matriz K (matriz de realimentación) y la ubicación
de los autovalores del sistema a lazo cerrado L resultantes de la realimentación a través
de la matriz K. Dicha función se denomina lqr

Para el cálculo de matrices de realimentación de estados la sintaxis a utilizar es la


siguiente:

[K,P,E]=lqr(A,B,Q,R)

y para el cálculo de la matriz L del filtro de Kalman:


[Ko,Po,Eo]=lqr(A',C',Qo,Ro)
L = Ko'

donde Qo,Ro deben calcularse como las varianzas de las señales de ruido W y V.
En el caso de la ecuación de Riccati, las matrices de covarianza a utilizarse se obtienen
con la función cov de MATLAB. La misma que se describe a continuación:
COV Covariance matrix.
COV(X), if X is a vector, returns the variance. For matrices,
where each row is an observation, and each column a variable,
COV(X) is the covariance matrix.
.....
Filtro de Kalman - Matlab

El estimador de estados de Kalman o conocido como filtro de Kalman se obtiene


usando las siguientes ecuaciones:

Modelo de la Planta:
x  Ax  Bu  Bw (1)

Salida:

y  Cx  Du  Hw  v (2)

Con w y v modelados con ruido blanco.

Modelo del Estimador o Filtro:


xˆ  Axˆ  Bu  L( y  Cxˆ  Du )

Con entrada u (control) e y (medida con ruido). La covarianza del ruido


E[ w(t ) w(t   )T ]  Q0 , E[v(t )v(t   )T ]  R0

Profesora del Curso MT227 – R.M.G 2


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

determinar la ganancia de Kalman L atravez de la ecuación de Riccati.


El filtro de Kalman es un estimador ópitmo que tiene que ver con el ruido blanco
Gaussiano. Especificamente, este minimiza la covarianza asintótica del error de estimación,
(x - x̂ ):

lim Ex  xˆ  x  xˆ   min


T

t 

Use la función kalman del Matlab para diseñar el filtro de Kalman.

[kest,L,S] = kalman(sys_kf,Qo,Ro,N);

Retorna el modelo de espacio estado (kest) del estimador de Kalman dando el modelo
sys_kf y los datos de la varianza del ruido, Qo, Ro, y N. El modelo de la planta están
dados por las ecuaciones (1) y (2) donde w y v son modelados con ruido blanco. L es la
ganancia de Kalman y S la matriz de Covarianza. Si N es 0 (D=0), Ud. puede omitirla.

Problema 1 (Aula)
Se dispone de un controlador ruidoso para controlar la posición de una Antena rígida, x1 , de
un solo canal con la siguiente dinámica:

Planta controlada

Donde w1 (t ) yw2 (t ) son las señales de ruido blanco que corresponden al ruido de la
rad 2
posición y la velocidad, no correlacionadas entre sí, con intensidades: I w  0.0025 y
1
Hz
rad 2
I w2  0.05 , respectivamente.
seg 2 Hz
a) Determine la dinámica de la planta controlada: x  Ac x  Bw , donde B ∈ ℝ2×2 , w ∈
ℝ2×1 .
b) Determine la matriz de Covarianza de los estados controlados, P, usando Lyapunov:
AP+PAT+BWBT=0, donde W=diag(𝐼𝑤1 , 𝐼𝑤2 ) ¿Cuál es la desviación estándar de la
posición y que significa?

Profesora del Curso MT227 – R.M.G 3


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

c) Determine la ganancia del filtro de Kalman estacionario si se conoce que la varianza del
ruido de la medida es R=0.25*10-2 y muestre la ecuación dinámica a implementar del
filtro de Kalman.
d) Simule la dinámica del proceso (planta y filtro con las señales de los ruidos) usando
Simulink/Matlab . Considere el punto inicial x0  [0.1 0]T y xˆ0  [0 0]T .
e) Muestre el diagrama de simulación, los siguientes gráficos:
Figura 1: Contiene y_ sensor, y_ideal, e y_estimado.
Figura 2: estados 1 y 2 vs estados estimados.
Obtenga la covarianza del error de estimación de la salida usando Matlab
Considere los ruidos con el comando de Simulink (Random Number).
Solución
a)
𝑥̇ 0 1 𝑥1 0
[ 1] = [ ][ ] + [ ]𝑢
𝑥̇ 2 0 −1 𝑥2 0.1
𝑥1
𝑦 = [1 0] [𝑥 ]
2

0.0025 0
W=matriz de intensidades =[ ] en un ruido Gaussiano centrado en cero, no
0 0.05
correlacionado (ruido blanco) se considera igual a la matriz de covarianza.

𝑢 = −[10 20]{𝑥 + 𝑤}
Sistema controlado
𝑥̇ 0 1 𝑥1 0 0
[ 1] = [ ] [𝑥 ] + [ ]𝑤
𝑥̇ 2 −2 −2 2 −2 −1

b) Para determinar las propiedades estadísticas del sistema controlado (A=a-b*k) se usa
lyapunov considerando Q (solución) la covarianza de los estados controlados en un
proceso estocástico.

AQ+QAT+BWBT=0
0 1 𝑞1 𝑞2 𝑞1 𝑞2 0 −2 0 0 0.0025 0 0 −2 0 0
[ ][ ]+[ 𝑞3 ] [1 −2]+[−2 −1] [ 0 ][ ]=[ ]
−2 −2 𝑞2 𝑞3 𝑞2 0.05 0 −1 0 0

2𝑞2 𝑞3 − 2𝑞2 − 2𝑞1 0 0 0 0


[ ]+[ ]=[ ]
𝑞3 − 2𝑞2 − 2𝑞1 −4𝑞2 − 4𝑞3 0 0.06 0 0
𝑞2 = 0
0.06
−4𝑞2 − 4𝑞3 = −0.05  𝑞3 = 4 = 0.015
0.015 0.0075 0
𝑞3 − 2𝑞2 − 2𝑞1 = 0  𝑞1 = = 0.0075
𝑄=[ ]
2 0 0.015
La desviación estándar del estado 1 estocástico será √0.0075 =0.0866 =5 grados, esto
significa que el controlador está manteniendo a la antena con 5 grados de diferencia con
respecto de la posición deseada.
c) Diseño del Filtro de Kalman
LQE: AS+SA-SCTV-1CS+Qw=0 Ojo en la prueba se tomó: Qw =BTQlyapB , se le considera ,
pero realmente sería: Qw =BTWB
0 1 𝑠1 𝑠2 𝑠1 𝑠2 0 −2 𝑠1 𝑠2 1 𝑠1 𝑠2 0 0 0.0025 0 0 −2 0 0
[ ][ 𝑠3 ]+[𝑠2 𝑠3 ] [1 −2] − [𝑠2 𝑠3 ] [0] 400[1 0] [𝑠 𝑠3 ] + [−2 −1] [ 0 ][ ]=[ ]
−2 −2 𝑠2 2 0.05 0 −1 0 0

Profesora del Curso MT227 – R.M.G 4


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

2𝑠2 𝑠3 − 2𝑠2 − 2𝑠1 𝑠 2 𝑠1 𝑠2 0 0 0 0


[ ]-400 [ 1 2 ] +[0 0.06] = [0 0]
𝑠3 − 2𝑠2 − 2𝑠1 −4𝑠2 − 4𝑠3 𝑠1 𝑠2 𝑠2

2𝑠2 − 400𝑠1 2 = 0
0.031 0.002
𝑠3 − 2𝑠2 − 2𝑠1 − 400𝑠1 𝑠2 = 0 𝑆=[ ]
0.002 0.0127

4𝑠2 + 4𝑠3 + 400𝑠2 2 = 0.06

L=SCTV-1

1.2532
𝐿=[ ]
0.7852
𝑦̅ = 𝐶𝑥 + 𝑣 (y alterado)

𝑥̂̇ −1.2532 1 𝑥̂1 1.2532


[ 1] = [ ][ ] + [ ] 𝑦̅
𝑥̂̇2 −2.7852 −2 𝑥̂2 0.7852
salida
1 0 𝑥̂1
𝑧=[ ][ ]
0 1 𝑥̂2
d)
prob2simu.mdl

e)
A=[0 1; -2 -2];
B=[0 0; -2 -1];
C=[1 0]
D=zeros(1,2);
W=diag([0.0025 0.05]);
Q=lyap(A,B*W*B')
R=0.25e-2;
sys=ss(A,B,C,D)
[KEST,L,P] = kalman(sys,B*W*B',R);
sim('prob2simu');
[ cov(e_est1) cov(e_est2)]

Profesora del Curso MT227 – R.M.G 5


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

Covarianzas de los errores de estimación


cov(e_est1)= 0.00025 cov(e_est2)= 0.00021 , lo cuál indica que las covarianzas
tienden a cero.
considerando la covarianza Q en el diseño del filtro de Kalman
0.0027 0.0014
L=[ 1.0628 ; 0.5648] 𝑆 = [ ]
0.0014 0.0096
cov(e_est1)= 0.00028 cov(e_est2)= 0.00022 . los resultados tienen la misma
precisición.

Profesora del Curso MT227 – R.M.G 6


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

% Ejemplo del Filtro de Kalman

1000
G ( s)  Planta Nominal
s  10 s  1000
2

sys=ss(tf(1000,[1 10 1000]));
s1=ss(sys.a,[sys.b sys.b],sys.c,sys.d);
s1.inputname={'u' 'w'};
s1.outputname={'y'};

% La perturbación y el ruido se modelan con señales


% gaussianas de media cero y covarianza conocida

Qn=0.1;Rn=0.2;

%filtro de Kalman B , B , y Matriz


correspondiendo a v
[kest,L,P]=kalman(s1,Qn,Rn);

% sistema alterado por el ruido

alterado=ss(sys.a,[sys.b sys.b 0*sys.b],...


[sys.c;sys.c ],[0 0 0;0 0 1]);
alterado.inputname={'u' 'w' 'v'}
alterado.outputname={'y' 'yv'};

% Creación de una onda cuadrada con 3 segundos de periodo,


% duración de 6 segundos y muestre cada 0.01 segundos.

[u,tt]=gensig('square',3,6,0.01);

% Ampliando el tamaño de la señal de entrada a 2


u=2*u;
% Creación de los ruidos Gaussianos

randn('seed',0);
w=sqrt(Qr)*randn(length(tt),1);
v=sqrt(Rr)*randn(length(tt),1);

% Simulación

[yideal,t]=lsim(alterado,[u,0*w,0*v],tt);
[yalterado,t]=lsim(alterado,[u,w,v],tt);
[yestim,t]=lsim(kest,[u,yalterado(:,2)],t);
y=yalterado(:,1);
yv=yalterado(:,2);
ye=yestim(:,1);

Profesora del Curso MT227 – R.M.G 7


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

emedida=y-yv;
eobs=y-ye;
[cov(emedida) cov(eobs)]

figure(1)
subplot(311); plot(t,yideal(:,1))
subplot(312); plot(t,yv,'k:',t,y,'r-',t,ye,'b--');
subplot(313); plot(t,emedida,'g:',t,eobs,'k-');

Gráfico 1- Señal sin Ruido


Gráfico 2- Señal con Ruido y señal estimada con el filtro de
Kalman.
Gráfico 3- Señal de los errores medidos y observados

Gráfico 1- Señal sin Ruido


4

-2
0 1 2 3 4 5 6
Gráfico 2- Señal con Ruido y señal estimada con el filtro de Kalman
6

-2
0 1 2 3 4 5 6
Gráfico 3- Señal de los errores medidos y observados
2

-1

-2
0 1 2 3 4 5 6

Regulador LQG

Para formar el regulador LQG, simplemente conecte el filtro de Kalman y la ganancia K


óptima como se muestra a continuación:

Profesora del Curso MT227 – R.M.G 8


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

Este regulador tiene las ecuaciones de estado:


Asumiendo que se tiene el filtro de Kalman, kest, y el
compensador, K, usamos el comando lqgreg para
crear el regulador LQG.
Sintaxis
Regulador = lqgreg(kest, K);

Problema 2

Se tiene una planta con el siguiente modelo:


x (t )  ax(t )  b(u (t )  w(t ))
, con a  1, b  1, c  1
y (t )  cx(t )  v(t ) 2

Siga los siguientes pasos para diseñar el control de este tipo de sistemas:

a) (2.0 p)Encuentre la ley de control u(t) que minimice el índice de costo J:


T 
 
J  lim E  y T y  u T u dt 
T 
0 
b) Diseñe la ganancia del filtro de Kalman (Lk) considerando las varianzas de los ruidos
w(t) y v(t) como 1 y 1/32 respectivamente.

c) Complete con valores el diagrama de simulación del sistema controlado con


compensador LQG, incluya los ruidos.

Profesora del Curso MT227 – R.M.G 9


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

d) Use Simulink y/o Matlab y presente en un gráfico la salida observada y estimada a una
entrada escalón unitario. Use el bloque Band Limited White-Noise para generar los
ruidos blancos. El gráfico correspondiente al error de estimación de la medida y la
varianza del error de estimación E[𝑥̃ 𝑇 𝑥̃] .
Solución
Problema 3
a)
x (t )   x(t )  u (t )  w(t )
y (t )  12 x(t )  v(t )
Control LQR
1
𝑄 = 𝐶 𝑇 𝐶 = 4 R=1
Aplicando la ecuación de Riccati
1
2𝑝 + 𝑝2 − 4 = 0
√5−2
𝑝= = 0.118
2
√5−2
𝑘= = 0.118
2

𝑢 = 0.118𝑥̂
b) Filtro de Kalman
Aplicando LQE:
1
−2𝑠 − 32 4 𝑠 2 + 1 = 0
1
𝑠=4

Profesora del Curso MT227 – R.M.G 10


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

32 1
𝐿= =4
4 2

Gráfico correspondiente a la salida estimada (izquierda) y al error de


estimación(derecha)

var(x_error) % obtenida a partir del bloque workspace en el diagrama de Simulink

ans =

1.6455e-05

Profesora del Curso MT227 – R.M.G 11


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

Problema – tarea

Levitador Magnético –LQ y LQG


Se considera un simple modelo de Levitador magnético, cuyo esquema es mostrado en la
figura, el cual corresponde a las siguientes ecuaciones diferenciales:
𝐾𝑚 𝑖 2 + 𝐾0
𝑚𝑥̈ = 𝑚𝑔 −
𝑥2
Asumiendo con el vector de estados 𝑥 = [𝑥1 , 𝑥2 ]𝑇 = [𝑥, 𝑥̇ ]𝑇 , el modelo es re-escrito
como:
𝑥̇ 1 = 𝑥2 𝐾 𝑖2 + 𝐾
1 (𝑚𝑔 − 𝑚 2 0 )
𝑥̇ 2 = 𝑥1
𝑚
En el levitador está instalado un sensor que permite medir la posición de la masa
suspendida. La señal de salida es proporcionada por el sensor. Depende de la posición x de
la masa de acuerdo a la relación:
𝑦 = 𝐾𝑣 𝑥 + 𝑣0
Dar los parámetros del problema mostrados en la tabla y definir 𝑔 = 9.81[𝑚 𝑠 −2 ]:
1. Definir el modelo en simulink, en el cuál la corriente i(t) es la variable de entrada y la salida
y(t).
2. A partir del modelo en simulink anteriormente definido, conseguir el modelo linealizado
alrededor del punto de equilibrio 𝑥𝑒 = [1, 0]𝑇 correspondiente a la variable de entrada en
2 −𝐾
𝑚𝑔𝑥1,𝑒 0
equilibrio 𝑖𝑒 = √ , donde 𝑥1,𝑒 es la primera componente del vector de estados en el
𝐾𝑚
equilibrio.
3. Evaluar la estabilidad del sistema linealizado.
4. Para el sistema diseñe el regulador óptimo que penalice la salida con el peso Qy =1 y la entrada
1 ∞
con R=20 según el siguiente índice de costo: = 2 ∫0 (𝑦 𝑇 𝑄𝑦 𝑦 + 𝑢𝑇 𝑅𝑢)𝑑𝑡. Simule la respuesta
del sistema con retroalimentación óptima de estados a partir de la condición inicial 𝛿𝑥0 =
[0.1 , 0]𝑇 (tiempo de simulación: 3 seg.). Grafique en una sola figura (use subplot) la variable
de control y la de salida.
5. Para crear un regulador basado en la retroalimentación de los estados estimados a partir de la
salida y(t), diseñe un estimador asintótico de estados ( escogiendo adecuadamente los valores
propios ) después verificar la observación completa del sistema. Grafique en una sola figura
(use subplot) la variable de control y la de salida y en otra figura los estados estimados y
verdaderos.
6. En lugar del modelo lineal, considere el sistema no lineal y simule nuevamente con las
condiciones iniciales 𝑥𝑒 + 𝛿𝑥0 ( Note que existen la siguiente relación entre las variables
lineales y no lineales: 𝛿𝑥 = 𝑥 − 𝑥𝑒 , 𝛿𝑢 = 𝑢 − 𝑢𝑒 , 𝛿𝑦 = 𝑦 − 𝑦𝑒 ). Graficar en una sola gráfica la
variable de control u(t) y la salida y(t).

Nota:
Considere para el diseño del Observador las varianzas del proceso, 𝜎𝑤2 = 0.005, y del ruido
𝜎𝑉2 = 0.001 u otros valores adecuados de varianzas que Ud. considere pertinente.
Compare con la ubicación de polos.

Profesora del Curso MT227 – R.M.G 12


UNIVERSIDAD NACIONAL DE INGENIERIA CONTROL MODERNO Y OPTIMO
FACULTAD DE INGENIERIA MECANICA P.A.2019
DAIA

Profesora del Curso MT227 – R.M.G 13

También podría gustarte