% Simulacão do Observador de Luemberger
clear; close all;
s=tf('s');
Gp=1/((s+1)*(s+2));

num=[1];
den=[1 3 2];
[A,B,C,D]=tf2ss(num,den);
autovalores=eig(A);
% Matrices discretas de la planta
T=0.1;
[G,H]=c2d(A,B,T);

%F=G-L*C
%F(nxn); G(nxn); L(nx1); C(1xn);
% Autovalores del observador: lambda=-4 (plano s)
% z=0,67 en el plano z
% Matriz L
% L=[-1;5]; % dominio continuo
%L=[-0.126;0.383]; % dominio discreto
L=[0.173;-0.087];
F=G-L*C;
J=H;
polos=[0.98 0.9801];
Lp = place(G',C',polos).';

Lp=L;
Fp=G-Lp*C;

Kincerteza=0.999;  %0.989
Gm=Kincerteza*G;

% Inicialización de variables
m=150;
x=0.0*ones(2,m);
y=zeros(1,m);
r=zeros(1,m);
ee=zeros(2,m);
t=zeros(1,m);
u=zeros(1,m);
xe=0.0*ones(2,m);

for k=1:m
    t(k)=k*T;
    r(k)=1;
    y(k)=C*x(:,k);
    u(k)=r(k);
    xe(:,k+1)=Fp*xe(:,k) + Lp*y(k) + J*u(k);
    x(:,k+1)=Gm*x(:,k) + H*u(k);
    ee(:,k)=x(:,k) - xe(:,k);
end

figure; plot(y,'g')
title('Salida')

figure;
plot(t,x(1,1:k)',t,xe(1,1:k)')
title('Estados Reales y Estados Estimados')
legend('Estado Real x1','Estado Estimado x1')

figure;
plot(t,x(2,1:k)',t,xe(2,1:k)')
title('Estados Reales y Estados Estimados')
legend('Estado Real x2','Estado Estimado x2')

% figure
% plot(xe','r')
% title('Estados Estimados')  

figure; plot(ee')
legend('Error de Estimacion')
% axis([0 m -0.1 0.1])