% Simulacion del Observador de Luemberger
clear
close all
num=2;
den=[1 3 2];
[A,B1,C,D]=tf2ss(num,den);

F=[-3 -1;0 -5];
B=[1;0];
G=[-0.5;2.5];
% F=A-G*C
BG=[1 -0.5;0 2.5];
[Ab,Bb]=c2d(A,B,0.1);
[Fb,Bg]=c2d(F,BG,0.1);
Gd(1:2,1)=Bg(1:2,2);
x(1:2,1)=[0;0];
q(1:2,1)=[0;0];
ee=[0;0];
u=[0;0];
y=[0;0];
for k=1:100
    u(k)=1;
    y(k)=C*x(1:2,k);
    x(1:2,k+1)=Ab*x(1:2,k)+Bb*u(k);
    q(1:2,k+1)=Fb*q(1:2,k)+Bb*u(k)+Gd*y(k);
    ee(:,k)=x(:,k) - q(:,k);
end
figure
plot(y,'g')
hold on
plot(x','r')
plot(q','k')

legend('Salida','Estados Medidos','Estados Estimados')

figure
plot(x','r')
title('Estados')

figure
plot(q','b')
title('Estados Observados') 

figure; plot(ee')
title('Error Estimación')