%  GAUSSMLE.M
%
%  Ellen R. McGrattan
%
%  created:        February 13, 1990
%  last revised:   February 16, 1990 
%

%  retrieve parameter values (param), A, B, C, D, E, Sigma, Omega 
%  gradA, gradB, gradS, X0

%eval(mlespec);

hsp4


%  retrieve steady states (analytical) and derivatives (gradx)

eval(hs3ssxu);
ns=length(X);
nc=length(u);
nx=length(x);

%  data


Omega=eye(5)*.000001;

X0=X;
load /home/erm/data/hsdata3
nobs=length(zxvar);
tem=lambda.^([1:nobs]');
Y=[zxvar(:,1)./tem,zxvar(:,2),zxvar(:,2).*(zxvar(:,3)-13.045*80*h1)./ ..
   (13.045*80*h2),zxvar(:,4)./tem,zxvar(:,5)./tem];

%  retrieve b=dr(x,theta)/x, q=d^2r(x,theta)/dx^2, cub=d^3r(x,theta)/dx^3
%  gradb, gradq

param=[param1;param2];
[ux,b,q,cub,du,db,dq]=hsu4(x,param);
p=1;
gradb=rand(nx,p);
gradq=rand(nx*nx,p);
gradx=rand(nx,p);
gradA=rand(ns*ns,p);
gradB=rand(ns*nc,p);
gradS=rand(ns*ns,p);
drdthet=rand(1,p);

b=b(:)


%  construct Q,R,W 

Q0=zeros(ns);
Q0(ct,ct)=ux-b'*x+x'*q*x/2;
d=(b'-x'*q)';
Q1=zeros(ns);
Q1(ct,:)=d'*D;
Q1(:,ct)=(d'*D)';
Q1=Q1/2;
Q1(ct,ct)=2*Q1(ct,ct);
Q=Q0+Q1+D'*q*D/2;
R=E'*q*E/2;
Q2=zeros(ns,nc);
Q2(ct,:)=(d'*E)/2;
W=Q2+D'*q*E/2;


%  make sure R is invertible and construct undiscounted, no cross-products
%  versions of A,B,Q,R 

if rank(R)>=length(R);
  An=sqrt(beta)*(A-B*(R\W'));
  Bn=sqrt(beta)*B;
  Qn=Q-W*(R\W');
else;
  disp('WARNING: R is not invertible')
  return
end;

[Fn,P]=double(An',Bn',Qn,R);
F=Fn'+(R\W');

Aon=An-Bn*Fn';
Ao=A-B*F;

tem1=zeros(ns,nc);
tem1(:)=[1:ns*nc]';
tem2=tem1'; 
tem2=tem2(:);
tem1=tem1(:);
Knk=zeros(ns,nc);
for i=1:ns*nc;
  Knk(tem1(i),tem2(i))=1;
end;
tem1=zeros(ns);
tem1(:)=[1:ns*ns]';
tem2=tem1'; 
tem2=tem2(:);
tem1=tem1(:);
Knn=zeros(ns*ns);
for i=1:ns*ns;
  Knn(tem1(i),tem2(i))=1;
end;

tem=(ns+1)*(ct-1);
gradQ=[zeros(tem,p);
       drdthet-x'*gradb+x'*q*gradx+kron(x',x')*gradq;
       zeros((ns*ns)-tem-1,p)];
tem= [zeros(ns*(ct-1),p);
      .5*D'*gradb-.5*D'*q*gradx-.5*kron(x',D')*gradq;
      zeros(ns*(ns-ct),p)];
gradQ=gradQ + tem + Knn*tem;
gradQ=gradQ + .5*kron(D',D')*gradq;
   
gradR=kron(E',E')*gradq/2;
gradWp=[zeros(nc*(ct-1),p);
        (E'*gradb-E'*q*gradx-kron(x',E')*gradq)/2;
        zeros((ns-ct)*nc,p)] +kron(D',E')*gradq/2; 
gradW=Knk*gradWp;
gradQn= gradQ-kron(W/R,eye(ns))*gradW+kron(W/R,W/R)*gradR;


ZO=inv(R+Bn'*P*Bn);
ZB=kron(Aon',ZO*Bn')*inv(eye(ns*ns)-kron(Aon',Aon'));
Z1=-kron(eye(ns),ZO*Bn'*P)-ZB*(kron(eye(ns),Aon'*P)+ ..
    kron(Aon'*P,eye(ns))*Knn); 
Z2=-kron(F',ZO*Bn'*P)-kron(Aon'*P,ZO)*Knk-ZB*(kron(F',Aon'*P)+ ..
    kron(Aon'*P,F')*Knk);
Z3=-ZB;
Z4=-kron(F',ZO)-ZB*kron(F',F');

gradAn=sqrt(beta)*gradA-sqrt(beta)*(kron(W/R,eye(ns))*gradB- ..
       kron(W/R,B/R)*gradR+kron(eye(ns),B/R)*gradWp);   % warning
gradBn=sqrt(beta)*gradB;
gradQn=gradQ-kron(W/R,eye(ns))*gradW+kron(W/R,W/R)*gradR- ..
       kron(eye(ns),W/R)*gradWp;
gradFn=Z1*gradAn+ Z2*gradBn+ Z3*gradQn+ Z4*gradR;
gradF=gradFn+kron(W/R,inv(R))*gradR+kron(eye(ns),inv(R))*gradWp;
gradAo=gradA-kron(F',eye(ns))*gradB-kron(eye(ns),B)*gradF;



%  data:  y(1), ... y(T)

T=nobs;

DEF=D-E*F;
[f,df]=hsf3(x(1:nov1),param1);
C =[DEF(2,:);DEF(4:5,:);df'*DEF(1:nov1,:)+[f-df'*x(1:nov1);
    zeros(ns-1,1)]'-DEF(6,:);DEF(6,:)];

[K,S]=kfilter(Ao,C,Sigma,Omega);


t0=3;

Xt(t0,:)=X0';
innov(t0,:)=Y(t0,:)-X0'*C';

sum1=innov(t0,:)'*innov(t0,:);
sum2=Xt(t0,:)'*innov(t0,:);
for i=t0+1:T;
  Xt(i,:)=Xt(i-1,:)*Ao'+innov(i-1,:)*K';
  innov(i,:)=Y(i,:)-Xt(i,:)*C';
  sum1=sum1+innov(i,:)'*innov(i,:);
  sum2=sum2+Xt(i,:)'*innov(i,:);
end;

sum1=sum1/(T-t0+1);
sum2=sum2/(T-t0+1);

Sigmaz=Omega+C*S*C';
L= (T-t0+1)*(log(det(Sigmaz))+trace(inv(Sigmaz)*sum1))


