% Simulacion drl 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.173;-0.087]; % dominio discreto
F=G-L*C;
J=H;

polos=[0.028 0.029];
Lp = place(G',C',polos).';

Fp=G-Lp*C;

% Ackermann
alfa_e=G^2 - 1.34*G + 0.4489*eye(2);
OB=obsv(G,C);

rank(OB)

Lack=(alfa_e/OB)*[0;1];

Lack=L;
% Lack=Lp;

Fack=G-Lack*C;

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

% Inicialización de variables
m=100;
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)=2;
    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(x','r');
% title('Estados verdaderos')
% grid
% 
% figure
% plot(xe','r')
% title('Estados Estimados')
% grid

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(ee'); %axis([0 200 -0.1 0.5])
title('Error de Estimacion')