subid3b.m

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

%                                                                         %
%  [h_{1}   :                          :                               ]  %
%  [        : Pi_{S}Y_{u}-T_{2i-1}U_{u}:Pi_{S+Y_{i}}Y_{L}-T_{2i-1}U_{L}]  %
%  [h_{2i-1}:                          :                               ]  %
%                           [Sigma_{1} 0][Omega^{*}_{1}]                  %
%  :=[Gamma_{1} Gamma_{2}]*                               (4.1)           %
%                           [0         0][Omega^{*}_{2}]                  %
%                                                                         %
%                                                                         %
% *********************************************************************** %
  
T2=bloctoep([H1; H2(1:end-p,:)],zeros((2*i-1)*p,m),2*i-1);
%  T_{2i-1}=T2
%  T2=[h_0,..,0;h_1,h_0,...,0;...;h_2i-2,...,h_0]
UU=UUU(i*m+1:(3*i-1)*m,:);
UL=UUU((i+1)*m+1:3*i*m,:);  
YU=YYY(i*p+1:(3*i-1)*p,:);
YL=YYY((i+1)*p+1:3*i*p,:);
PSYU1=orthproj(YU,S)-T2*UU; clear UU YU
UI=UUU(i*m+1:(i+1)*m,:);
YI=YYY(i*p+1:(i+1)*p,:);
SYI=[S;YI];
PSYU2=orthproj(YL,SYI)-T2*UL; clear UL YL
if strcmpi(method,'mp')
  H=zeros((2*i-1)*p,m);
  H=[H1(p+1:end,:);H2];  % H=[h_1;h_2;...;h_2i-1] 
  [GAMMA,SIGMA,OMEGA]=svd([H PSYU1 PSYU2]);
else
  [GAMMA,SIGMA,OMEGA]=svd([PSYU1 PSYU2]);
end
clear PSYU1 PSYU2

% Select model order
n = orderselect(n,SIGMA);
if i <= n
  warning('Block size k <= n. The estimated model might be poor.')
end
 
try
  SIGMA1=SIGMA(1:n,1:n); clear SIGMA
catch
  error('The block size is incompatible with this choice of order.')
end



GAMMA1=GAMMA(:,1:n); clear GAMMA
%GAMMA2=GAMMA(:,(n+1):(2*i-1)*p);
OMEGA=OMEGA';
OMEGA1=OMEGA(1:n,:); clear OMEGA
%if method is 'si' or 'ss', then OMEGA2=OMEGA(:,(n+1):2*jj);
%else if method is 'mp', then OMEGA2=OMEGA(:,(n+1):2*jj+m);

switch method
  case {'si'}

   % ************************************************************ %
   %                                                              %
   %                STEP 5                                        %
   %                                                              %
   %  O_{2i-1}=Gamma1*Sigma1^{1/2}                                %
   %                                                              %
   %  [Pi_{S}X_{k} : Pi_{S+Y_{k}}X_{k+1}]=Sigma1^{1/2}Omega1'     %
   %                                                              %
   % ************************************************************ %
   
   O2=GAMMA1*SIGMA1^0.5;  %  O2=O_{2i-1}
   SIGOM=SIGMA1^0.5*OMEGA1;
   XKHAT=SIGOM(:,1:jj);  % hat(x_k)
   XKKHAT=SIGOM(:,jj+1:2*jj); % hat(x_{k+1})
   
   % ********************************************************* %
   %                                                           %
   %               STEP 6                                      %
   %                                                           %
   %   Determine A and C from O_{2i-1} by using the shift      %
   %                                                           %
   %   invariance property.                                    %
   %                                                           %
   %********************************************************** %
   
   C=O2(1:p,:);
   %A=pinv(O2(1:2*(i-1)*p,:))*O2((p+1):(2*i-1)*p,:);
   A=O2(1:2*(i-1)*p,:)\O2((p+1):(2*i-1)*p,:);
   
   % ************************************************************ %
   %                                                              %
   %               STEP 7                                         %
   %                                                              %
   %  Reconstruct O_2i.  Solve for B and D from                   %
   %                                                              % 
   %           ||      [YF]                              [UF] ||  % 
   % B,D=argmin||Pi_{S}    -O_{2i}Pi_{S}X_{k}-T_{2i}(B,D)     ||  %
   %      B,D  ||      [YR]                              [UR] ||F % 
   %                                                              %
   %            [D]                                               %
   %     BB*VEC(   )=VEC(AA)                             (7.1)    %
   %            [B]                                               %
   % ************************************************************ %
   
   YFR=[YF;YR];
   %AA =orthproj(YFR,S)-obmat(A,C,2*i)*orthproj(XKHAT,S);
   AA=orthproj(YFR,S)-obmat(A,C,2*i)*XKHAT; % Should give same result
   BB=sumkromat(A,C,UUU,i);
   if isDzero
	 kk = 0;
	 ll = 0;
	 BBB = zeros(size(BB,1),n*m);
	 %BBB = [];
	 for jj = 1:m
	   for j = 1:p
		 kk = kk+1;
	   end
	   for j = 1:n
		 kk = kk+1;
		 ll = ll+1;
		 BBB(:,ll) = BB(:,kk);
		 %BBB = [BBB BB(:,kk)];
	   end
	 end
	 clear kk ll
	 B=BBB\AA(:); clear BBB
	 B=reshape(B,n,m);
	 D=zeros(p,m);
   else
	 DB=BB\AA(:);
	 DB=reshape(DB,p+n,m);
	 D=DB(1:p,:);
	 B=DB(p+1:p+n,:);
	 clear DB
   end
   clear AA BB
    
 case {'ss'}
  
  % ************************************************************ %
  %                                                              %
  %                STEP 5                                        %
  %                                                              %
  %  Determine XKHAT and XKKHAT                                  %
  %                                                              %
  %  [ Pi_{S}X_{k} : Pi_{S+Y_{k}}X_{k+1}]=Sigma1^{1/2}Omega1'    %
  %                                                              %
  % ************************************************************ %
  
  SIGOM =SIGMA1^0.5*OMEGA1; 
  XKHAT =SIGOM(:,1:jj);  % hat(x_k)
  XKKHAT=SIGOM(:,jj+1:2*jj); % hat(x_k+1)
  
  % ************************************************************ %
  %                                                              %
  %               STEP 6                                         %
  %                                                              %
  %  Determine A,B,C and D  from                                 %
  %                                                              % 
  %                    || [XKKHAT]    [A B] [XKHAT] ||           % 
  %  A,B,C,D = argmin  ||           -               ||           %
  %           A,B,C,D  ||   [YI]      [C D]   [UI]  ||F          % 
  %                                                              %
  %                                                              %
  % ************************************************************ %

  AB = XKKHAT/[XKHAT;UI]; 
  A = AB(1:n,1:n);
  B = AB(1:n,n+1:n+m);
  clear AB
  if ~isDzero
	CD = YI/[XKHAT;UI];
	C = CD(1:p,1:n);
	D = CD(1:p,n+1:n+m);
	clear CD
  else
	C = YI/XKHAT;
	D = zeros(p,m);
  end
  
 case {'mp'}

  % ************************************************************ %
  %                                                              %
  %                STEP 5                                        %
  %                                                              %
  %  D=h_{0}, O_{2i-1}=Gamma1*Sigma1^{1/2}                       %
  %                                                              %
  %  [B : Pi_{S}X_{k} : Pi_{S+Y_{k}}X_{k+1}]=Sigma1^{1/2}Omega1' %
  %                                                              %
  % ************************************************************ %
  
  D=H1(1:p,:); %  D=h_0
  O2=GAMMA1*SIGMA1^0.5; %  O2=O_{2i-1}
  
  SVDM=SIGMA1^0.5*OMEGA1; % The matrix to be separated
  B=SVDM(:,1:m);
  XKHAT=SVDM(:,m+1:jj+m);  % hat(x_k)
  XKKHAT=SVDM(:,m+jj+1:m+2*jj); % hat(x_k+1)

  % ********************************************************* %
  %                                                           %
  %               STEP 6                                      %
  %                                                           %
  %   Determine A and C from O_{2i-1} by using the shift      %
  %                                                           %
  %   property.                                               %
  %                                                           %
  %********************************************************** %
  
  C=O2(1:p,:);
  %A=pinv(O2(1:2*(i-1)*p,:))*O2((p+1):(2*i-1)*p,:);
  A=O2(1:2*(i-1)*p,:)\O2((p+1):(2*i-1)*p,:);
  
end

clear OMEGA1 



% ***************************************************** %
%                                                       %
%               PENULTIMATE STEP                        %
%                                                       %
%  Determine the noise covariance matrix                %
%                                                       %
%  [Epsilon_w]=[proj(SYI,X_{i+1})]-[A B][ proj(S,XK)]   %
%  [Epsilon_v]=[        YI       ]-[C D][  UI       ]   %
%                                                       % 
% ***************************************************** %

EPSILONW=XKKHAT-A*XKHAT-B*UI;
EPSILONV=YI-C*XKHAT-D*UI;
RES=[EPSILONW;EPSILONV];
COV=cov(RES');

% ***************************************************************** %
%                                                                   %
%              FINAL STEP                                           %
%                                                                   %
%  Determine Kalman filter gain K                                   %
%                                                                   %
%  Determine error covariance P                                     %
%                                                                   %
%  SIGMA^{S}=A*SIGMA^{S}*A^{*}+SIGMA^{W}                            %
%                                                                   %
%  G=A*SIGMA^{S}*C^{*}+SIGMA^{WV}                                   %
%                                                                   %
%  LAMBDA_{0}=C*SIGMA^{S}*C^{*}+SIGMA^{V}                           %
%                                                                   %
%  K=(G+APC^{*})(LAMBDA_{0}+CPC^{*})^{-1}                           %
%                                                                   %
%  P=APA^{*}-(G+APC^{*})(LAMBDA_{0}+CPC^{*})^{-1}(G+APC^{*})^{*}    %
%                                                                   % 
% ***************************************************************** %

SIGMAW=COV(1:n,1:n);
SIGMAV=COV(n+1:n+p,n+1:n+p);
SIGMAWV=COV(1:n,n+1:n+p);
SIGMAS=dlyap(A,SIGMAW); % Requires Control System Toolbox
G=A*SIGMAS*C'+SIGMAWV;
LAMBDA0=C*SIGMAS*C'+SIGMAV;
[P,L,K,RR]=dare(A',C',zeros(n,n),LAMBDA0,G,eye(n)); % Requires Control System Toolbox
K=K';

if nargout > 2
  SV = diag(SIGMA1(1:n,1:n));
  %SV=zeros(n,1);
  %for j=1:n
  %  SV(j)=SIGMA1(j,j);
  %end
end
  
%M = idss(A,B,C,D,K,zeros(size(A,1),1),Ts);
%[E,X0] = pe(M,data);
X0 = zeros(size(A,1),1);
M = idss(A,B,C,D,K,X0,Ts,'OutputName',data.OutputName,'OutputUnit',data.OutputUnit,'InputName',data.InputName,'InputUnit',data.InputUnit,'TimeUnit',data.TimeUnit);

if isDzero
  M.nk = ones(1,m);
end

M.EstimationInfo.Status='estimated';
switch method
 case {'si'}
  M.EstimationInfo.Method='SUBID3B - Shift invariance approach';
 case {'ss'}
  M.EstimationInfo.Method='SUBID3B - State sequence approach';
 case {'mp'}
  M.EstimationInfo.Method='SUBID3B - Markov parameter approach';
end
M.EstimationInfo.DataName=data.Name;
M.EstimationInfo.DataLength=cy;
M.EstimationInfo.DataTs=data.Ts;
M.EstimationInfo.DataInterSample=data.InterSample;
%M.EstimationInfo.LossFcn=0;
%M.EstimationInfo.FPE=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                           %
%                                  END ALGORITHM                            %
%                                                                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% *** last line of subid3b.m ***