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

% num=[0 0 1];
% den=[1 1 0];
% [A,B,C,D]=tf2ss(num,den);

T=0.1;
% Matrices discretas de la planta
% x(k)=[posicion  velocidad]'
G=[1  0.0952;0  0.905]; H=[0.00484;0.0952];
C=[1 0]; D=0;

Gc=[0 1;-4.52 -1.12];
Hc=[0;4.52];
Cc=[1 0]; Dc=0;

t=0:T:10;
figure; step(Gc,Hc,Cc,Dc,1,t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Raices deseadas: lambda(1)=0.888+j*0.173 lambda(2)=0.888-j*0.173
% Vector de ganancias por ubicacion de polos
K=[4.52 1.12];
[num,den]=ss2tf(Gc,Hc,Cc,Dc);
Gsc=tf(num,den); Glc=minreal(Gsc/(1+Gsc));
% La ganancia cc para este sistema con realimentacion de estados resulta
% igual a 0.2214
Kr=1/0.2214;

% Raiz deseada del estimador reducido: z=0.819
pdo=0.819;
% Matrices del observador
Aaa=G(1,1); Aab=G(1,2); Aba=G(2,1); Abb=G(2,2);
Ba=H(1,1); Bb=H(2,1);
% Ackermann
alfa_er=Abb-pdo; OBR=Aab;
L=alfa_er/OBR*1;

% Funcion place o acker
polos=pdo;
Lp = place(Abb',Aab',polos).';

% Inicialización de variables
m=70;
x=zeros(2,m);
y=zeros(1,m);
r=zeros(1,m);
t=zeros(1,m);
u=zeros(1,m);
xb=zeros(1,m);
xo=zeros(2,m);
x(1,:)=0*ones(1,m);
xb(:) =0*ones(1,m);
for k=2:m
    t(k)=k*T;
    r(k)=1;
    y(k)=x(1,k);
    xo(:,k)=[x(1,k);xb(k)];
    xb(k)=(Abb-L*Aab)*xb(k-1) + L*y(k) + (Aba-L*Aaa)*y(k-1) + (Bb-L*Ba)*u(k-1);
    u(k)=-K(1)*x(1,k) - K(2)*xb(k) + Kr*r(k);
    x(:,k+1)=G*x(:,k) + H*u(k);
end

figure; plot(t,x(1,1:k)','r',t,x(2,1:k)','k')
title('estados medidos')
legend('posicion', 'velocidad')

figure; plot(t,xb','b',t,x(2,1:k)','k')
legend('velocidad estimada','velocidad medida')