% example 4.6, Aström_Adaptive Control, pag. 155
% MA control of a nonminimum-phase system
% based on the system Ay(t)=Bu(t)+Ce(t) 
% using extended least squares (els) estimation and 
% minimum variance controller: d=1
% retardo: tau=0.4 --> 0.6 al tiempo t=100
% elaboro E.Quintero.M 10sep'98
%----------------------------------------------------
clear
% parametros
time=0; tf=400; tau=0.4;
lambda=0.99; % forgetting factor
h=1;    % sampling period
ulim=20;   % limit of control signal
th1=0; th2=1; th3=0; th4=0; % initial estimates
p11=100; p22=100; p33=100; p44=100;  % initial covariances
p12=0; p13=0; p14=0; p23=0; p24=0; p34=0;
A1=-1; B0=1-tau; B1=tau; C1=-0.8;
f1=0; f2=0; f3=0; f4=0; 
u=0; s0=0; V=0; c2=0; a2=0;
% initial 
randn('seed',2); 
i=1; e=randn(tf/h+1,1);
while(time<tf)
%--------------------------------------------------------
      if(time==100) B0=0.4; B1=0.6; end     
      y=A1*f1+B0*f2+B1*f3+C1*f4+e(i);
      i=i+1;
      if(time==0) xx=[time y u th1 th2 th3 th4]; end
%--------------------------------------------------------

%     sort
%     1.0 parameter estimation
%     1.1 computation of p*f and estimator gain k
      pf1=p11*f1+p12*f2+p13*f3+p14*f4;
      pf2=p12*f1+p22*f2+p23*f3+p24*f4;
      pf3=p13*f1+p23*f2+p33*f3+p34*f4;
      pf4=p14*f1+p24*f2+p34*f3+p44*f4;
      denom=lambda+f1*pf1+f2*pf2+f3*pf3+f4*pf4;
      k1=pf1/denom;
      k2=pf2/denom;
      k3=pf3/denom;
      k4=pf4/denom;

%     1.2 update estimates and covariances
      eps=y-f1*th1-f2*th2-f3*th3-f4*th4;
      th1=th1+k1*eps;
      th2=th2+k2*eps;
      th3=th3+k3*eps;
      th4=th4+k4*eps;
      p11=(p11-pf1*k1)/lambda;
      p12=(p12-pf1*k2)/lambda;
      p13=(p13-pf1*k3)/lambda;
      p14=(p14-pf1*k4)/lambda;
      p22=(p22-pf2*k2)/lambda;
      p23=(p23-pf2*k3)/lambda;
      p24=(p24-pf2*k4)/lambda;
      p33=(p33-pf3*k3)/lambda;
      p34=(p34-pf3*k4)/lambda;
      p44=(p44-pf4*k4)/lambda;

%     2.0 control design

%     2.1 rename parameters
      a1=th1;
      b0=th2;
      b1=th3;
      c1=th4;

%     2.2 solve the polynomial identity AR+BS=AoAm
      s0=(c1-a1)/b0;
      r1=b1/b0;

%     3.0 control law with anti-windup
      v=-s0*y-r1*u;
      if(v<-ulim) u=-ulim; elseif(v<ulim) u=v; else u=ulim; end

%     1.3 update and filter regression vector
      f1=-y;
      f3=f2;
      f2=u;
      f4=eps;

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time y u th1 th2 th3 th4];
end
format short e
time,a1,b0,b1,c1
subplot(2,1,1),plot(xx(:,1),xx(:,2)),axis([0 tf -5 5])
subplot(2,1,2),plot(xx(:,1),xx(:,3))
%pause
%subplot(2,1,1),plot(xx(:,1),xx(:,4),xx(:,1),xx(:,5))
%subplot(2,1,2),plot(xx(:,1),xx(:,6),xx(:,1),xx(:,7))
