www.pudn.com > Closid30.rar > Cltailoe.m, change:1998-07-16,size:6481b


function [thoe]=cltailoe(z,nn,sc,thi,tsamp) 
 
% SYNTAX : [thoe]=cltailoe(z,nn,sc,thi) 
% PURPOSE: performs closed loop identification on the basis of measurements of 
%          r(t) and y(t) in z=[y r] with a tailor-made parametrization given by  
%  
%            y(t)=inv(1+C G) G r(t)  
% 
%          where G is parametrized in a numerator/denominator form with an 
%          output error structure with order nn=[nb nf nk], and C is the known  
%          controller with system matrix sc. 
%  
% OUTPUT: thoe   : Estimated model in theta-format   
%            
% Optional input : thi is an (optional) initial parameter vector; 
%                  tsamp is the sampling interval. 
 
% Functions called: poly2th,th2ss,clsplit,dtrend,ss2tf,dlsim,arx,oe,th2poly, 
%                   feedback 
 
% 31-10-1996 
% (c) Edwin van Donkelaar, Paul Van den Hof 
% Mechanical Engineering Systems and Control Group 
% Delft University of Technology 
% Last update: 16-06-1997; 
%              16-07-1998: correction model structure nk>1 in initial estimate 
 
if nargin<5, tsamp=1; end 
if nargin<4, thi=[]; end 
 
% Check input variables 
y=z(:,1); 
r=z(:,2); 
 
[ac,bc,cc,dc]=clsplit(sc); 
nc=length(ac); 
[numc,denc]=ss2tf(ac,bc,cc,dc); 
numc=numc/denc(1);denc=denc/denc(1); 
N=length(r); 
 
if length(nn)~=3; 
 error('Order not specified correctly') 
end 
 
na=nn(2);nb=nn(1);nk=nn(3); 
if nk==0 
  if dc~=0 
   disp('No direct feedthrough estimated. Controller not strictly proper!') 
   nk=1;  
   nb=nb+nk; 
   nn=[nb na nk]; 
  end 
end 
 
 
%%%%% Determine Sylvester matrices and M and rho 
 
n1=max(na+nc,(nb+nk-1)+nc); 
Pd1=zeros(n1,na); 
for i=1:na 
 Pd1(i:i+nc,i)=denc'; 
end 
 
 
Pn=zeros(n1,nb); 
for i=1:nb 
 if nk==0 
  Pn(i:i+nc-1,i)=numc(2:nc+1)'; 
 else 
  Pn(i:i+nc,i)=numc'; 
 end 
end 
 
Pd2=zeros(nb+nc,nb); 
for i=1:nb 
 Pd2(i:i+nc,i)=denc'; 
end 
 
M=[Pd1 Pn;zeros(nb+nc,na) Pd2]; 
rho=[denc(2:nc+1) zeros(1,max(na,nb+nk-1))]'; 
 
%%%%% Initial estimate for iteration 
 
if ~isempty(thi) 
  if max(abs(roots([1 [[Pd1 Pn]*thi+rho]'])))>1 
   error('WARNING: initial model not stabilized by the controller, iteration stopped') 
  end 
else 
% Initial estimate using direct ARX  
 u=r-dlsim(numc,denc,y);u=dtrend(u,1); 
 
 th0=arx([y u],[na nb nk]); 
 [a,b,c,d,f]=th2poly(th0); 
 thi=[a(2:na+1)';b(1+nk:nb+nk)']; 
  
 if max(abs(roots([1 [[Pd1 Pn]*thi+rho]'])))>1 
  th0=oe([y r],[nb+nc n1 nk]);  %correction: [nb+nc n1 1]-->[nb+nc n1 nk]; 16/07/98 
   [a,b,c,d,f]=th2poly(th0); 
   th0=[f(2:n1+1)';b(1+nk:nb+nc+nk)']; 
   thi=M\(th0-[rho;zeros(nb+nc,1)]); 
 end 
 if max(abs(roots([1 [[Pd1 Pn]*thi+rho]'])))>=1 
  error('WARNING: initial model not stabilized by the controller, iteration stopped'),return 
 end 
end 
 
 % Determine the output prediction for thi 
% [a,b,c,d]=tf2ss([0 thi(na+1:na+nb)'],[1 thi(1:na)']); 
 thix=poly2th([1 thi(1:na)'],[zeros(1,nk) thi(na+1:na+nb)']); [a,b,c,d]=th2ss(thix); 
 [aps,bps,cps,dps]=feedback(a,b,c,d,ac,bc,cc,dc,-1); 
 yhat=dlsim(aps,bps,cps,dps,r);yhat=dtrend(yhat,1); 
 
 % Determine initial loss function 
 V=(y-yhat)'*(y-yhat); 
 disp(['Initial loss function: ', num2str(V/N)]); 
 
%%%%% OPTIMIZATION 
 
 
% Initialisation 
 
th=thi; 
maxiter=50; 
k=0; 
kold=0; 
l=0; 
stop=0; 
 
% Start the iteration 
while [stop~=1 k<maxiter ] 
    Vold=V;     
    %                   -1 
    % Determine filter F(q,theta)      
     denfinv=[1 ([Pd1 Pn]*th+rho)']; 
     numfinv=[1 zeros(1,n1)]; 
 
    % Determine the regression matrix 
     phi=zeros(N,n1+nb+nc); 
     for i=1:n1 
      phi(:,i)=[zeros(i,1); -yhat(1:N-i,1)]; 
     end 
     for i=1:nb+nc        
       phi(:,n1+i)=[zeros(i-1+nk,1); r(1:N-i+1-nk,1)]; 
     end 
 
    % Determine filtered regression matrix 
     phif=zeros(N,n1+nb+nc); 
     for i=1:n1 
      phif(:,i)=dlsim(numfinv,denfinv,[zeros(i,1); -yhat(1:N-i,1)]); 
     end 
     for i=1:nb+nc        
       phif(:,n1+i)=dlsim(numfinv,denfinv,[zeros(i-1+nk,1); r(1:N-i+1-nk,1)]); 
     end 
 
 
    % Determine the gradient vector of the output prediction and criterion  
    % function 
     dyhatdth=phif*M; 
     dVdth=dyhatdth'*(yhat-y); 
    % Determine an approximation of the Hessian      
      Rhat=dyhatdth'*dyhatdth; 
      if 1/cond(Rhat)<1e-10 
        Rhat=Rhat+1e-4*eye(na+nb,na+nb); 
      end 
 
    % Determine the new parameter estimate and value for alpha 
     alpha=1;  
     % Check whether Phat is stabilized by C otherwise adjust alpha 
     ok1=0; 
     j=0; 
     while [ok1==0 j<25 stop==0] 
         thx=th-alpha*inv(Rhat)*dVdth; 
         if max(abs(roots([1 [[Pd1 Pn]*thx+rho]'])))>1 
           alpha=alpha/2; 
           j=j+1; 
           if j==25 
             disp('WARNING: no model stabilizable by C(z) found in search direction, iteration stopped') 
             stop=1;  
           end  
         else          
            ok1=1;  
         end   
     end 
 
     % Check whether the criterion function decreases otherwise decrease alpha 
 
     ok=0; i=0;alphax=alpha;      
     while [i<15  ok==0 stop==0]        
       % Determine the output prediction 
%       [a,b,c,d]=tf2ss([zeros(nk,1); thx(na+1:na+nb)]',[1 ;thx(1:na)]'); 
       thx1=poly2th([1 thx(1:na)'],[zeros(1,nk) thx(na+1:na+nb)']); 
       [a,b,c,d]=th2ss(thx1); 
       [aps,bps,cps,dps]=feedback(a,b,c,d,ac,bc,cc,dc,-1); 
       yhat=dlsim(aps,bps,cps,dps,r);yhat=dtrend(yhat,1); 
 
       % Determine new loss function 
       V=(y-yhat)'*(y-yhat); 
       % Check deviation in loss functions 
       if Vold>V 
          ok=1; 
          th=thx;  
       else           
          alphax=alphax/2; 
          thx=th-alphax*inv(Rhat)*dVdth;         
          i=i+1; 
          if i==15 
           disp('WARNING: no improvement in search direction, iteration stopped') 
           stop=1;  
          end  
       end 
     end             
 
     % Check whether the criterion value is unaltered for more than 3 iterations 
     if stop==0 
     if abs(Vold-V)< 1e-4 
      if k-kold==1 
       l=l+1; 
       if l==3  
         disp('OK: stationary point reached, iteration stopped') 
         stop=1;  
       end   
      else 
       l=0;   
      end 
     end 
     end 
  
     % Display new loss function if the iteration has not stopped 
     if stop==0      
       disp(['New loss function: ', num2str(V/N), '  Alpha: ', num2str(alpha)]);   
       kold=k; 
       k=k+1; 
     end 
end 
% 
thoe=poly2th(1,[zeros(1,nk) th(na+1:na+nb)'],1,1,[1 th(1:na)'],[],tsamp);