%% Observadores por asignación de polos.
% © 2020, Antonio Sala Piqueras. Universitat Politècnica de València. Todos los derechos reservados.
% Vídeo-presentación en: http://personales.upv.es/asala/YT/V/obsml1.html
% *Observadores porasignación de polos (teoría), (32:27): http://personales.upv.es/asala/YT/V/obs.html
%
% Modelo del sistema de orden 2 cuyo estado queremos estimar a partir de 1 sensor:

A=[-2 -1;-3 -4]; B=[0;1];
C=[1 -1];%sensores disponibles
sys=ss(A,B,C,0);

%
%% Observador por Asignación de polos, diseño:
% Vamos a discretizar, para hacer control por computador.

Ts=0.1;
sysd=c2d(sys,Ts,'zoh'); %retenedor de orden cero, modela el convertidor D->A.
poloobserv=-8;

% y pasemos al plano discreto:

poloobserv_discr=exp(poloobserv*Ts)
dospolos=poloobserv_discr*[1 .98]
%observador discreto adelantado: polos de (A-LCA)= polos de (A'-A'C'L')
L=place(sysd.a',sysd.a'*C', dospolos)'

% Comprobemos los valores propios resultantes

eig(sysd.a-L*C*sysd.a)'

%% Simulación del observador:
% Simulemos unas cuantas muestras... Pongamos una entrada cualquiera...

Nmuestras_observ=35;
tiemposobserv=(0:(Nmuestras_observ-1))*Ts;
grx=zeros(2,Nmuestras_observ);
grxest=zeros(2,Nmuestras_observ);
gry=zeros(1,Nmuestras_observ);
xinicial=[4;1];
xestimadoinicial=[0;0];
x=xinicial;
xestimado=xestimadoinicial;
predictor=xestimado;
TIEMPOREAL=0; %estamos simulando!!!

for k=1:Nmuestras_observ
%     t=tiemposobserv(k);
    u=2*sin(4*t); 
    y=C*x+randn()*0.03; %leemos "ahora" sensor:
    gry(k)=y; %gráfica
    grx(:,k)=x;
    %observador LINEAL
    
    %aplicamos "corrector" del observador:    
    corrector=L*(y-C*predictor);
    xestimado=predictor+corrector;
    predecimos el siguiente estado...
    predictor=sysd.a*xestimado+sysd.b*u;
    grxest(:,k)=xestimado; %grafica de xestimado en unidades absolutas
    
    %dejamos pasar un muestreo antes de volver a medir...
    if(TIEMPOREAL)
         escribeactuadores(u);
         espera(Ts);
    else %simulacion
       x=sysd.a*x+sysd.b*u+randn(2,1)*0.05;
    end %if
end %for
plot(tiemposobserv,gry,'xr');
grid on
title('Medidas disponibles');

plot(tiemposobserv,grx')
hold on
plot(tiemposobserv,grxest','x');
hold off
title('estados reales/estimados')
xlabel('tiempo')
grid on
legend('x1','x2','x1est','x2est')


errorobservacion=grxest-grx;
plot(tiemposobserv,errorobservacion)
grid on
plot(tiemposobserv,log(sqrt(sum(errorobservacion.^2))))
grid on
ylabel('logaritmo  norma error')
xlabel('tiempo')


