function [yhat, th, model, state] =...
pem2pred(k,y,u,sequence,init_pred,lambda,model_in,...
        state_in,p,a_order,nb_row,c_row,nd);

% PEM2PRED Iterative RPEM model-sequence k-step prediction with a number of
%     recursive predictors. Yields YHAT when known/prediced U are supplied,
%     because the predictors for the last known measured data are stored.
%
%     [YHAT TH MODEL STATE] = ...
%       pem2pred(K,Y,U,SEQUENCE,INIT_PRED,LAMBDA,MODEL_IN,...
%               STATE_IN,P,A_ORDER,NB_ROW,C_ROW,ND)
%
%     or
%
%     [YHAT TH MODEL STATE] = ...
%       pem2pred(K,Y,U,SEQUENCE,INIT_PRED,LAMBDA,MODEL_IN,STATE_IN)
%
%     The model   y = (B1/A) * u1 + ... + (Bn/A) * un + (C/D) * e
%     is estimated using a recursive prediction error method (RPEM).
%
%     YHAT is the iterated k-step predictor. It is situated k steps
%     forward comparad with the inputs. TH is parameter curves with parameters
%     in corresponence with Y and INIT_PRED (see below). MODEL are the recursive 
%     predictors ready to be used as MODEL_IN for the next
%     input. STATE contains values for dW/dA, dW/dB and psi.
%
%     K represents the horizon for the predictor to be calculated, Y and U are out- 
%     and in-signal as colon vectors. SEQUENCE represents the ordering
%     of predictors. SEQUENCE may contain integers > 0.
%     INIT_PRED indicates the sequence number of the predictor for the first element
%     in the u- and y-vectors. This makes it possible to store the predictors with
%     unchanged information about their placi in the cycle. It is set to 1 when
%     not using cyclic models. If INIT_PRED < 1 then INIT_PRED is fetched from MODEL_IN.
%
%     LAMBDA corresponds to the decay parameter in the recursive algoritm having an
%     cycle length of 1. P is the starting value of the diagonal elements of the 
%     covariance matrix.
%     MODEL_IN is a model with previously estimated predictors. If this parameter is
%     missing, use []. STATE_IN is dW/dA, dW/dA and psi for earlier estimated
%     models.
%
%     A_ORDER is the order of the A-polynomial, NB_ROW is a row vector with orders 
%     for the B-polynomials. C_ROW is a row vector containing the (positive)
%     delays that shall have parameters estimeted in the C-polynomial. 
%     ND represents the order of the D-polynomial.

%     Roger Felix, 26 May 1996.
%

[ns interval] = size(sequence);
NoOfModels=max(sequence); ModelCount = zeros(1, NoOfModels);

for i = 1:NoOfModels,    % COUNT NO OF OCCURENCIES
  for j = 1:interval, 
    if sequence(1, j) == i, 
        ModelCount(i) = ModelCount(i)+1;
    end; % (if)
  end; % (for)
end; % (for)

for i = 1:NoOfModels,      % EVERY MODEL HAS EQUAL FORGETTING RATE
  if ModelCount(i) > 0, 
     lambdas(i) = 1-interval * (1-lambda)/ModelCount(i);  
  else 
     lambdas(i) = 1;
  end; % (if)
  if lambdas(i) < 0, error('Too small LAMBDA'), end;
end; % (for)

if init_pred > interval, 
   error('INIT_PRED must be within 1..INTERVAL');
end; % (if)

if isempty(model_in),        % CREATE MODEL
   [ma na] = size(a_order); [mb nb] = size(nb_row);
   nb_row = nb_row + ones(mb, nb); % NO OF PARAMETERS, NOT ZEROES ! 
   [mc nc] = size(c_row); c_len = max([0 c_row]);
   nTheta = nc+nd+sum(nb_row)+a_order;
   model_size = [4+nTheta, max([8+na+nb+nc,k+c_len,nd+nb*(a_order) ...
                a_order+sum(nb_row),(nTheta+1) * NoOfModels])];
   model = zeros(model_size(1), model_size(2)); eloc = 1; 
   model(1, 1:8+na+nb+nc) = ...
       [na nb nc nd init_pred interval eloc p a_order nb_row c_row];
else                         % FETCH MODEL
   model = model_in; model_size = size(model);
   state = state_in; [ModelCount, nState] = size(state);
   na = model(1, 1); nb = model(1, 2); nc = model(1, 3);nd = model(1, 4);
   init_p = model(1, 5); interval = model(1, 6);
   eloc = model(1, 7);
   p = model(1, 8);
   if init_p ~= init_pred, 
      if init_pred < 1, 
         init_pred = init_p;
      else
         disp('Warning! --- non-cyclic calculation !');
      end
   end; % (if)
   a_order = model(1, 9:8+na);               % na = 1 (!) 
   nb_row = model(1, 9+na:8+na+nb); 
   c_row = model(1, 9+na+nb:8+na+nb+nc); 
   nTheta = a_order+nc+nd+sum(nb_row); 
end; % (if)

[mu nu] = size(u); [my ny] = size(y);

if nu ~= nb,
   error('The number of input signals must equal the number of orders nb_row'); 
end; %(if)

if mu < my+k, 
   error('Exogenous signals must exist for all Y(1..i+K)');
end; % (if)

    % THE INDEX OF THE PSI-VECTORS 
    % ----------------------------

c_len = max([0 c_row]);
a_len = max([nd,a_order+c_len]);
b_len = ones(1,nb).*max([c_len+a_order,nb_row,nd]); 
d_len = max([c_len,nd]);

a1_psi = 1;
b0_psi = a_len+1+cumsum(b_len)-b_len(1);

a_psi = 1 : a_len;
b_psi = a_psi(length(a_psi)) + [1 : sum(b_len)];
c_psi = b_psi(length(b_psi)) + [1 : sum(c_len)];
if c_len > 0,
   d_psi = c_psi(length(c_psi)) + [1 : sum(d_len)];
else
   d_psi = b_psi(length(b_psi)) + [1 : sum(d_len)];
end; %(if)

psi_index = [1 : a_order];

for i = 1:nb,
   psi_index = [psi_index (b0_psi(i) : b0_psi(i) + nb_row(1,i) - 1)];
end; %(for)
if c_len > 0,
   psi_index = [psi_index (c_psi(1)+c_row-1) (d_psi(1)+[1:nd] - 1)];
else
   psi_index = [psi_index (d_psi(1)+[1:nd]-1)];
end; %(if)

     % INDEX FOR dWdA AND dWdB 
     % --------------------------

dWdA1_index =  1+d_psi(length(d_psi));
dWdB1_index =  1+d_psi(length(d_psi))+a_order+...
               cumsum(max(ones(size(nb_row))*nd,max(a_order,nb_row))) - ...
                max(max(a_order,nb_row(1)),nd);

dWdA_index = dWdA1_index(1) - 1 + [1: a_order];
dWdB_index = dWdB1_index(1) - 1 + ...
             [1: sum(max(ones(size(nb_row))*nd,...
              max(a_order*ones(size(nb_row)),1+nb_row)))];

if isempty(state),
    nState = dWdB_index(length(dWdB_index));
    state = zeros(NoOfModels,nState);
else
    if nState ~= dWdB_index(length(dWdB_index)),
        error('STATE is of wrong size');
    end; % (if)
end; % (if)

     % INDEX OF THE THETA VECTORS
     % --------------------------

a_theta = 1:a_order;
b_theta = a_order+1 : a_order + sum(nb_row);
c_theta = a_order+sum(nb_row)+1 : ...
          a_order + sum(nb_row) + nc;
d_theta = a_order+sum(nb_row)+nc+1 : ...
          a_order + sum(nb_row) + nc + nd;

a1_theta = 1;
b0_theta = 1+a_order+cumsum(nb_row)-nb_row(1);

th = zeros(nTheta, my); 
yhat = zeros(1, my) ; yth = zeros(k, 1);             
ukol = zeros(sum(nb_row), 1);      

     % INITIATE W_SEKV, NY_SEKV, U & Y WITH OLD VALUES
     % ------------------------------------------------

offset = max([a_order nb_row nd]);
w_sekv = zeros(offset+my,nu);ny_sekv = zeros(offset+my,1);
u = [zeros(offset, nu); u]; y=[zeros(offset, 1); y];

y(offset:-1:offset-a_order+1, 1) = model(4, 1:a_order)';
ny_sekv(offset:-1:offset-nd+1, 1)  = model(3, 1:nd)';
for i = 1:nu, 
    u(offset:-1:offset-nb_row(1, i)+1, i) = ...
        model(4, a_order+sum(nb_row(1:i-1)):...
        a_order+sum(nb_row(1:i))-1)';

    w_sekv(offset:-1:offset- a_order+1,i) = ...
        model(3, nd+a_order*(i-1)+1:...
        nd+a_order*i)';
end; % (for)

Prows = 5:nTheta+4; THrows = Prows; kcmax = k+c_len;

    % INITIATE COVARIANCE MATRICES
    % ----------------------------

if nargin > 8,
    for i = NoOfModels:nTheta:NoOfModels * (nTheta+1)-nTheta,          
        for j = Prows, 
            model(j, i+j-4) = p; 
        end; % (for)
    end; % (for)
else

end; % (if)

for i = 1:my,                             % FOR ALL KNOWN Y

       % CALCULATE INDEX 
       % ---------------

    ii=i+offset;                          % ii = INDEX(u, y, w_sekv, ny_sekv)

    predictor = sequence(init_pred);
    epsi=y(ii)-model(2,eloc);             % epsi = Y-YHAT(0)
    model(2,eloc) = 0;

       % UPDATE THETA
       % ------------

    Pindex = ...
        (NoOfModels+(predictor-1) * nTheta+1:NoOfModels+predictor * nTheta); 

    K =  model(Prows, Pindex) * state(predictor,psi_index)'/ ...
         (lambdas(predictor)+...
         state(predictor,psi_index) * model(Prows, Pindex) * ...
         state(predictor,psi_index)');

    model(Prows, Pindex) = (model(Prows, Pindex) - K * ...
           state(predictor,psi_index)*model(Prows, Pindex)) / ...
           lambdas(predictor);

     % SAVE OLD D & A-parameters
     % -------------------------

       D_old(1,1:nd) = ...
          model(THrows(d_theta), predictor)';
       A_old(1,1:a_order) = ...
          model(THrows(a1_theta:a1_theta+a_order-1), predictor)';
    
  
    model(THrows, predictor) = ...
        model(THrows, predictor) + K * epsi;

    th(:,i) = model(THrows, predictor);

    % STABILIZE D (mirroring)
    % ------------------------

%    D = fstab([1  model(THrows(d_theta), predictor)']);
%    for nn = 1+length(D) : 1+nd,
%       D(1,nn) = 0;
%    end; %(for)
%    model(THrows(d_theta),predictor) = ...
%       D(1,2:nd+1)';


    %  STABILIZE D (Prevention of update)
    %  --------------------------------------
    if nd > 0
        D = [1 model(THrows(d_theta), predictor)'];
       if length(D) > 1,
            if any((abs(roots(D))+eps-1) > 0), % INSTABLE ROOT ?
                D = [1 D_old(1,1:nd)];
            end; % (if)
        end; %(if)
        model(THrows(d_theta),predictor) = ...
                      D(1,2:nd+1)';
    end; %(if)


    % STABILIZE A (Mirroring)
    % ------------------------

    A = fstab([1  model(THrows(a1_theta:...
        a1_theta+a_order-1), predictor)']);
    for nn = 1+length(A) : 1+a_order,
        A(1,nn) = 0;
    end; %(for)
    model(THrows(a1_theta:a1_theta+a_order-1),predictor) = ...
        A(1,2:a_order+1)';

    % STABILIIZE A (Prevention of update)
    % -----------------------------------

%    A = [1 model(THrows(a1_theta:a1_theta+a_order-1), predictor)'];
%    if length(A) > 1,
%        if any((abs(roots(A))+eps-1) > 0), % INSTABLE ROOT ?
%            A = [1 A_old(1,1:a_order)];
%        end; % (if)
%    end; %(if)

%    model(THrows(a1_theta:a1_theta+a_order-1),predictor) = ...
%       A(1,2:a_order+1)';

    % CALCULTE w_sekv AND ny_sekv AND ESTIMATED ERROR e USING NEW THETA
    % -----------------------------------------------------------------

    for signal = 1:nu,  % BERÄKNA NYTT w OCH ny
       %  A w_sekv = B u   <=>   w_sekv(0) = B u - (A - 1) w_sekv  
       w_sekv(ii, signal) = model(THrows([ ...
           (a1_theta:a1_theta+a_order-1) ...
           (b0_theta(signal):b0_theta(signal)+nb_row(signal)-1)]), ...
           predictor)' * [-w_sekv(ii-1:-1:ii-a_order,signal); ...
                 u(ii :-1:ii-nb_row(signal)+1, signal)];
    end; %(for)

    ny_sekv(ii,1) = y(ii,1) - sum(w_sekv(ii,:));

    model(2,eloc) = ny_sekv(ii,1) - ...
        model(THrows([c_theta d_theta]),predictor)' *...
        [model(2,1+rem(eloc-1+kcmax-c_row,kcmax)) ...
         -ny_sekv(ii-1:-1:ii-nd,1)']';

    % UPDATE ALL PSI'S BY FILTERING U & Y
    % -----------------------------------

    for n = 1: NoOfModels,      

        % FETCH D, C AND A 
        % ----------------

        A = [1  model(THrows(a1_theta:a1_theta+a_order-1),n)'];

        D = [1; model(THrows(d_theta), n)];

        if c_len > 0,  
            C_explicit = [1 zeros(1,c_len)];
            C_explicit(1,1+c_row) = ...
                model(THrows(c_theta), n)';
        end; % (if)
        C_explicit(1,1) = 1;

        % CALCULATE NEW dWdA, DWdB, PsiB AND PsiA
        % --------------------------------------

        %  A dWdA = (0 | -w_sekv(t-j)) =>
        %    dWdA(t) = -(A - 1) dWdA - (0 | w_sekv(t-j))

        new_dWdA = - A(2:1+a_order) * ...
          state(n, dWdA1_index:dWdA1_index + ...
          a_order-1)' - (n == predictor) * sum(w_sekv(ii,:));

        %  C PsiA = D dWdA   <=>  PsiA(0) = D dWdA - (C - 1) PsiA

        newPsiA =  D' * [new_dWdA ...
                state(n,[dWdA1_index:dWdA1_index+nd-1])]' ...
                - C_explicit * [0 state(n, a1_psi: ...
                    a1_psi+c_len-1)]';

        for signal = 1:nu,

            %  A dWdB = (0 | u(t-j))  =>
            %     dWdB(t) = -(A - 1) dWdB + (0 | u(t-j))

            new_dWdB(signal) = - A(2:1+a_order) * ...
              state(n,dWdB1_index(signal):dWdB1_index(signal)+ ...
              a_order-1)' + (n == predictor) * u(ii,signal);

            %  C PsiB = D dWdB  <=>  PsiB(0) = D dWdB - (C - 1) PsiB

            newPsiB(signal) =  D' * [new_dWdB(signal) ...
            state(n,dWdB1_index(signal):dWdB1_index(signal)+nd-1)]' - ...
                C_explicit * [0  state(n, b0_psi(signal):b0_psi(signal)+...
                    c_len-1)]';
        end; % (for)

           % DETERMINE NEW PsiC AND PsiD
           % -------------------------

        %  C PsiC = e   <=>  PsiC(0) = e - (C - 1) PsiC  

        if c_len > 0     
             newPsiC =  (n == predictor) * model(2,eloc) - ...
                        C_explicit * [0 state(n, c_psi)]';
        end; % (if)

        %  D PsiD = -e   <=>  PsiD(0) = -e - (D - 1) PsiD  
        if nd > 0  
            newPsiD = - (n == predictor) * model(2,eloc) - ...
                      D' * [0 state(n, d_psi(1:nd))]';
        end; % (if)

        % SHIFTA psi, dWdA & dWdB
        % -----------------------

        state(n, 2 : nState) = state(n, 1 : nState - 1);

        state(n,a1_psi) =  newPsiA;
        state(n,dWdA1_index) =  new_dWdA;

        for signal = 1:nu 
            state(n,b0_psi(signal)) = newPsiB(signal);
            state(n,dWdB1_index(signal)) = new_dWdB(signal);
        end; % (for) 

        if c_len > 0,
            state(n, c_psi(1)) = newPsiC;
        end; % (if)

        if nd > 0, 
            state(n, d_psi(1)) = newPsiD;
        end; % (if)

    end; % (for)

         % COPY OLD NY_HAT AND W_VEC_HAT
         % ----------------------------------

    ny_hat = ny_sekv(ii:-1:ii-nd+1,1);

    w_vec_hat(1:a_order,:) = w_sekv(ii:-1:ii-a_order+1,:);

    for j = 1:k,                                   % FOR FORECASTING HORIZON 1..k

        predictor = sequence(1+rem(init_pred-1+j+interval, interval));

        model(2, 1+rem(kcmax+eloc-1+j, kcmax)) = 0; % ERASE OLD VALUES OF e.

        % SHIFT & CALCULATE w_vec_hat USING LOCAL MODEL
        % -------------------------------------------

        % A w_vec_hat = B u <=> w_vec_hat(0) = B u - (A - 1) w_vec_hat 

        w_vec_hat(2:a_order+1,:) = w_vec_hat(1:a_order,:);

        % A w_vec_hat = B u <=> w_vec_hat(0) = B u - (A - 1) w_vec_hat 
        for signal = 1:nu,
            w_vec_hat(1,signal) = model(THrows([ ...
            (a1_theta:a1_theta+a_order-1), ...
            (b0_theta(signal):b0_theta(signal)+nb_row(signal)-1)]'), ...
            predictor)' *  [-w_vec_hat([2:a_order+1],signal); ...
            u(ii+j:-1:ii+j-nb_row(signal)+1, signal)];
        end; % (for)

        % SHIFTA & CALCULATE ny_hat USING LOCAL MODEL
        % ----------------------------------------
        % ny_hat =  C e_hat - (D - 1) * ny_hat

        ny_hat(2:nd+1,1) = ny_hat(1:nd,1);
        ny_hat(1,1) = model(THrows([c_theta, d_theta]),predictor)' *...
             [model(2,1+rem(kcmax+j+eloc-1-c_row,kcmax)), -ny_hat(2:nd+1,1)']';

        % CALCULATE y_hat USING LOCAL MODEL
        % ------------------------------

        yth(j) = sum(w_vec_hat(1,:)) + ny_hat(1,1);

    end; %(for)

    eloc = eloc+1; if eloc > kcmax, eloc = eloc-kcmax;end;

    init_pred = init_pred+1;
    if init_pred > interval, init_pred = init_pred-interval;end;

    model(2, eloc) = yth(1);         % SAVE THE PREDICTION (!)
   
    yhat(1, i) = yth(k);             % HERE YHAT IS DELIVERED

end % (for)

     % SAVE W_SEKV, NY_SEKV, IN-SIGNALS AND OUT-SIGNAL
     % --------------------------------------------

model(4,1:a_order) = y(my+offset:-1:my+offset-a_order+1, 1)' ;
model(3,1:nd) = ny_sekv(my+offset:-1:my+offset-nd+1, 1)' ;
for i = 1:nu,          
   model(4, a_order+sum(nb_row(1:i-1))+i:...
         a_order+sum(nb_row(1:i))+i-1) = ...
         u(my+offset:-1:my+offset-nb_row(1, i)+1, i)';
   model(3, nd+a_order*(i-1)+1:...
         nd+a_order*i) = ...
         w_sekv(my+offset:-1:my+offset-a_order+1, i)';
end; % (for)

model(1, 5) = init_pred;
model(1, 7) = eloc;