stls_cost_fun.m

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

% STLS_COST_FUN - Structured total least squares cost function
function [cost,dp] = stls_cost_fun( x, c, struct )

[S,V] = decode_struct( struct );

[m,n_d] = size(c);
[n,d]   = size(x);
if isstruct(struct)
  K = struct.k;
  struct = struct.a;
else
  K = 1;
end
t = K;
s = size(V,3);
q = size(struct,1);

% Find yr
if t*d == 1
  f = zeros(1,s);
  for k = 1:s
    f(k) = [x;-1]' * V(:,:,k)' * [x;-1];
  end
  r  = c(:,1:n)*x-c(:,n+1);
  yr = mb02gd(f,r);
  cost = r'*yr;
else
  x_ext = [x;-eye(d)];
  if t > 1
    x_ext = kron(eye(t),x_ext);
  end
  f = zeros(t*d,t*d,s);
  for k = 1:s
    f(:,:,k)  = x_ext' * V(:,:,k)' * x_ext;
  end
  F  = reshape(f,size(f,1),size(f,2)*size(f,3));
  r  = c(:,1:n)*x-c(:,n+1:end);
  rt = r';
  yr = mb02gd(F,rt(:));
  cost = rt(:)'*yr;
end

if nargout == 2
  % Compute Vp G^T yr
  dp = [];
  Yr = reshape(yr,d,m)';
  col_k_b = 0; % first column of X_ext,k in X_ext 
  col_k_e = 0; %  last column of X_ext,k in X_ext  
  x_ext   = [x;-eye(d)];
  % first compute G^T yr
  for k = 1:q
    col_k_b = col_k_e + 1;
    col_k_e = col_k_e + struct(k,2);
    x_ext_k = x_ext(col_k_b:col_k_e,:);
    switch struct(k,1)
     case {1,2} % Toeplitz/Hankel
      nk = struct(k,2);
      bm = K;
      bn = struct(k,3);
      mb = m/bm;
      nb = nk/bn;
      % Form the matrix gk    
      if bm == 1 & bn == 1 % scalar case
        gk = zeros((m+nk-1),d*nk);
        for l = 1:nk
          gk(l:l-1+m,(l-1)*d+1:l*d) = Yr;
        end
      else % block case
        Yr_ = zeros(bn*size(Yr));
        for l = 1:d
          Yr_(:,l:d:end) = kron(Yr(:,l),eye(bn));
        end
        gk = zeros((mb+nb-1)*bm*bn,d*nk);
        for l = 1:nb
          gk((l-1)*bn*bm+1:(l-1)*bn*bm+m*bn, (l-1)*d*bn+1:l*d*bn) = Yr_; 
        end
      end
      % find dp_k
      if struct(k,1) == 1
        x_ext_k = fliplr(reshape(x_ext_k',bn*d,nb));      
      else
        x_ext_k = x_ext_k';
      end
      dp_k = gk * x_ext_k(:);
     case 3     % Unstructured
      dp_k = x_ext_k * Yr';
     case 4
      dp_k = [];
    end
    dp = [dp; dp_k(:)];
  end  
end