 % pfc_observer_state
%clear
%close
tf = 1600;%duration
w= 1:1:tf;
tech=1; %sampling period
T=30;ap=exp(-tech/T); bp=1-ap; %cte  temps process
r=5;
%-----------------
u=zeros(1, tf);% variables
mv=u; Que=u; y=u;mvpid=u;
sm=u; pert=u; pertestim=u;
smm=u;CCons=u;y=u;sp=u;CC=u;
PPert=u;Pestim=u;Spert=u;D=u;G=u;
%pert=measured state perturbation
%---------
TRBF1= 80; %trbf  process tuning  (cltr!)
lh= 1-exp(-tech*3/TRBF1);
%----------------
TRBF2=2;%trbf estimator tuning
lhh=1-exp(-tech*3/TRBF2);
%--------------------------------------
 gtend =input(' gtend < 1! =  ');% 0<gtend<0.999
%gtend=0.65;%gain of prediction of perturbation
fl=input(' fl=1 with feed forward  fl=0 without feed forward = ')
%' classical feed forward with measured disturbance/ 0 no feed forward
Gm=1;
Gp=1;
for ii=50:1: tf
   pert(ii)=0;
   if ii>400;pert(ii)=20;end;% state disturbance
   if ii>1000;Gp=1.2;end
   %pfc
   %-------------
   sm(ii)=sm(ii-1)*ap+bp*Gm*mv(ii-1);%model process
   sp(ii)=sp(ii-1)*ap+bp*Gp*(mv(ii-1-r)+pert(ii-1-r));%process
   spred =sp(ii)+sm(ii)-sm(ii-r);%classical prediction time delay compensation
   % measured disturbance ,classical pfc compensation 
   Spert(ii)=Spert(ii-1)*ap+bp*PPert(ii-1);
   D(ii)=bp* ( PPert(ii-1)-Spert(ii-1-5));
   %classical pfc from shelf
   d= (50- spred)*lh+sm(ii)*bp-D(ii-1)*fl*gtend;%process pfc
   mv(ii)=d/bp;% actuator
   %non measured disturbance but with a global estimator
   %model
   y(ii)=y(ii-1)*ap+bp*CC(ii-1-r);%y = output of estimator
   smm(ii)=smm(ii-1)*ap+bp*CC(ii-1);%model
   spred= y(ii)+smm(ii)-smm(ii-r);
   %pfc global observer
   d= (sp(ii-1)-spred)*lhh+smm(ii)*bp;
   CC(ii)=d/bp;
   if ii <300; PPert(ii)=0;else;
   PPert(ii)=PPert(ii-1)*.95+0.05*(CC(ii)-mv(ii));end
   
end
crit=sum(abs(sp(400:tf)))
CRITstate=sum(abs(sp(406:700)-50))
CRITstruct=sum(abs(sp(950:1300)-50))
figure(1)
hold on
plot(w,pert,'g',w,mv,'k',w,sp,'r',w,PPert,'b',w,pert,'m')
grid on
zoom on
title( 'GLOBAL ESTIMATOR pert  g / mv k / sp / r / PPert b / pert m')


