bilinid.m

剑桥大学用于线性和双线性动力学系统辨识的工具箱

%jj
pack

% **************************************************** %
%                                                      %
%               STEP 2                                 %
%                                                      %
%  Construct S:=Y^p+U^p,u,y +U^f,u,y +U^r,u,y          %
%                                                      %
%  R=proj_S Y^f +U^f,u,y                               %
%                                                      %
%  Solve equation                                      %
%                                                      %
%  cproj(R,proj(S,Y^{r})=B_{k}*cproj(R, U^{r,u,y})     %
%                                                      %
% **************************************************** %

disp('Step 1/5. Decomposing the block equation...')

disp('...1/9...')
if algorithm ~= 42
  YUS=blociobi(UUU(1+3*i*m:4*i*m,:),YYY(1+3*i*p:4*i*p,:),i,p,m);
  UPLS=blocplbi(UUU(1+3*i*m:4*i*m,:),i,m); 
end

disp('...2/9...')
switch algorithm
 case {3,42}
  UUF=blociobi(UUU(1+i*m:2*i*m,:),UUU(1+i*m:2*i*m,:),i,m,m);
  UFU=krons(UUU(1+i*m:2*i*m,:),i,m);  
  UPLF=blocplbi(UUU(1+i*m:2*i*m,:),i,m); 
  UFY=khatri(UPLF,YYY(i*p+1:(i+1)*p,:));
  UFUY=[UUF;UFU;UFY];  clear UUF UFU UFY
  disp('...3/9...')
  UUR=blociobi(UUU(1+2*i*m:3*i*m,:),UUU(1+2*i*m:3*i*m,:),i,m,m);
  URU=krons(UUU(1+2*i*m:3*i*m,:),i,m); 
  UPLR=blocplbi(UUU(1+2*i*m:3*i*m,:),i,m); 
  URY=khatri(UPLR,YYY(2*i*p+1:(2*i+1)*p,:));
  URUY=[UUR;URU;URY];  clear UUR URU URY  
  disp('...4/9...')
  UPLP=blocplbi(UUU(1:i*m,:),i,m);
  if algorithm == 3
	UUS=blociobi(UUU(1+3*i*m:4*i*m,:),UUU(1+3*i*m:4*i*m,:),i,m,m);
	USU=krons(UUU(1+3*i*m:4*i*m,:),i,m); 
	USY=khatri(UPLS,YYY(3*i*p+1:(3*i+1)*p,:));
	USUY=[UUS;USU;USY]; clear UUS USU USY
	disp('...5/9...')
	YUF=blociobi(UUU(1+i*m:2*i*m,:),YYY(1+i*p:2*i*p,:),i,p,m);
	S=[YUF;UFUY;URUY;USUY;khatri(UPLP,UPLF);khatri(UPLF,UPLR); khatri(UPLR,UPLS)];
	clear YUF UFUY UPLP	
	YUR=blociobi(UUU(1+2*i*m:3*i*m,:),YYY(1+2*i*p:3*i*p,:),i,p,m);
	disp('...6/9...')
	R=[orthproj(YUR,S);URUY; khatri(UPLF,UPLR)];   % R=proj(S,Y^r+U^{r,u,y}+Ur*Uf) 
	clear YUR UPLF 	
  else
	clear UPLF UPLR		
	UPY=khatri(UPLP,YYY(1:p,:));  clear UPLP%  U^{p,y}
	UUP=blociobi(UUU(1:i*m,:),UUU(1:i*m,:),i,m,m);
	UPU=krons(UUU(1:i*m,:),i,m);  % U^{++}  
	UPUY=[UUP;UPU;UPY]; clear UUP UPU UPY   %  U^{p,u,y}
	disp('...5/9...')
	YUP=blociobi(UUU(1:i*m,:),YYY(1:i*p,:),i,p,m);
	S=[YUP;UPUY;UFUY;URUY];
	clear YUP UPUY	
	YUF=blociobi(UUU(1+i*m:2*i*m,:),YYY(1+i*p:2*i*p,:),i,p,m);
	disp('...6/9...')
	R=[orthproj(YUF,S);UFUY];  % R=proj(S,Y^f+U^{f,u,y}  	
  end
 case 41  
  UPLF=blocplbi(UUU(1+i*m:2*i*m,:),i,m); 
  UUP=blociobi(UUU(1:i*m,:),UUU(1:i*m,:),i,m,m);
  UFUY=[UPLF;khatri(UPLF,UUP)]; clear UPLF UUP
  disp('...3/9...')
  UPLR=blocplbi(UUU(1+2*i*m:3*i*m,:),i,m); 
  UUF=blociobi(UUU(1+i*m:2*i*m,:),UUU(1+i*m:2*i*m,:),i,m,m);
  URUY=[UPLR;khatri(UPLR,UUF)]; clear UPLR UUF  
  disp('...4/9...')
  UUR=blociobi(UUU(1+2*i*m:3*i*m,:),UUU(1+2*i*m:3*i*m,:),i,m,m);
  USUY=[UPLS;khatri(UPLS,UUR)]; clear UPLS UUR
  disp('...5/9...')
  YUF=blociobi(UUU(1+i*m:2*i*m,:),YYY(1+i*p:2*i*p,:),i,p,m);
  S=[YUF;UFUY;URUY;USUY]; clear YUF UFUY
  YUR=blociobi(UUU(1+2*i*m:3*i*m,:),YYY(1+2*i*p:3*i*p,:),i,p,m);
  disp('...6/9...')
  R=[orthproj(YUR,S);URUY];   % R=proj(S,Y^r+U^{r,u,y}+Ur*Uf)  
  clear YUR 
end

disp('...7/9...')
AA = coorproj(URUY,R); clear URUY % also used if algorithm == 42
rank1 = rank(AA);
row1 = size(AA);
if rank1 ~= row1
  warning('coorproj(URUY,R) is not row full rank.')
end  
clear rank1 row1

disp('...8/9...')
switch algorithm
 case 3
  AA=coorproj([USUY;khatri(UPLR,UPLS)],R);   
  disp('...9/9...')
  BB=coorproj(YUS,R);
 case 42
  YUR=blociobi(UUU(1+2*i*m:3*i*m,:),YYY(1+2*i*p:3*i*p,:),i,p,m);  
  disp('...9/9...')
  BB=coorproj(YUR,R); clear YUR 
 case 41
  AA=coorproj(USUY,R);  
  disp('...9/9...')
  BB=coorproj(YUS,R); 
end
clear R

disp('Step 2/5. Computing the constant matrix via pseudo-inverse...')
%BK = BB*pinv(AA);
BK = BB/AA; % finish here
clear AA BB

disp('Garbage collection...')
pack

% ************************************************************************** %
%                                                                            %
%               STEP 3                                                      %
%                                                                            % 
%  S1=Y_k|0+U^u,y_k|0+U^u,y_2k|k+U^u,y_3k-1|2k-1                             %
%                                                                            %
%  SVD decomposition                                                          %
%                                                                            %
%  [proj(S,Y_2k-1|k) : proj(S_1, Y_2k|k+1]                                   %
%               - B_k[U^{u,y}_2k-1|k  U^{u,y}_2k|k+1]                        %
%                       SVD=A_k[Xhat_k,  Xhat_k+1]                           %
%                                                                            %
% ************************************************************************** %

disp('Step 3/5. Constructing matrices for SVD decomposition...')

disp('...1/9...')
if algorithm ~= 42
  YK2K=blociobi(UUU((i+1)*m+1:2*(i+1)*m,:),YYY((i+1)*p+1:2*(i+1)*p,:),(i+1),p,m);
end

disp('...2/9...')
switch algorithm
 case 3
  UK2K=blociobi(UUU((i+1)*m+1:2*(i+1)*m,:),UUU((i+1)*m+1:2*(i+1)*m,:),(i+1),p,m);
  UFU1=krons(UUU((1+i)*m+1: 2*(i+1)*m,:),i,m);  
  UPLF1=blocplbi(UUU(1+(i+1)*m:2*(i+1)*m,:),i+1,m); 
  UFY1=khatri(UPLF1,YYY((i+1)*p+1:(i+2)*p,:));
  UFUY1=[UK2K;UFU1;UFY1]; clear UK2K UFU1 UFY1
  disp('...3/9...')
  U2K3K=blociobi(UUU(2*(i+1)*m+1:3*(i+1)*m,:),UUU(2*(i+1)*m+1:3*(i+1)*m,:),(i+1),p,m); 
  URU1=krons(UUU(1+2*(i+1)*m:3*(i+1)*m,:),i,m); 
  UPLR1=blocplbi(UUU(1+2*(i+1)*m:3*(i+1)*m,:),i+1,m); 
  URY1=khatri(UPLR1,YYY((2*i+1)*p+1:(2*i+2)*p,:)); 
  URUY1=[U2K3K;URU1;URY1]; clear U2K3K URU1 URY1 %(finish here)
  disp('...4/9...')
  U3K4K=blociobi(UUU(3*i*m+1:(4*i+1)*m,:),UUU(3*i*m+1:(4*i+1)*m,:),(i+1),p,m); 
  USU1=krons(UUU(1+3*i*m:(4*i+1)*m,:),i,m); 
  USY1=khatri(UPLR1,YYY((3*i+1)*p+1:(3*i+2)*p,:)); 
  USUY1=[U3K4K;USU1;USY1]; clear U3K4K USU1 USY1 %(finish here)
  disp('...5/9...')
  UPLP1=blocplbi(UUU(1:(i+1)*m,:),i+1,m);             %  U^{+}
  UPLS1=blocplbi(UUU(1+3*i*m:(4*i+1)*m,:),i+1,m);  
  S1=[YK2K;UFUY1;URUY1;USUY1;khatri(UPLP1,UPLF1);khatri(UPLF1,UPLR1);khatri(UPLR1,UPLS1)];
  clear YK2K UFUY1 URUY1 USUY1 UPLP1 UPLF1 UPLR1 UPLS1
  G1=[USUY;khatri(UPLR,UPLS)]; clear USUY UPLR UPLS
  disp('...6/9...')
  U2=blociobi(UUU((3*i+1)*m+1:(4*i+1)*m,:),UUU((3*i+1)*m+1:(4*i+1)*m,:),i,m,m);
  USU2=krons(UUU((3*i+1)*m+1: (4*i+1)*m,:),i,m);  	 
  UPLS2=blocplbi(UUU(1+(3*i+1)*m:(4*i+1)*m,:),i,m); 
  USY2=khatri(UPLS2,YYY((3*i+1)*p+1:(3*i+2)*p,:)); clear UPLS2
  USUY2=[U2;USU2;USY2]; clear U2 USU2 USY2
  disp('...7/9...')
  UPLR2=blocplbi(UUU(1+(2*i+1)*m:(3*i+1)*m,:),i,m); 
  UPLS2=blocplbi(UUU(1+(3*i+1)*m:(4*i+1)*m,:),i,m); 
  G2=[USUY2; khatri(UPLR2,UPLS2)]; clear USUY2 UPLR2 UPLS2
  Y1=YUS; clear YUS
  Y2=blociobi(UUU((3*i+1)*m+1:(4*i+1)*m,:),YYY((3*i+1)*p+1:(4*i+1)*p,:),i,p,m);
  disp('...8/9...')
  Y1=orthproj(Y1,S); clear S
  disp('...9/9...')
  Y2=orthproj(Y2,S1); clear S1

  disp('Step 4/5. Performing SVD decomposition...')
  [GAMMA,SIGMA,OMEGA] =svd([Y1 Y2]-BK*[G1 G2]); 
  clear G1 G2 Y1 Y2 BK GAMMA

 case 41
  
  UPLP1=blocplbi(UUU(1:(i+1)*m,:),i+1,m);             %  U^{+}
  UPLF1=blocplbi(UUU(1+(i+1)*m:2*(i+1)*m,:),i+1,m); 
  UFUY1=[UPLF1; khatri(UPLF1,UPLP1)]; clear UPLP1
  disp('...3/9...')
  UPLR1=blocplbi(UUU(1+2*(i+1)*m:3*(i+1)*m,:),i+1,m); 
  URUY1=[UPLR1; khatri(UPLR1,UPLF1)]; clear UPLF1 %(finish here)
  disp('...4/9...')
  UPLS1=blocplbi(UUU(1+3*i*m:(4*i+1)*m,:),i+1,m); 
  USUY1=[UPLS1; khatri(UPLS1,UPLR1)]; clear UPLS1 UPLR1 %(finish here)
  disp('...5/9...')
  S1=[YK2K;UFUY1;URUY1;USUY1]; clear YK2K UFUY1 URUY1 USUY1
  Y1=YUS; clear YUS
  disp('...6/9...')
  Y2=blociobi(UUU((3*i+1)*m+1:(4*i+1)*m,:),YYY((3*i+1)*p+1:(4*i+1)*p,:),i,p,m);
  disp('...7/9...')
  Y1=orthproj(Y1,S); clear S
  disp('...8/9...')
  Y2=orthproj(Y2,S1); clear S1
  disp('...9/9...')
  UPLS2=blocplbi(UUU(1+(3*i+1)*m:(4*i+1)*m,:),i,m); 
  switch lsmethod
   case 'ols'
	UPLR2=blociobi(UUU(1+(2*i+1)*m:(3*i+1)*m,:),UUU(1+(2*i+1)*m:(3*i+1)*m,:),i,m,m); 
   case 'cls'
	UPLR2=blocplbi(UUU(1+(2*i+1)*m:(3*i+1)*m,:),i,m); 
  end
  USUY2=[UPLS2;khatri(UPLS2,UPLR2)]; clear UPLR2 UPLS2

  disp('Step 4/5. Performing SVD decomposition...')
  [GAMMA,SIGMA,OMEGA] =svd([Y1 Y2]-BK*[USUY USUY2]); 
  clear Y1 Y2 BK USUY USUY2 GAMMA
  
 case 42
  U0K=blociobi(UUU(1:(i+1)*m,:),UUU(1:(i+1)*m,:),(i+1),p,m); 
  UPU1=krons(UUU(1:(i+1)*m,:),i,m);  % U^{++}
  UPLP1=blocplbi(UUU(1:(i+1)*m,:),i+1,m);             %  U^{+}
  UPY1=khatri(UPLP1,YYY(1:p,:)); clear UPLP1 %  U^{p,y}
  UPUY1=[U0K;UPU1;UPY1]; clear U0K UPU1 UPY1  %  U^{p,u,y}
  disp('...3/9...')
  UK2K=blociobi(UUU((i+1)*m+1:2*(i+1)*m,:),UUU((i+1)*m+1:2*(i+1)*m,:),(i+1),p,m);
  UFU1=krons(UUU((1+i)*m+1: 2*(i+1)*m,:),i,m);  
  UPLF1=blocplbi(UUU(1+(i+1)*m:2*(i+1)*m,:),i+1,m); 
  UFY1=khatri(UPLF1,YYY((i+1)*p+1:(i+2)*p,:)); clear UPLF1
  UFUY1=[UK2K;UFU1;UFY1]; clear UK2K UFU1 UFY1
  disp('...4/9...')
  U2K3K=blociobi(UUU((2*i-1)*m+1:3*i*m,:),UUU((2*i-1)*m+1:3*i*m,:),(i+1),p,m); 
  URU1=krons(UUU(1+(2*i-1)*m:3*i*m,:),i,m); 
  UPLR1=blocplbi(UUU(1+(2*i-1)*m:3*i*m,:),i+1,m);  
  URY1=khatri(UPLR1,YYY((2*i-1)*p+1:2*i*p,:)); clear UPLR1
  URUY1=[U2K3K;URU1;URY1]; clear U2K3K URU1 URY1%(finish here)
  disp('...5/9...')
  Y0K=blociobi(UUU(1:(i+1)*m,:),YYY(1:(i+1)*p,:),(i+1),p,m);
  S1=[Y0K;UPUY1;UFUY1;URUY1]; clear Y0K UPUY1 UFUY1 URUY1
  Y1=YUF; clear YUF
  Y2=blociobi(UUU((i+1)*m+1:(2*i+1)*m,:),YYY((i+1)*p+1:(2*i+1)*p,:),i,p,m);
  disp('...6/9...')
  Y1=orthproj(Y1,S); clear S
  disp('...7/9...')
  Y2=orthproj(Y2,S1); clear S1
  disp('...8/9...')
  U2=blociobi(UUU((i+1)*m+1:(2*i+1)*m,:),UUU((i+1)*m+1:(2*i+1)*m,:),i,m,m);
  UFU2=krons(UUU((i+1)*m+1: (2*i+1)*m,:),i,m);  	 
  disp('...9/9...')
  UPLF2=blocplbi(UUU(1+(i+1)*m:(2*i+1)*m,:),i,m); 
  UFY2=khatri(UPLF2,YYY((i+1)*p+1:(i+2)*p,:)); clear UPLF2
  UFUY2=[U2;UFU2;UFY2]; clear U2 UFU2 UFY2


  disp('Step 4/5. Performing SVD decomposition...')
  [GAMMA,SIGMA,OMEGA] = svd([Y1 Y2]-BK*[UFUY UFUY2]);
  clear Y1 Y2 BK UFUY UFUY2 GAMMA
end

% Choose model order
n = orderselect(n,SIGMA);
if i < n
  warning('Block size k < n. The estimated model might be poor.')
end

disp('Garbage collection...')
pack

% ********************************************************************** %
%                                                                        %
%               STEP 4                                                   %
%                                                                        % 
% Solve [Xhat_k+1]=[A N B] [      Xhat_k        ]                        %
%       [        ]=[     ] [U_k \otimes Xhat_k  ]                        %
%       [  y_k   ]=[C 0 D] [        U_k         ]                        %
%                                                                        %
%              AA = PPP * BB                                             %
%                                                                        %
% ********************************************************************** %

try
  SIGMA=SIGMA(1:n,1:n);
catch
  error('The block size is incompatible with this choice of order.')
end

OMEGA=OMEGA'; 
OMEGA=OMEGA(1:n,:);
XHAT=SIGMA^(0.5)*OMEGA; clear OMEGA
XHAT1=XHAT(:,1:jj);
XHAT2=XHAT(:,1+jj:2*jj);
clear XHAT
pack

if algorithm == 42
  AA=[XHAT2; YYY(i*p+1:(i+1)*p,:)];
  BB=[XHAT1;khatri(UUU(i*m+1:(i+1)*m,:),XHAT1);UUU(i*m+1:(i+1)*m,:)];
else
  AA=[XHAT2; YYY(3*i*p+1:(3*i+1)*p,:)];
  BB=[XHAT1;khatri(UUU(3*i*m+1:(3*i+1)*m,:),XHAT1);UUU(3*i*m+1:(3*i+1)*m,:)];
end

ANB = AA(1:n,:)/BB;
switch lsmethod
 case 'ols'
  disp('Step 5/5. Determining the system matrices using ordinary least squares...')
  if ~isDzero
	COD = AA(n+1:n+p,:)/BB;
	PPP = [ANB; COD]; clear COD
  else
	CO  = AA(n+1:n+p,:)/BB(1:n+n*m,:);
	PPP = [ANB; CO zeros(p,m)]; clear CO
  end  
 case 'cls'
  disp('Step 5/5. Determining the system matrices using constrained least squares...')
  if ~isDzero
	CD  = AA(n+1:n+p,:)/[BB(1:n,:); BB(n+n*m+1:n+n*m+m,:)];
	PPP = [ANB; CD(:,1:n) zeros(p,n*m) CD(:,n+1:n+m)]; clear CD
  else
	CC = AA(n+1:n+p,:)/BB(1:n,:);
	PPP = [ANB; CC zeros(p,n*m+m)]; clear CC
  end
end
clear ANB AA BB

%switch lsmethod
% case 'ols'
%  disp('Step 5/5. Determining the system matrices using ordinary least squares...')
%  PPP=AA/BB; clear AA BB
% case 'cls'
%  disp('Step 5/5. Determining the system matrices using constrained least squares...')
%  ANB = AA(1:n,:)/BB;
%  CD  = AA(n+1:n+p,:)/[BB(1:n,:); BB(n+n*m+1:n+n*m+m,:)];
%  PPP = [ANB; CD(:,1:n) zeros(p,n*m) CD(:,n+1:n+m)]; clear ANB CD
%  %OLD code - required Optimization toolbox
%  %CC=AA'; clear AA %DD XX == CC;
%  %DD=BB'; clear BB 
%  %CCC=CC(:); clear CC
%  %DDD=kron(eye(n+p),DD); clear DD
%  %EEE=zeros(p*n*m,(n+n*m+m)*(n+p));
%  %for kkk=1:p,
%  %  for  lll=1:n*m,
%  %    EEE((kkk-1)*n*m+lll,(n+n*m+m)*(n+kkk-1)+n+lll)=1;
%  %  end
%  %end
%  %XX=lsqlin(DDD,CCC,[],[],EEE,zeros(p*n*m,1),[],[],[],optimset('LargeScale','off'));
%  %clear CCC DDD EEE
%  %PPP=reshape(XX,n+n*m+m,n+p)'; clear XX
%end 

A=PPP(1:n,1:n);
N=PPP(1:n,n+1:n+n*m);
B=PPP(1:n,n+n*m+1:n+n*m+m);
C=PPP(n+1:n+p,1:n);
D=PPP(n+1:n+p,n+n*m+1:n+n*m+m);

model = bilin(A,B,C,D,N,[],Ts);
%[E,X0] = pe(model,data,'e'); clear E
%model = bilin(A,B,C,D,N,X0,Ts);

if nargout > 1
  extra.ls = PPP(n+1:n+p,n+1:n+n*m); % Should be 0 if good estimate.
  extra.sv = diag(SIGMA);
  clear PPP SIGMA

  disp('Garbage collection...')
  pack

% ***************************************************** %
%                                                       %
%               STEP 5                                  %
%                                                       %
%  Determine the noise covariance matrix                %
%                                                       %
%  [Epsilon_w]=[Xhat_k+1] [A N B] [     Xhat_k        ] %        
%              [        ]-[     ] [U_k \otimes Xhat_k ] % 
%  [Epsilon_v]=[  y_k   ] [C 0 D] [        U_k        ] %
%                                                       % 
% ***************************************************** %

  disp('Finally, determining the noise covariance matrix...')
  EPSILONW=XHAT2-A*XHAT1-B*UUU(i*m+1:(i+1)*m,:)-N*khatri(UUU(i*m+1:(i+1)*m,:),XHAT1);
  EPSILONV=YYY(i*p+1:(i+1)*p,:)-C*XHAT1-D*UUU(i*m+1:(i+1)*m,:); 
  clear YYY UUU XHAT1 XHAT2
  RES=[EPSILONW;EPSILONV]; clear EPSILONW EPSILONV

  extra.COV = cov(RES');
  extra.Q = extra.COV(1:n,1:n);
  extra.R = extra.COV(n+1:n+p,n+1:n+p);
  extra.S = extra.COV(1:n,n+1:n+p);

end

disp(' ')
disp('Done.')
disp(' ')
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                           %
%                                  END ALGORITHM                            %
%                                                                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% *** last line of bilinid.m ***