%Exemplo LQR
clear all, close all
% sistema
h=0.5;
A=[1 h ; 0 1];
B=[h^2/2 ; h];
I=[1 0; 0 1];
C=[1 0];
G=eye(2);
H=1;
%ruidos
Cov=[h^2/3 h^2/2 ; h^2/2 h];
R1=1e-03*Cov; % matriz de covariancia do ruido v_t do sistema
re=1e-03; % variancia do ruido e_t de saida
%custos LQ
Q=[1 0 ;0 1]; % matriz de pesos do estado para o custo LQ
R=0.1; % peso do controle para o custo LQ

%sistema em malha aberta c/ ruido de entrada
sys_out=ss(A,[B G],C,0,-1,'inputname',{'u' 'v1' 'v2'},'outputname',{'y'});
sys_state=ss(A,[B G],eye(2),0,-1,'inputname',{'u' 'v1' 'v2'},'outputname',{'x1' 'x2'});

%Ganho do Controlador Otimo por realimentacao de estados
K = dlqr(A,B,Q,R);
sys_mf1 = feedback(sys_state, K , 1, [1 2],-1); % constroi objeto
M=[C*(I-A+B*K)^-1*B]^-1; % valor inverso do ganho estatico 
sys_mf1=series(M,sys_mf1,1,1); % ajuste para ganho unitario em regime

% simulacao do sistema
t=0:100; % vetor com instantes de simulacao
u = square((2*pi/40)*t,50)'; % sinal de controle com periodo de 40 pontos
figure(1), lsim(sys_mf1,u,t);

%Kalman Filter
[kest,L]=kalman(sys_out,R1,re);
kest.InputName={'u' 'ym'};

%Sistema em Malha Aberta e Filtro:
%conexao paralela: a 1a.entrada de cada um dos sistemas eh comum 
sys_paralelo=parallel(sys_out,kest, 1,1,[],[]);
% conexao para o filtro: saida medida do sistema e' uma das entradas do filtro
sysfil_ma=feedback(sys_paralelo,1,4,1,+1);

%simulacao do sistema em m.a. e filtro
n=length(t);
randn('seed',0)
v=randn(n,2)*sqrt(R1);
e=randn(n,1)*sqrt(re);

[out,t]=lsim(sysfil_ma, [v u H*e]);
y=out(:,1); ye=out(:,2);
figure(2), stairs(t,[y ye])
legend('saida', 'saida estimada'),grid, title('sistema em malha aberta') 

% sistema em malha fechada e filtro com entrada de referencia u
sysfil_mf=feedback(sys_paralelo,[1 0 0; 0 -K],[4 3],[1 3 4],+1);
sysfil_mf.b=sysfil_mf.b*diag([1 1 M 1]); % ajuste para ganho unitario em regime

%simulacao do sistema em m.f. e filtro
[out,t,x]=lsim(sysfil_mf, [v u H*e], t, [1 1 0 0]'); %estabelece condicoes inciais diferentes
y=out(:,1); ye=out(:,2);
esforco_controle= -out(:,2:3)*K'+M*u;
figure(3), stairs(t,[y ye]), grid, legend('saida', 'saida estimada')
figure(4), stairs(t,y-ye), grid, title('erro de estimacao')
figure(5), stairs(t,esforco_controle), grid, title('esforco de controle') 




