subid3b.m

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

function [M,P,SV] = subid3b(data,n,i,Dzero,method)
%SUBID3B   Three-block subspace method for identification of linear systems.
%
%   SUBID3B estimates a combined deterministic-stochastic linear state-space
%   model using a three-block unbiased subspace algorithm.
%
%   Usage:
%      [M,P,SV] = subid3b(DATA)
%      [M,P,SV] = subid3b(DATA,n)
%      [M,P,SV] = subid3b(DATA,n,k)
%      [M,P,SV] = subid3b(DATA,n,k,DMAT)
%      [M,P,SV] = subid3b(DATA,n,k,DMAT,ALG)
%
%   Inputs:  
%      DATA  := Input-output data given as an IDDATA object.
%      n     := System order (optional):
%                (1) If n = [] then the algorithm will automatically
%                    select the order such that n <= 10 (default).
%                (2) If n is a scalar, then n is the system order.
%                (3) If entered as a vector (e.g. [1 2 3 4 5]), a plot of 
%                    singular values will be given and the user will be
%                    prompted to select an order.
%      k     := Block size given as a positive integer (optional). If not
%                specified, k is chosen to be as large as possible while
%                still trying to be compatible with the size of DATA. It
%                is recommended that k > max(n).
%      DMAT := Estimate/set D matrix (optional):
%                'Estimate': Estimate D (default).
%                'Zero'    : Set D = 0.
%      ALG  := Identification algorithm (optional):
%                'si': Shift invariance approach (default).
%                'ss': State sequence approach.
%                'mp': Markov parameter approach.
%
%   Outputs:
%      M   := The estimated state-space model given as a discrete-time
%             IDSS model object: 
%               x[k+1] = A x[k] + B u[k] + K e[k];     x[0] = X0
%                 y[k] = C x[k] + D u[k] + e[k]
%      P   := Estimated Kalman filter covariance matrix (optional).
%      SV  := The retained singular values from the SVD decomposition (optional).    
%
%   See also IDDATA, IDSS, BALPEM, N4SID.
%
% CUED System Identification Toolbox.
% Cambridge University Engineering Department.
% Copyright (C) 1998-2002. All Rights Reserved.

% Version 1.00, Date: 01/06/2002
% Created by H. Chen and E.C. Kerrigan.

% For details of the algorithm, see N.L.C. Chui and
% J.M. Maciejowski. "Subspace identification - a Markov parameter
% approach". Technical Report CUED/F-INFENG/TR.337, December 1998,
% University of Cambridge, UK.

% Check the arguments

if nargin < 1
  error('SUBID3B needs at least one input argument.');
end

% Turn the data into column vectors and check
%
% Y=[y(1), y(2), ..., y(N)];  U=[u(1), u(2), ... , u(N)]
%

if ~isa(data,'iddata')
  error('DATA should be an IDDATA object.')
end

Y = get(data,'OutputData');
U = get(data,'InputData');
Ts = get(data,'Ts');

Y = Y';
[p,cy] = size(Y);

if p < 1
  error('Need a non-empty output vector in DATA.')
end

U = U';
m = size(U,1);

if m < 1
  error('Need a non-empty input vector in DATA.')
end

if nargin > 2
  if ~isempty(i)
	if i < 1 | fix(i) ~= i
	  error('Block size k should be a positive integer.')
	end
	if cy < 3*i*(m+1)-1
	  error('Not enough data points.')
	end
  else
	i = fix((cy+1)/3/(m+1));
	disp(sprintf('Block size k = %d.',i))
  end
else
  i = fix((cy+1)/3/(m+1));
  disp(sprintf('Block size k = %d.',i))
end

if nargin > 1
  if ~isempty(n)
	n = n(:);
	if any(n < 1 | fix(n) ~= n)
	  error('Only positive integers are allowed for the choice of system order.')
	end
%	if max(n) > i*p
%	  warning('The block size might be too small to support this choice of order.')
%	end
  end
else
  n = [];
end

if nargin > 3
  if ~isempty(Dzero)
	switch lower(Dzero)
	 case {'e','estimate','estimated'}
	  isDzero=0;
	 case {'z','zero','zeros'}
	  isDzero=1;
	 otherwise
	  error('Invalid choice for DMAT. Select ''Estimate'' or ''Zero''.')
	end
	clear Dzero;
  else
	isDzero=0;
  end
else
  isDzero=0;
end

if nargin > 4
  if ~isempty(method)
	switch lower(method)
	 case {'mp','si','ss'}
	  method = lower(method);
	 otherwise
	  error('Invalid choice for ALG. Select ''si'', ''ss'' or ''mp''.')
	end
  else
	method = 'si';
  end
else
  method = 'si';
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                           %
%                              BEGIN ALGORITHM                              %
%                                                                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ************************************************** %
%               STEP 1                               %
%                                                    %   
% Construct U_{p},U_{f},U_{r},Y_{p},Y_{f} and Y_{r}  %
%                                                    %
% ************************************************** %

% Call Blochank
% Call Bloctoep

jj = cy-3*i+1;  % jj=N-3i+1, N=cy; 
U1=U(:); clear U % U1=[u(1);u(2);...;u(N)]
UUU=blochank(U1,3*i,cy); clear U1 %  UUU=[u(1),...,u(N-3i+1);...;u(3i),...,u(N)]
if rank(UUU) ~= 3*i*m,
  error('The input block Hankel matrix is not full row rank. Add some noise to the input.')
end
UP=UUU(1:i*m,:);
UF=UUU(i*m+1:2*i*m,:);
UR=UUU(2*i*m+1:3*i*m,:);

Y1=Y(:); clear Y% Y1=[y(1);y(2);...;y(N)]
YYY=blochank(Y1,3*i,cy); clear Y1 %  YYY=[y(1),...,y(N-3i+1);...;y(3i),...,y(N)]
YP=YYY(1:i*p,:);
YF=YYY(i*p+1:2*i*p,:);
YR=YYY(2*i*p+1:3*i*p,:);  

pack

% **************************************************** %
%                                                      %
%               STEP 2                                 %
%                                                      %
%  Determine h_{0},...,h_{i-1} (First m columns of     %
%                                                      %
%  T_{i})                                              %
%                                                      %
%  cproj(R,proj(S,Y_{r}))=T_{i}*cproj(R, U_{r}) (2.1)  %
%                                                      %
%  S:=Y_{p}+U_{p}+U_{f}+U_{r}                          %
%                                                      %
%  R:=proj(S, Y_{f}) + U_{f}                           %
%                                                      %
% **************************************************** %

S=[YP;UP;UF;UR];   % S=yp+up+uf+ur
R=[orthproj(S,YF);UF]; % R=proj(S,YF)+uf

%Old code - does not always work in practice
%cURR=coorproj(UR,R);
%rank1=rank(cURR);
%if rank1 ~= i*m
%  warning('coorproj(UR,R) is not full row rank.')
%end  
%TI=coorproj(orthproj(YR,S),R)/cURR; clear cURR; % solve (2.1)
%H1=TI(:,1:m);      % H1=[h_0;...;h_i-1]

cpRUR=coorproj(UR,R);
if rank(cpRUR) ~= i*m
  warning('coorproj(UR,R) is not full row rank.')
end

if ~isDzero
  [TI,H1]=soltritoep(coorproj(orthproj(YR,S),R),cpRUR,i); % solve (2.1)
else
  SS = coorproj(orthproj(YR,S),R); % SS = [S1;...;Sk]
  SS = SS(p+1:end,:);  % SS = [S2;...;Sk]
  PP = cpRUR(1:end-m,:); % PP = [P1;...;Pk-1]
  [TI,H1]=soltritoep(SS,PP,i-1); % H1 = [h_1;...;h_i-1]
  H1 = [zeros(p,m); H1];         % H1 = [0;h_1;...;h_i-1], h_0 = 0
  TI = bloctoep(H1,[H1(1:p,:); zeros(p*i-p,m)],i); % Correct TI
  clear SS PP
end
% H1 = [h_0;...;h_i-1]
clear cpRUR

% ************************************************************ %
%                                                              %
%               STEP 3                                         %
%                                                              % 
%  Compute Z_{f} and Z_{r} and                                 %
%                                                              %
%  Determine h_{i},...,h_{2i-1}                                %
%                                                              %
%  cproj(Q, proj(S, Z_{r}))=Upsilon_{L}*cproj(Q, U_{f})  (3.1) % 
%                                                              %
%  Q=proj(S, Z_{f})                                            %
%                                                              %
%  [Z_{f}] := [Y_{f}]-[  T_{i}       0  ] [U_{f}]              %
%  [Z_{r}] := [Y_{r}]-[Upsilon_{U} T_{i}] [U_{r}]              %
%                                                              %
% ************************************************************ %
 
HH1=zeros((i-1)*p,m);
for j=1:i-1,
  HH1((j-1)*p+1:j*p,:)=H1((i-j)*p+1:(i-j+1)*p,:);  % HH1=[h_i-1;...;h_1]
end      
UPSILONU=bloctoep(zeros(i*p,m),[zeros(p,m); HH1],i); clear HH1
%  UPSILONU=[0 h_i-1 ... h_1; 0 0 h_i-1... h_2; ... ; 0 0 ... h_i-1]   
ZF=YF-TI*UF;
ZR=YR-UPSILONU*UF-TI*UR; clear UPSILONU TI
Q=orthproj(ZF,S); clear ZF

%Old code - does not always work in practice
%cUFQ=coorproj(UF,Q);
%rank1=rank(cUFQ);
%if rank1 ~= i*m,
%  warning('coorproj(UF,Q) is not full row rank.')
%end
%UPSILONL=coorproj(orthproj(ZR,S),Q)/cUFQ; clear ZR cUFQ;  % Solve (3.1)
%  UPSILONL=[h_i 0 ... 0;h_i+1  h_i, ... 0; ...; h_2i-1 ... h_i]  
%H2=UPSILONL(:,1:m);  % H2=[ h_{i};...;h_{2i-1}]

cpQUF=coorproj(UF,Q);
rank1=rank(cpQUF);
if rank1 ~= i*m,
  warning('coorproj(UF,Q) is not full row rank.')
end
[UPSILONL,H2]=soltritoep(coorproj(orthproj(ZR,S),Q),cpQUF,i); % Solve (3.1)
% UPSILONL=[h_i 0 ... 0; h_i+1  h_i, ... 0; ...; h_2i-1 ... h_i] 
% H2=[h_i;...;h_2i-1]
clear ZR cpQUF UPSILONL


% *********************************************************************** %
%                                                                         %
%               STEP 4                                                    %
%                                                                         % 
%  Extract U_{u},U_{L},Y_{U} and Y_{L}                                    %
%                                                                         %
%  Calculate the following SVD and partition accordingly by               % 
%  selecting a model order                                                %
%                                                                         %
%  if method is 'si' or 'ss', then                                        %
%                                                                         %
%  [Pi_{S}Y_{U} : Pi_{S+Y_{i}}Y_{L}] -T_{2i-1} [U_{U} : U_{L}]            %
%                                                                         %
%                           [Sigma_{1} 0][Omega^{*}_{1}]                  %
%  :=[Gamma_{1} Gamma_{2}]*                               (4.1)           %
%                           [0         0][Omega^{*}_{2}]                  %
%                                                                         %
%  if method is 'mp', then                                                %