% Observador de Luemberger de Orden Reducida
clear; close all;

% Definición de las matrices de estado del sistema
% x1=theta,  x2=w,  x3=ia
Ra=2; La=5e-3; b=0.01; J=2; Kt=1; Kb=0.2; 
A=[0 1 0;0 -b/J Kt/J;0 -Kb/La -Ra/La];
B=[0;0;1/La];
C=[1 0 0]; D=0;

CTRB_SC=rank(ctrb(A,B));
OSVB_SC=rank(obsv(A,C));
T=0.01;
sysc=ss(A,B,C,D);
[num,den]=ss2tf(A,B,C,D);
Gp=tf(num,den);
Gpd=c2d(Gp,T);

[G,H]=c2d(A,B,T);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matrices del observador
Aaa=G(1,1); Aab=[G(1,2) G(1,3)]; Aba=[G(2,1);G(3,1)]; Abb=[G(2,2) G(2,3);G(3,2) G(3,3)];
Ba=H(1,1); Bb=[H(2,1);H(3,1)];

% Raiz deseada del estimador reducido: z=0.819
polos=[0.9 0.95];

% Funcion place
L = place(Abb',Aab',polos).';

K=[4.52 1.12 0.05];
Kr=1/0.0791;
% Inicialización de variables
m=4000;
x=zeros(3,m);
y=zeros(1,m);
r=zeros(1,m);
t=zeros(1,m);
u=zeros(1,m);
xb=zeros(2,m);
xo=zeros(3,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) K(3)]*xb(:,k) + Kr*r(k);
    x(:,k+1)=G*x(:,k) + H*u(k);
end

figure; plot(t,x(:,1:k)')
title('estados medidos')

figure; plot(t,xb')
title('estados estimados')