inistate.m

The main features of the considered identification problem are that there is no an a priori separati

function xini = inistate(w,sys,T)
% INISTATE - compute initial condition for a trajectory

% Define constants
[p,m] = size(sys);
n     = size(sys,'order');
N     = size(w,3);

if nargin < 3
  T = max(ceil(n/p),2);
end

% Define inputs and outputs
u  = w(1:T,1:m,:);
y  = w(1:T,m+1:end,:);
clear w

% Compute the extended observability matrix
O = zeros(T*p,n);
O(1:p,:) = sys.c;
for t = 2:T
  O((t-1)*p+1:t*p,:) = O((t-2)*p+1:(t-1)*p,:)*sys.a;
end

% Compute initial consitions
xini = zeros(n,N);
sys.Ts = -1;
mo = (m > 0);
for k = 1:N
  if mo
    y0 = (y(:,:,k) - lsim(sys,u(:,:,k)))';
  else
    y0 = y(:,:,k)';
  end
  xini(:,k) = O \ y0(:);
end