% [Ad,Bd,Cd,Dd,A,C,K,Lambda]=IO_new(y,u,ks,ns,nr);
%
%
%  [A,C,K,Lambda] is an innovation representation of the stochastic component
%  Lambda is the variance of the innovation
%
%  y : output data as row vector y = [y(1), y(2),......,y(Ndat)]
%  ks dimension of past and future in stochastic identification
%  ns  choosen order
%

%% this file computes the bias term.
%% This version use a weighted-constrained least squares step in lieu of
%% the oblique projection 

% recursive (in N) version

function [Ad,Bd,Cd,Dd,K]=PBSID_opt_ARX(y,u,ks,n,dflag);

if ((nargin > 5) | (nargin < 2)) 
   disp('Wrong number of input arguments')
   return
end;


if ((nargin < 5) | isempty(dflag))
   dflag = 0;  % don't estimate direct term in F by default
end;

if ((nargin < 4) )
   n=[1:10];
end;




if ((nargin < 3) | isempty(ks))
   ksp = ceil(1.2*max(ns)+3); % as in the N4SID file of matlab
   ksf = ksp;
else
   ksp = ks(1);
   ksf = ks(2);
end;

[p,Ny]=size(y);
[pu,Nu]=size(u);

pp = p+pu;

if Nu ~= Ny
   disp('u and y must have the same number of columns')
   return
end;


N = Ny-(ksf+ksp)+1;


%%% Hankel matrices:
z = [y;u];
%X0 = x(:,1:N);
%Xt = x(:,1:N);



%YYarx = Hankel(y,ksp+1,N+ksf-1);


N0=min(N,1500);
YYarx = y(:,ksp+1:ksp+1+N0+ksf-2)/sqrt(N0+ksf-1);
%YYarx = YYarx(ksp+1,:);
ZZarx = Hankel(z,ksp,N0+ksf-1);



Szz=pinv(ZZarx*ZZarx');
THETA = YYarx*ZZarx'*Szz;
%THETA = YYarx/ZZarx;

N0+ksf
N+ksf

for j=N0+ksf:N+ksf-1,
    zz=z(:,j:j+ksp-1);
    zz=zz(:);
    Szz1=Sigmainv(Szz,zz,j);
    yy = y(:,ksp+j);
    THETA = ls_rec(THETA,Szz,Szz1,zz,yy,j);
    Szz=Szz1;
if mod(j,10)==0,
        j
    end;
end;

Past = zeros(ksf*p-p,(ksp+1)*pp);
Past1 = zeros(ksf*p-p,(ksp+1)*pp);



for i=1:ksf-1,
    for j=i:ksp,
        Past((i-1)*p+1:i*p,(j-1)*pp+1:j*pp)=THETA(:,(j-i)*pp+1:(j-i+1)*pp);
        Past1((i-1)*p+1:i*p,(j)*pp+1:j*pp+pp)=THETA(:,(j-i)*pp+1:(j-i+1)*pp);
    end;
end;

    
Future = zeros(ksf*p-p,(ksf-1)*pp);

for i=2:ksf-1,
    for j=1:i-1,
        Future((i-1)*p+1:i*p,(j-1)*pp+1:j*pp)=THETA(:,(ksp-i+j)*pp+1:(ksp-i+j+1)*pp);
      
    end;
end;


W = zeros((ksf-1)*p,(ksf-1)*p);
N


for j=1:N,
    
   yc=y(:,ksp+j:ksp+ksf-1+j-1);
   yc=yc(:);
   zc = z(:,ksp+j:ksp+ksf-1+j-1);
   zc=zc(:);
   yfc = yc-Future*zc;
   W = ((j-1)*W+yfc*yfc')/j;
if mod(j,100)==0,
        j
    end;
end;

Wp = pinv(sqrtm(W));
    



%[U S V]=svd([Yh*Yh' Yh1*Yh1']);

SigmaZpp=zeros((ksp+1)*pp,(ksp+1)*pp);
N

for j=1:N,
    zpp = z(:,j:j+ksp);
    zpp = zpp(:);
    SigmaZpp = ((j-1)*SigmaZpp + zpp*zpp')/j;
if mod(j,100)==0,
        j
    end;
end;


[U S V]=svd([Wp*Past*SigmaZpp*Past'*Wp']);

save sim_y60_u60_04_09_06_kf10_kp15_datirealplant_closedloop_std15_noAtm

if min(n)<max(n),     % if def_order
      figure
      dS=diag(S);
      testo=(diag(S));
      ndef=max(max(find(testo>(max(testo)+min(testo))/2)));
      bar(((testo)));
      disp('Press any key to continue');
       
      title('Select model order in command window.')
      disp(['default=', num2str(ndef)])
      n=input('Select model order:(''Return'' gives default) ');
      if isempty(n)
         %testo=log(diag(SS));
         n=ndef;
         disp(['Order chosen to ',int2str(n)]);
      end
      close;
     else n=n; 
end  
 
%
Gammak= U(:,1:n);

pGammak=Gammak'*Wp; 
%Yh1 = U1(:,1:n)*S1(1:n,1:n)*V1(:,1:n)';

ThetaX=pGammak*Past;
ThetaX1=pGammak*Past1;
ThetaEh=[zeros(p,ksp*pp) eye(p,p) zeros(p,pu)]-Past(1:p,:);
ThetaU=[zeros(pu,ksp*pp) zeros(pu,p) eye(pu,pu)];


%Eh = Y-Yh(1:p,:);

%Eh = Y-Y*pinv(Zpp(1:ksp*pp,:))*Zpp(1:ksp*pp,:);
%U = u(:,ksp+1:ksp+N)/sqrt(N);
%X = pGammak*Yh;
%X1 = pGammak*Yh1;

N0=min(1000,N);

Zpp=Hankel(z,ksp+1,N0);

Sigmaxinv=pinv(ThetaX*Zpp*Zpp'*ThetaX');
Sigmaxueinv = pinv([ThetaX ; ThetaU; ThetaEh]*Zpp*Zpp'*[ThetaX' ThetaU' ThetaEh']);
Theta2 = Past*Zpp*Zpp'*ThetaX'*Sigmaxinv;
Theta1 = ThetaX1*Zpp*Zpp'*[ThetaX' ThetaU' ThetaEh']*Sigmaxueinv;

N0
N
for j=N0+1:N,
    zpp=z(:,j:j+ksp);
    zpp=zpp(:);
    Sigmaxinv1=Sigmainv(Sigmaxinv,ThetaX*zpp,j);
    Sigmaxueinv1=Sigmainv(Sigmaxueinv,[ThetaX; ThetaU; ThetaEh]*zpp,j);
    Theta2 = ls_rec(Theta2,Sigmaxinv,Sigmaxinv1,ThetaX*zpp,Past*zpp,j);
    Theta1 = ls_rec(Theta1,Sigmaxueinv,Sigmaxueinv1,[ThetaX;ThetaU;ThetaEh]*zpp,ThetaX1*zpp,j);
    Sigmaxinv=Sigmaxinv1;
    Sigmaxueinv=Sigmaxueinv1;
    if mod(j,100)==0,
        j
    end;
end;


Ad = Theta1(1:n,1:n);
Bd = Theta1(1:n,n+1:n+pu);
K = Theta1(1:n,n+pu+1:n+pu+p);
Cd = Theta2(1:p,1:n);

Dd = zeros(p,pu);
return


    