function [L,dL,Xt,Gbar,Y,DY,innov,G] = hpmle(Theta,dummy);
%HPMLE Maximum likelihood estimation for a distorted economy.
%      [L,dL]=hpmle(Theta) has user-defined sections and thus must
%      be edited.  In hpmle.m the specific application (a home
%      production model) is described and instructions for the 
%      user are provided.

%      Ellen R. McGrattan, 5-10-90
%      Revised, ERM, 1-5-95

%      Model:      max  E[   sum beta^t r(X[t],u[t]) | X[0] ] 
%                 {u[t]}   t=0:inf 
%
%                  subject to  X[t+1]=A*X[t]+B*u[t]+e[t+1]
%                              X[0] given
%
%      Approximation:  
%                  r(X[t],u[t]) = X[t]'Q X[t]+u[t]'R u[t]+2X[t]'W u[t]  
%                                                 + higher order terms 
%           
%      Equilibrium in Approximated Economy:
%                  u[t] = -F X[t] 
%
%      Transition function for States in Approximated Economy:
%                  X[t+1] = (A-B*F) X[t] + C * omega[t+1]
%                         =  Ao * X[t] + C * omega[t+1]
%        
%      Observations:  z(1),z(2),...z(T)
%
%                  Y[t] = G X[t] + v[t]   
%                  Y[t] = g(z[t],Theta) 
%                  v[t] = D v[t-1] + w[t]    
%
%                  E[ e[t] e[s]'] = C*C', t=s
%                                 = 0      t~=s
%                  E[ w[t] w[s]'] = RR, t=s
%                                 = 0      t~=s
%
%      Gaussian Likelihood Function:
%                  L= T*( ln(|Omega| + trace(inv(Omega)*Sum) )
%
%                  Omega = E[      ]
%                  Sum    = sum(innov(t)*innov(t)')/T
%                          t=1:T
%
%
%      Miscellaneous: 
%                  x[t]  = set of variables in return function
%                        = D*X[t] + E*u[t], D,E are selector matrices
%                  param = vector of parameters needed to evaluate
%                          the return function 
%                  rfn   = a string giving name of m-file with 
%                          inputs x and param and outputs: 
%                          b   = dr(x,param)/x 
%                          q   = d^2r(x,param)/dx^2
%                          cub = d^3r(x,param)/dx^3
%                          du  = dr(x,param)/dparam
%                          db  = db/dparam 
%                          dq  = dq/dparam
%              
%                  

%      References:
%      [1] Wilson, David A., Alok Kumar, "Derivative Computations
%          for the Log Likelihood Function," IEEE Transactions on
%          Automatic Control, Vol. AC-27, No. 1, pp. 230-232, 1982. 
%      [2] Zadrozny, Peter, "Analytic Derivatives for Estimation of
%          Discrete-Time, Linear-Quadratic, Dynamic, Optimization
%          Models," Econometrica, 56:467-472, 1988. 

% Model: Home Production, version 3
%  U  = u^(1-risk)/(1-risk)
%  u  = c^b*l^(1-b)
%  c  = ces(cp,cg)                                     a1,b1
%  cp = ces(cm,cn)                                     a2,b2
%  cg = g*Y
%  cm = ces(km,exp(sm)*hm) - cg -i                     a4,b4
%  cn = ces(kn,exp(sn)*hn)                             a3,b3
%  l  = H-2*h
%  h  = ces(hm,hn)                                     a0,b0
%  k  = km+kn

global zxvar
Theta = Theta(:);

%---------------------------------------------------------------------
% (OPTIONAL)
% 1. Provide default values of the parameters:  
%    [a0,b0,a1,b1,a2,b2,a3,b3,a4,b4,b,risk,delta,beta,grate, ...
%        c0(:)',c1(:)',c2(:)',c3(:)',c4(:)',varm',coefm']';


%---------------------------------------------------------------------

Tsize = ones(size(Theta));    % default values for optional Tsize,Lsize
Lsize = 1;

%---------------------------------------------------------------------
% (OPTIONAL)
% 2. Scale Theta and L by choosing Tsize, Lsize.  I suggest having L 
%    and Theta of order unity. 

Tsize=[ ...
     4.118451340790457e-01
     3.264667093451085e-01
     1.395903314754713e-01
     2.694731026194350e-01
     2.057384105018968e-01
     5.395877388567430e-02
     4.888062254454232e-01
     5.799121288582520e+00
     2.948621262659013e-02
     9.867635335825705e-01
    -2.183641008599923e-03
     7.301015559557668e-02
     4.534967512554883e-02
     1.522472481446659e-02
     1.314894445924849e+00
     3.315813413313373e-01
     1.841619174157227e-02
    -7.771953363140530e-02
     1.450123131977230e-02
     9.452828626949317e-01
    -5.171350963936094e-02
    -2.114930805598293e-02
     9.500362185514252e-01
    -7.836341467720140e-02
     1.303132275532359e-01
     1.378608347770163e+00
     4.058982656548626e-02
     2.673719355316040e-01
    -7.922908762167054e-02
     1.253003934550169e-01
     1.250693215465323e+00
    -3.534582890335129e-01
    -1.819689457511789e-01
     6.646846575863550e-03
     7.387861881927069e-02
    -9.202732377225567e-03
     4.359448373920997e-02
     4.426952430338023e-02
     2.138762124353035e-02
     4.779830451464454e-02
     1.266737258558379e-01
    -2.096646711069167e-01
    -4.471659731987154e-01
    -3.783580321263214e-02
    -4.215520407704157e-01
    -1.134612040125469e-01
    -1.125111101169787e-01
    -3.423925351660507e-01
     2.086022118057248e-03
    -3.901973210267111e-04
     8.351910844774578e-04
     1.075949020701798e-03
     9.868438788995194e-03
     9.744547753546014e-05
     1.251783699168274e-04
     5.362462614506805e-02
     4.359639794327184e-03
     1.115332548819985e-03
     8.656802387997009e-04
     2.977357743939200e+02
     9.231503181059209e-01
     2.953371525874999e+01
     1.024018769950728e+00
     1.145726455199807e+01
     1.106326457772020e-02
     6.318658434927192e-05
     2.370033737744029e-06
     6.377396318999999e-01
     4.009464154894549e-01
     6.729778965000002e-01
     4.999709753489135e-04
     5.348447493416087e-01
     5.010795896777193e-03
     7.998670494727780e-01
     7.997984315320779e-01
];

Tsize=Tsize([1:22,24:39,41:59,61,63:67,69,71:75]);
Lsize=4500;

%---------------------------------------------------------------------
Theta = diag(Tsize)*Theta;

%---------------------------------------------------------------------
% 3. Assign elements of Theta to underlying parameters.
%
%    Parameters to be estimated
%
%    [a0,b0,a1,b1,a2,b2,a3,b3,a4,b4, b,risk,delta,beta,grate, ...
%      1  2  3  4  5  6  7  8  9 10 11   12    13   14    15
%
%     c0(:)',c1(:)',c2(:)',c3(:)',c4(:)',varm',coefm']';
%        20     45     70     95     120  128    136
%
%    ind(i) = 1 if ith parameter to be estimated
%    ind(i) = 0 otherwise
%

ind0            = zeros(136,1);
ind0            = [0,0,0,0,1,1,1,1,1,1,1,1,1,1,0, 1,1,0,1,1,  ...
                   1,1,0,1,1, 1,1,0,1,1, 0,0,1,0,0, 1,1,0,1,1, 1,1,0,1,1, ...
                   1,1,0,1,1, 1,1,0,1,1, 0,0,1,0,0, 1,1,0,1,1, 1,1,0,1,1];
ind0            = [ind0,  ...
                   0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, ...
                   1,1,0,1,1, 0,1,0,1,1, 0,0,1,0,0, 0,0,0,1,1, 0,0,0,0,1, ...
                   1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1];

ind             = zeros(136,1);
ind             = [0,0,0,0,1,1,1,1,1,1,1,1,1,1,0, 1,1,0,1,1,  ...
                   1,1,0,1,1, 1,1,0,1,1, 0,0,0,0,0, 1,1,0,1,1, 1,1,0,1,1, ...
                   1,1,0,1,1, 1,1,0,1,1, 0,0,0,0,0, 1,1,0,1,1, 1,1,0,1,1];
ind             = [ind,  ...
                   0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, ...
                   1,1,0,1,1, 0,1,0,1,1, 0,0,1,0,0, 0,0,0,1,1, 0,0,0,0,1, ...
                   0,1,0,1,1,1,1,1, 0,1,0,1,1,1,1,1];

fin             = find(ind);
si              = cumsum(ind);
i               = find(si==1);
si(1:i-1)       = ones(1,i-1);
np              = sum(ind);
a0              = .5;
b0              = .999;
a1              = .9999;
b1              = .00001;
a2              = Theta(1);
b2              = Theta(2);
a3              = Theta(3);
b3              = Theta(4);
a4              = Theta(5);
b4              = Theta(6);
b               = Theta(7);
risk            = Theta(8);
delta           = Theta(9);
beta            = Theta(10);
grate           = 1.0;
c0              = zeros(5,1);
c0([1,2,4,5])   = Theta([12:15]-1);
c1              = ones(5); c1(3,[1,2,4,5])=zeros(1,4); c1(:,3)=zeros(5,1);
c2              = c1;
%c1(3,3)        = 1;
%c2(3,3)        = 1;
c1(find(c1))    = Theta([16:31]-1);
c1(3,3)         = 9.500362185514252e-01;
c2(find(c2))    = Theta([32:47]-1);
c2(3,3)         = 4.809830451464454e-02-.0003;
c3              = zeros(5);
c4              = zeros(5);
c4([1,2,4,5],1) = Theta([48:51]-1);
c4([2,4,5],2)   = Theta([52:54]-1);
c4(3,3)         = Theta(54);
c4([4,5],4)     = Theta([55:56]);
c4(5,5)         = Theta(57);
varm            = zeros(8,1);
coefm           = zeros(8,1);
varm([2,4:8])   = Theta([60:65]-2);
coefm([2,4:8])  = Theta([66:71]-2);


varm(1)         = 2.978357743939200e+02-.1;
coefm(1)        = 6.377396318999999e-01;
varm(3)         = 2.954371525874999e+01-.01;
coefm(3)        = 6.729778965000001e-01;


%---------------------------------------------------------------------

% parameter vector:
param= [a0,b0,a1,b1,a2,b2,a3,b3,a4,b4,b,risk,delta,beta,grate, ...
        c0(:)',c1(:)',c2(:)',c3(:)',c4(:)',varm',coefm']';
%---------------------------------------------------------------------
% 4. Set gratein = i   if the ith element of Theta corresponds to grate
%                = 0   if grate is not to be estimated     
%---------------------------------------------------------------------

RR    = diag(varm);
D     = diag(coefm);
X     = zeros(14,1);
X(2)  = 1;
tm1   = c1+c2;
X(3:7)= (eye(5)-tm1)\c0;
s=[sprintf('means of exog states = %g,',X(3)),sprintf('%g,',X(4)), ...
   sprintf('%g,',X(5)),sprintf('%g,',X(6)),sprintf('%g',X(7))];
%disp(s);

%---------------------------------------------------------------------
% 5. Put bounds on Theta if there are values of the parameters for 
%    which L cannot be computed.

emax  = 0.998;
hmax  = 1.65;
hmax  = 1.1;
imax  = 54/50;
kmax  = 502/500;
lb    = [ .05; -10; 0; -10;  0; -10; 0; -10; 0; -10;     0;  ...
          -10; .001;     0; 0.999; -100;-100;-100;0;0; -20000*ones(100,1); ...
           zeros(8,1);     -.99*ones(8,1)];
ub    = [  1;    1; 1;   1;  1;   1; 1;   1; 1;   1; 0.999;  ...
           7; .040; 0.999;  1.01; 100000*ones(105,1);  ...
          100000*ones(8,1); 0.8*ones(8,1)];

L     = 0;
dL    = zeros(np,1);

if any(param(129:136)>1) | any(abs(eig(tm1))>1) |  ...
   any(X(4)>4.0) | any(X([3,6,7])<0.05) | any(varm<0) | beta>.999 | X(4)<1; 
  disp('Parameter vector such that likelihood can''t be computed');
  L  = 1e+15;
  dL = 1e+15*ones(size(Theta));
  return
end;

lb    = lb(fin);
ub    = ub(fin);
gamma = Lsize*1e+4;
varmh = varm(2)/(1-coefm(2)*coefm(2));
varmi = varm(3)/(1-coefm(3)*coefm(3))/50;
varmk = varm(1)/(1-coefm(1)*coefm(1))/500;
if any(Theta>ub) | any(Theta<lb) | any(abs(eig(tm1))>emax) | varmh>hmax ...
  | varmi>imax | varmk>kmax;
  penalty = gamma * ( sum(max(Theta-ub,0).^2 + max(lb-Theta,0).^2) ...
                     +sum(max(abs(eig(tm1))-emax,0).^2)  ...
                     +max(varmh-hmax,0)^2  +max(varmi-imax,0)^2 ...
                     +max(varmk-kmax,0)^2  );
  %disp(sprintf('Penalty = %g',penalty));
  L   = L  + penalty;
  dL  = dL + gamma * (2*max(Theta-ub,0) - 2*max(lb-Theta,0)); 
  dL(si(121)) = dL(si(121)) + 2*gamma*max(varmk-kmax,0)/(1-coefm(1)* ...
                coefm(1))*ind(121)/500;
  dL(si(129)) = dL(si(129)) + 2*gamma*max(varmk-kmax,0)*2*varmk*coefm(1)/ ...
                (1-coefm(1)*coefm(1))*ind(129)/500;
  dL(si(122)) = dL(si(122)) + 2*gamma*max(varmh-hmax,0)/(1-coefm(2)* ...
                coefm(2))*ind(122);
  dL(si(130)) = dL(si(130)) + 2*gamma*max(varmh-hmax,0)*2*varmh*coefm(2)/ ...
                (1-coefm(2)*coefm(2))*ind(130);
  dL(si(123)) = dL(si(123)) + 2*gamma*max(varmi-imax,0)/(1-coefm(3)* ...
                coefm(3))*ind(123)/50;
  dL(si(131)) = dL(si(131)) + 2*gamma*max(varmi-imax,0)*2*varmi*coefm(3)/ ...
                (1-coefm(3)*coefm(3))*ind(131)/50;
  dd  = [];
  for j=1:5;
    for i=1:5;
      del      = max(abs(c1(i,j))*1e-7,1e-8); 
      c1m      = c1;
      c1p      = c1;
      c1p(i,j) = c1(i,j) + del;
      c1m(i,j) = c1(i,j) - del;
      dd       = [dd;(gamma*sum(max(abs(eig(c1p+c2))-emax,0).^2)- ...
                      gamma*sum(max(abs(eig(c1m+c2))-emax,0).^2))/(2*del)];
    end;
  end; 
  for j=1:5;
    for i=1:5;
      del      = max(abs(c2(i,j))*1e-6,1e-8); 
      c2m      = c2;
      c2p      = c2;
      c2p(i,j) = c2(i,j) + del;
      c2m(i,j) = c2(i,j) - del;
      dd       = [dd;(gamma*sum(max(abs(eig(c1+c2p))-emax,0).^2)- ...
                      gamma*sum(max(abs(eig(c1+c2m))-emax,0).^2))/(2*del)];
    end;
  end; 
  i  = [21:70]';
  j  = i(find(ind(i)));
  dL(si(j)) = dL(si(j)) + dd(find(ind(21:70)));
  tem1=find(Theta>ub);
  tem2=find(Theta<lb);
  tem3=find(abs(eig(tm1))>emax);
  tem4=varmh>hmax;
  tem5=varmi>imax;
  tem6=varmk>kmax;
  if ~isempty(tem1);
    disp('the following parameters exceed the upper bound')
    disp(tem1)
  end;
  if ~isempty(tem2);
    disp('the following parameters are below the lower bound')
    disp(tem2)
  end;
  if ~isempty(tem3);
    disp(sprintf('the following eigenvalues of c1+c2 exceed %g',emax))
    disp(tem3)
  end;
  if tem4==1;
    disp(sprintf('the variance of measurement error in hours exceeds %g',hmax))
  end;
  if tem5==1;
    disp(sprintf(['the variance of measurement error in investment ', ...
                 'exceeds %g'],50*imax))
  end;
  if tem6==1;
    disp(sprintf(['the variance of measurement error in capital ', ...
                 'exceeds %g'],500*kmax))
  end;
end;
%---------------------------------------------------------------------


%---------------------------------------------------------------------
% 6. Specify Ay,Az,B,Cy,Sx,Su,Thet,Psi,ct.
                                         
Ay         = [(1-delta)/grate,zeros(1,11);
              0,1,zeros(1,10);
              zeros(5,1),  c0,c1,c2;
              zeros(5,2),  eye(5), zeros(5)];
Az         =  zeros(12,2);
By         = [0 0 1/grate 0;zeros(11,4)];
C          = zeros(12);
C(3:7,3:7) = c4;
Sx         = zeros(12,14);
Su         = zeros(12,4);
Sx(5:12,[1,3:7,13:14]) = eye(8);
Su(1:4,:)  = eye(4);
Su(5,1)    = -1;
ct         = 2;
Thet       = zeros(2,12);
Psi        = [eye(2),zeros(2)];
%---------------------------------------------------------------------


%---------------------------------------------------------------------
% 7. Specify steady states of X, u, x.                      
%    states:    k,1,g,sm,sn,tk,tn,g(-1),sm(-1),sn(-1),tk(-1),tn(-1),Km,Hm
%    controls:  km,hm,i,hn
%    x:         km,hm,i,hn,kn=k-km,g,sm,sn,tk,tn,Km,Hm

X(8:12)  = X(3:7);
khm      = ( (-a4 + ( (grate-beta*(1-delta)-beta*delta*X(6))/ ...
           (beta*a4*(1-X(6))) )^(b4/(1-b4)) )/(exp(X(4))^b4*(1-a4)) )^(-1/b4);
z0       = [200;200;20];
z        = newton('sshp4b',z0,[param(1:15);X(3:7);khm],1e-7,100);
hm       = z(1); 
hn       = z(2); 
khn      = z(3);
u(2)     = hm;
u(1)     = khm*u(2);
X(1)     = u(1)+khn*hn;
u(3)     = (grate-1+delta)*X(1);
u(4)     = hn;
u        = u(:);
X(13:14) = u(1:2);
x        = Sx*X+Su*u;
nx       = length(x);
ny       = 12;
nz       = 2;
nX       = 14;
nu       = 4;
%---------------------------------------------------------------------


%---------------------------------------------------------------------
% 8. Specify ufn (or replace this string in eval statement in (9) ).

ufn='hp7u';
%---------------------------------------------------------------------

% Compute Q,R,W and necessary matrices for gradQ, gradR, gradW. 
% retrieve vx  = dr(x,theta)/x 
%          vxx = d^2r(x,theta)/dx^2
% plus     vxxx= d^3r(x,theta)/dx^3
%          dv  = dr(x,theta)/dtheta
%          dvx = dvx/dtheta 
%          dvxx= dvxx/dtheta   if derivatives required
if nargout ==2;
  eval(['[v,vx,vxx,vxxx,dv,dvx,dvxx]=',ufn,'(x,param(1:13));'])
  %-------------------------------------------------------------------
  tem1 = np-sum(ind(1:13));
  dv   = [dv(find(ind(1:13)));zeros(tem1,1)];
  dvx  = [dvx(:,find(ind(1:13))),zeros(nx,tem1)];
  dvxx = [dvxx(:,find(ind(1:13))),zeros(nx*nx,tem1)];
  %-------------------------------------------------------------------
else;
  eval(['[v,vx,vxx]=',ufn,'(x,param(1:13));'])
end;
vx=vx(:);

x        = Sx*X+Su*u;
e        = zeros(nX,1);
e(ct)    = 1;
Q        = e*(v-vx'*x+.5*x'*vxx*x)*e'+.5*e*(vx'-x'*vxx)*Sx+ ...
           .5*Sx'*(vx-vxx*x)*e'+.5*Sx'*vxx*Sx;
R        = .5*Su'*vxx*Su;
W        = .5*e*(vx'-x'*vxx)*Su+.5*Sx'*vxx*Su;
Qy       = Q(1:ny,1:ny);
Qz       = Q(1:ny,ny+1:nX);
Wy       = W(1:ny,:);
Wz       = W(ny+1:nX,:);

% Compute the equilibrium decision function F

Ri       = inv(R);
BRi      = By*inv(R);
sbeta    = sqrt(beta);
ay       = sbeta*(Ay-BRi*Wy');
az       = sbeta*(Az-BRi*Wz');
by       = sbeta*By;
q        = Q - W*Ri*W';
qy       = q(1:ny,1:ny);
qz       = q(1:ny,ny+1:nX);
Iny      = eye(ny);
Inz      = eye(nz);
psi      = inv(Inz+Psi*Ri*Wz');
thet     = psi*(Thet-Psi*Ri*Wy');
psi      = psi*Psi;
ahat     = ay+az*thet;
bhat     = by+az*psi;
qhat     = qy+qz*thet;
atil     = ay-by*Ri*psi'*qz';
brb      = bhat*Ri*by';
[v,d]    = eig([ahat,zeros(ny);-qhat,Iny],[Iny,brb;zeros(ny),atil']);
d        = diag(d);
[d,in]   = sort(abs(d));
if (~( (d(ny) < 1) & (d(ny+1) > 1) ) ) error('Can''t order eigenvalues'); end;
W11      = v(1:ny,in(1:ny));
W21      = v(ny+1:2*ny,in(1:ny));
P        = real(W21/W11);
Rbpi     = inv(R+by'*P*bhat);
P        = qhat+atil'*P*ahat-atil'*P*bhat*Rbpi*by'*P*ahat;
f        = Rbpi*(by'*P*ahat);
F        = (R+Wz'*Psi)\(R*f+Wy'+Wz'*Thet);
% Compute the matrix for state transition function
Ao       = Ay+Az*Thet-Az*Psi*F-By*F;

%---------------------------------------------------------------------
% 9. Specify G.
%    G =[ K=Km+Kn; Hm; I; Y; g*Y; Kn; tk; tn];

yk  = a4*X(13)^b4;
yh  = (1-a4)*(exp(X(4))*X(14))^b4;
y   = (yk+yh)^(1/b4);
G   = [1,zeros(1,11);
       -F(2:3,:);
       [0,-y*yh*X(4)/(yk+yh),0,y*yh/(yk+yh),zeros(1,8)]- ...
          y*yk/(yk+yh)/X(13)*F(1,:)-y*yh/(yk+yh)/X(14)*F(2,:); 
       [0,-y*yh*X(4)*X(3)/(yk+yh)-X(3)*y,y,y*yh*X(3)/(yk+yh),zeros(1,8)]- ...
          y*yk*X(3)/(yk+yh)/X(13)*F(1,:)-y*yh*X(3)/(yk+yh)/X(14)*F(2,:);
       [1,zeros(1,11)]+F(1,:);
       0,0,0,0,0,1,zeros(1,6);
       0,0,0,0,0,0,1,zeros(1,5)];
Gp  = G';
%---------------------------------------------------------------------
 

%---------------------------------------------------------------------
% 10. Observations.
%     data = Y = [k ,hm, i, cm+i+g, g, kn, tk, tn]';
%              = [knnr+kncd+knr 
%                 lhours
%                 if+cd
%                 cn+cs+cd+if+g-csh
%                 g
%                 kncd+knr
%                 taxk
%                 taxn]
%
%                                
no  = 8;
T   = length(zxvar);
Y   = zxvar;
DY  = Y(2:T,:)-Y(1:T-1,:)*D';
T   = T-1;
%---------------------------------------------------------------------


%---------------------------------------------------------------------
% 11. Specify initial state and initial date, X0, t0, T0.

X0  = X(1:ny);     
t0  = 1;
T0  = 2;  % choose >=2
%---------------------------------------------------------------------

nct          = [1:ct-1,ct+1:ny];
Sigma        = C*C';
nobs         = T-t0+1;
Nobs         = T-T0+1;
Gbar         = G*Ao-D*G;
Rbar         = RR+G*Sigma*G';
[K,S]        = kfilter(Ao,Gbar,Sigma,Rbar,Sigma*G');
S0           = zeros(ny);     % alternative initial variance-covariances
%S0(nct,nct) = dlyap(Ao(nct,nct),Sigma(nct,nct));
S0           = S;

S1           = S0;
Xt           = zeros(T,ny);
innov        = zeros(T,no);
Xt(t0,:)     = X0';
innov(t0,:)  = DY(t0,:)-X0'*Gbar';
Omega        = Rbar+Gbar*S1*Gbar';
Omegai       = inv(Omega);
L            = L + log(det(Omega))+innov(t0,:)*Omegai*innov(t0,:)';
for i=t0+1:T0-1;
  K1         = (Ao*S1*Gbar'+Sigma*G')*Omegai;
  Xt(i,:)    = Xt(i-1,:)*Ao'+innov(i-1,:)*K1';
  innov(i,:) = DY(i,:)-Xt(i,:)*Gbar';
  S1         = Ao*S1*Ao'+Sigma-(Ao*S1*Gbar'+Sigma*G')*Omegai* ...
               (Gbar*S1*Ao'+G*Sigma);
  Omega      = Rbar+Gbar*S1*Gbar';
  Omegai     = inv(Omega);
  L          = L + log(det(Omega))+innov(i,:)*Omegai*innov(i,:)';
end;

Omega        = Rbar+Gbar*S*Gbar';
Omegai       = inv(Omega);
Xt(T0,:)     = Xt(T0-1,:)*Ao'+innov(T0-1,:)*K';
innov(T0,:)  = DY(T0,:)-Xt(T0,:)*Gbar';
for i=T0+1:T;
  Xt(i,:)    = Xt(i-1,:)*Ao'+innov(i-1,:)*K';
  innov(i,:) = DY(i,:)-Xt(i,:)*Gbar';
end;   
sumii        = innov(T0:T,:)'*innov(T0:T,:)/Nobs;
L            = L + Nobs*( log(det(Omega)) + trace(Omegai*sumii) );

%-------------------------------------------------------------------
% 12. Add Jacobian to L if gratein = 1

%-------------------------------------------------------------------
L=L/Lsize;

if nargout ==2;
  %-------------------------------------------------------------------
  % 13. Specify gradAy, gradAz, gradBy, gradC, gradRR, gradD, 
  %     gradx, gradX.

  % user-defined
  gradAy  = zeros(ny*ny,np);
  gradAz  = zeros(ny*nz,np);
  gradBy  = zeros(ny*nu,np);
  gradGp  = zeros(ny*no,np);
  gradC   = zeros(ny*ny,np);
  gradD   = zeros(no*no,np);
  gradRR  = zeros(no*no,np);

  % derivatives with respect to delta
  if ind(13);
    i                = si(13);
    gradAy(1,i)      = -1/grate;
  end;

  % derivatives with respect to c0
  gradAy(15,si(16))  = gradAy(15,si(16))  + ind(16);
  gradAy(16,si(17))  = gradAy(16,si(17))  + ind(17);
  gradAy(17,si(18))  = gradAy(17,si(18))  + ind(18);
  gradAy(18,si(19))  = gradAy(18,si(19))  + ind(19);
  gradAy(19,si(20))  = gradAy(19,si(20))  + ind(20);

  % derivatives with respect to c1
  gradAy(27,si(21))  = gradAy(27,si(21))  + ind(21);
  gradAy(28,si(22))  = gradAy(28,si(22))  + ind(22);
  gradAy(29,si(23))  = gradAy(29,si(23))  + ind(23);
  gradAy(30,si(24))  = gradAy(30,si(24))  + ind(24);
  gradAy(31,si(25))  = gradAy(31,si(25))  + ind(25);
  gradAy(39,si(26))  = gradAy(39,si(26))  + ind(26);
  gradAy(40,si(27))  = gradAy(40,si(27))  + ind(27);
  gradAy(41,si(28))  = gradAy(41,si(28))  + ind(28);
  gradAy(42,si(29))  = gradAy(42,si(29))  + ind(29);
  gradAy(43,si(30))  = gradAy(43,si(30))  + ind(30);
  gradAy(51,si(31))  = gradAy(51,si(31))  + ind(31);
  gradAy(52,si(32))  = gradAy(52,si(32))  + ind(32);
  gradAy(53,si(33))  = gradAy(53,si(33))  + ind(33);
  gradAy(54,si(34))  = gradAy(54,si(34))  + ind(34);
  gradAy(55,si(35))  = gradAy(55,si(35))  + ind(35);
  gradAy(63,si(36))  = gradAy(63,si(36))  + ind(36);
  gradAy(64,si(37))  = gradAy(64,si(37))  + ind(37);
  gradAy(65,si(38))  = gradAy(65,si(38))  + ind(38);
  gradAy(66,si(39))  = gradAy(66,si(39))  + ind(39);
  gradAy(67,si(40))  = gradAy(67,si(40))  + ind(40);
  gradAy(75,si(41))  = gradAy(75,si(41))  + ind(41);
  gradAy(76,si(42))  = gradAy(76,si(42))  + ind(42);
  gradAy(77,si(43))  = gradAy(77,si(43))  + ind(43);
  gradAy(78,si(44))  = gradAy(78,si(44))  + ind(44);
  gradAy(79,si(45))  = gradAy(79,si(45))  + ind(45);

  % derivatives with respect to c2 
  gradAy(87,si(46))  = gradAy(87,si(46))  + ind(46);
  gradAy(88,si(47))  = gradAy(88,si(47))  + ind(47);
  gradAy(89,si(48))  = gradAy(89,si(48))  + ind(48);
  gradAy(90,si(49))  = gradAy(90,si(49))  + ind(49);
  gradAy(91,si(50))  = gradAy(91,si(50))  + ind(50);
  gradAy(99,si(51))  = gradAy(99,si(51))  + ind(51);
  gradAy(100,si(52)) = gradAy(100,si(52)) + ind(52);
  gradAy(101,si(53)) = gradAy(101,si(53)) + ind(53);
  gradAy(102,si(54)) = gradAy(102,si(54)) + ind(54);
  gradAy(103,si(55)) = gradAy(103,si(55)) + ind(55);
  gradAy(111,si(56)) = gradAy(111,si(56)) + ind(56);
  gradAy(112,si(57)) = gradAy(112,si(57)) + ind(57);
  gradAy(113,si(58)) = gradAy(113,si(58)) + ind(58);
  gradAy(114,si(59)) = gradAy(114,si(59)) + ind(59);
  gradAy(115,si(60)) = gradAy(115,si(60)) + ind(60);
  gradAy(123,si(61)) = gradAy(123,si(61)) + ind(61);
  gradAy(124,si(62)) = gradAy(124,si(62)) + ind(62);
  gradAy(125,si(63)) = gradAy(125,si(63)) + ind(63);
  gradAy(126,si(64)) = gradAy(126,si(64)) + ind(64);
  gradAy(127,si(65)) = gradAy(127,si(65)) + ind(65);
  gradAy(135,si(66)) = gradAy(135,si(66)) + ind(66);
  gradAy(136,si(67)) = gradAy(136,si(67)) + ind(67);
  gradAy(137,si(68)) = gradAy(137,si(68)) + ind(68);
  gradAy(138,si(69)) = gradAy(138,si(69)) + ind(69);
  gradAy(139,si(70)) = gradAy(139,si(70)) + ind(70);

  % derivatives with respect to c3 (need to fill in if 3rd lag is added)
 
  % derivatives with respect to c4
  gradC(27,si(96))   = gradC(27,si(96))   + ind(96);
  gradC(28,si(97))   = gradC(28,si(97))   + ind(97);
  gradC(29,si(98))   = gradC(29,si(98))   + ind(98);
  gradC(30,si(99))   = gradC(30,si(99))   + ind(99);
  gradC(31,si(100))  = gradC(31,si(100))  + ind(100);
  gradC(39,si(101))  = gradC(39,si(101))  + ind(101);
  gradC(40,si(102))  = gradC(40,si(102))  + ind(102);
  gradC(41,si(103))  = gradC(41,si(103))  + ind(103);
  gradC(42,si(104))  = gradC(42,si(104))  + ind(104);
  gradC(43,si(105))  = gradC(43,si(105))  + ind(105);
  gradC(51,si(106))  = gradC(51,si(106))  + ind(106);
  gradC(52,si(107))  = gradC(52,si(107))  + ind(107);
  gradC(53,si(108))  = gradC(53,si(108))  + ind(108);
  gradC(54,si(109))  = gradC(54,si(109))  + ind(109);
  gradC(55,si(110))  = gradC(55,si(110))  + ind(110);
  gradC(63,si(111))  = gradC(63,si(111))  + ind(111);
  gradC(64,si(112))  = gradC(64,si(112))  + ind(112);
  gradC(65,si(113))  = gradC(65,si(113))  + ind(113);
  gradC(66,si(114))  = gradC(66,si(114))  + ind(114);
  gradC(67,si(115))  = gradC(67,si(115))  + ind(115);
  gradC(75,si(116))  = gradC(75,si(116))  + ind(116);
  gradC(76,si(117))  = gradC(76,si(117))  + ind(117);
  gradC(77,si(118))  = gradC(77,si(118))  + ind(118);
  gradC(78,si(119))  = gradC(78,si(119))  + ind(119);
  gradC(79,si(120))  = gradC(79,si(120))  + ind(120);

  % derivatives with respect to varm
  gradRR(1,si(121))  = gradRR(1,si(121))  + ind(121);
  gradRR(10,si(122)) = gradRR(10,si(122)) + ind(122);
  gradRR(19,si(123)) = gradRR(19,si(123)) + ind(123);
  gradRR(28,si(124)) = gradRR(28,si(124)) + ind(124);
  gradRR(37,si(125)) = gradRR(37,si(125)) + ind(125);
  gradRR(46,si(126)) = gradRR(46,si(126)) + ind(126);
  gradRR(55,si(127)) = gradRR(55,si(127)) + ind(127);
  gradRR(64,si(128)) = gradRR(64,si(128)) + ind(128);


  % derivatives with respect to coefm
  gradD(1,si(129))   = gradD(1,si(129))   + ind(129);
  gradD(10,si(130))  = gradD(10,si(130))  + ind(130);
  gradD(19,si(131))  = gradD(19,si(131))  + ind(131);
  gradD(28,si(132))  = gradD(28,si(132))  + ind(132);
  gradD(37,si(133))  = gradD(37,si(133))  + ind(133);
  gradD(46,si(134))  = gradD(46,si(134))  + ind(134);
  gradD(55,si(135))  = gradD(55,si(135))  + ind(135);
  gradD(64,si(136))  = gradD(64,si(136))  + ind(136);

  % derivatives for Thet, Psi
  gradTh = zeros(24,np);
  gradPs = zeros(8,np); 

  % gradx, gradX
  tm4 = fin(find(fin<96));
  del = diag(max(abs(param(1:95))*1e-5,1e-8));
  for i=1:length(tm4);
    gradx(:,i) = (grad7(param(1:95)+del(:,tm4(i))) -  ...
                  grad7(param(1:95)-del(:,tm4(i))))/(2*del(tm4(i),tm4(i)));
  end; 
  gradx = [gradx,zeros(12,np-length(tm4))];
  gradX([1,3:12],:) = [gradx(1,:)+gradx(5,:); ...
                       gradx(6:10,:);gradx(6:10,:)];
  gradX([13:14],:)  = [gradx(1:2,:)];
  %-------------------------------------------------------------------

  tem1    = zeros(ny,nu);
  tem1(:) = [1:ny*nu]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Syu     = zeros(ny,nu);
  for i=1:ny*nu;
    Syu(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(nu,ny);
  tem1(:) = [1:ny*nu]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Suy     = zeros(nu,ny);
  for i=1:ny*nu;
    Suy(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(ny,nz);
  tem1(:) = [1:ny*nz]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Syz     = zeros(ny,nz);
  for i=1:ny*nz;
    Syz(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(nz,nu);
  tem1(:) = [1:nz*nu]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Szu     = zeros(nz,nu);
  for i=1:nz*nu;
    Szu(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(ny);
  tem1(:) = [1:ny*ny]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Syy     = zeros(ny);
  for i=1:ny*ny;
    Syy(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(no);
  tem1(:) = [1:no*no]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Soo     = zeros(no);
  for i=1:no*no;
    Soo(tem1(i),tem2(i))=1;
  end;
  tem1    = zeros(ny,no);
  tem1(:) = [1:ny*no]';
  tem2    = tem1'; 
  tem2    = tem2(:);
  tem1    = tem1(:);
  Syo     = zeros(ny,no);
  for i=1:ny*no;
    Syo(tem1(i),tem2(i))=1;
  end;
  gradvx   = vxx*gradx+dvx;
  gradvxx  = vxxx*gradx+dvxx;
  e        = e(1:ny);
  sx       = Sx(:,1:ny);
  sz       = Sx(:,ny+1:nX);

  gradQy   = kron(e,e)*dv(:)'-kron(e*x',e)*gradvx-kron(e,e*vx')*gradx+ ...
             .5*kron(e*x',e*x')*gradvxx+ ...
             .5*(kron(e*x'*vxx,e)+kron(e,e*x'*vxx))*gradx+ ...
             .5*(kron(sx',e)+kron(e,sx'))*gradvx- ...
             .5*(kron(sx',e*x')+kron(e*x',sx'))*gradvxx- ...
             .5*(kron(sx'*vxx,e)+kron(e,sx'*vxx))*gradx+ ...
             .5*kron(sx',sx')*gradvxx;
  gradQz   = .5*kron(sz',sx')*gradvxx+.5*kron(sz',e)*gradvx- ...
             .5*kron(sz'*vxx,e)*gradx-.5*kron(sz',e*x')*gradvxx;
  gradR    = .5*kron(Su',Su')*gradvxx;
  gradWy   = .5*kron(Su',sx')*gradvxx+.5*kron(Su',e)*gradvx- ...
             .5*kron(Su',e*x')*gradvxx-.5*kron(Su'*vxx,e)*gradx;
  gradWz   = .5*kron(Su',Sx(:,ny+1:nX)')*gradvxx;
  Inu      = eye(nu);
  PRWi     = -inv(Inz+Psi/R*Wz');
  RWPi     = inv(R+Wz'*Psi);
  ftil     = (R+bhat'*P'*by)\bhat'*P'*atil;
  aotil    = atil-by*ftil;
  kwri     = kron(Wy*Ri,Iny);
  graday   = sbeta*(gradAy-kwri*gradBy+ ...
             kron(Wy*Ri,BRi)*gradR-kron(Iny,BRi)*Syu*gradWy);
% gradaz   = sbeta*(gradAz-kron(Wz*Ri,Iny)*gradBy+ ...
%            kron(Wz*Ri,BRi)*gradR-kron(Inz,BRi)*Szu*gradWz);
  gradaz   = sbeta*(kron(Wz*Ri,Iny)*gradBy+ ...
             kron(Wz*Ri,BRi)*gradR-kron(Inz,BRi)*Szu*gradWz);
  gradby   = sbeta*gradBy;

  graday(:,si(14)) = graday(:,si(14)) + 0.5*ay(:)/beta*ind(14); 
  gradaz(:,si(14)) = gradaz(:,si(14)) + 0.5*az(:)/beta*ind(14); 
  gradby(:,si(14)) = gradby(:,si(14)) + 0.5*by(:)/beta*ind(14); 

% gradth   = kron((Wy+thet'*Wz)*Ri,PRWi)*gradPs-kron((Wy+thet'*Wz)*Ri, ...
%            PRWi*Psi*Ri)*gradR+kron(thet',PRWi*Psi*Ri)*Szu*gradWz- ...
%            kron(Iny,PRWi)*gradTh+kron(Iny,PRWi*Psi*Ri)*Syu*gradWy;
  gradth   =-kron((Wy+thet'*Wz)*Ri,PRWi*Psi*Ri)*gradR+ ...
             kron(thet',PRWi*Psi*Ri)*Szu*gradWz+ ...
             kron(Iny,PRWi*Psi*Ri)*Syu*gradWy;
% gradps   = kron(psi'*Wz*Ri,PRWi)*gradPs-kron(psi'*Wz*Ri,PRWi*Psi*Ri)* ...
%            gradR+kron(psi',PRWi*Psi*Ri)*Szu*gradWz-gradPs;
  gradps   =-kron(psi'*Wz*Ri,PRWi*Psi*Ri)*gradR+ ...
             kron(psi',PRWi*Psi*Ri)*Szu*gradWz;
  gradah   = graday+kron(thet',Iny)*gradaz+kron(Iny,az)*gradth;
  gradbh   = gradby+kron(psi',Iny)*gradaz+kron(Inu,az)*gradps;
  gradqy   = gradQy-kwri*gradWy+kron(Wy*Ri,Wy*Ri)*gradR ...
             -kron(Iny,Wy*Ri)*Syu*gradWy;
  gradqz   = gradQz-kron(Wz*Ri,Iny)*gradWy+kron(Wz*Ri,Wy*Ri)*gradR ...
             -kron(Inz,Wy*Ri)*Szu*gradWz;
  gradqh   = gradqy+kron(thet',Iny)*gradqz+kron(Iny,qz)*gradth;
  gradat   = graday-kron(qz*psi*Ri,Iny)*gradby+kron(qz*psi*Ri,by*Ri)* ...
             gradR-kron(qz,by*Ri)*Szu*gradps-kron(Iny,by*Ri*psi')*Syz*gradqz;
  tem      = sparse(eye(ny*ny)-sbeta*kron(Ao',aotil'));
  gradP    = tem\ ...
             (gradqh+kron(Iny,aotil'*P)*gradah+sbeta*kron(Ao'*P',Iny)* ...
             Syy*gradat-kron(f',aotil'*P)*gradbh-sbeta* ...
             kron(Ao'*P',ftil')*Syu*gradby+kron(f',ftil')*gradR);
  gradf    = -kron(f',Rbpi)*gradR+sbeta*kron(Ao'*P',Rbpi)*Syu*gradby ...
             +sbeta*kron(Ao',Rbpi*by')*gradP+kron(Iny,Rbpi*by'*P)* ...
             gradah-kron(f',Rbpi*by'*P)*gradbh;
% gradF    = kron(f'-F',RWPi)*gradR+kron(Iny,RWPi*R)*gradf+kron(Iny,RWPi)* ...
%            Syu*gradWy+kron((Thet-Psi*F)',RWPi)*Szu*gradWz+ ...
%            kron(Iny,RWPi*Wz')*gradTh-kron(F',RWPi*Wz')*gradPs;
  gradF    = kron(f'-F',RWPi)*gradR+kron(Iny,RWPi*R)*gradf+kron(Iny,RWPi)* ...
             Syu*gradWy+kron((Thet-Psi*F)',RWPi)*Szu*gradWz;
  gradAo   = (gradah-kron(f',Iny)*gradbh-kron(Iny,bhat)*gradf)/sbeta;
  gradAo(:,si(14)) = gradAo(:,si(14))-.5*Ao(:)/beta*ind(14);

  %-------------------------------------------------------------------
  % 14.  Specify gradGp.

  gradyk           = zeros(1,np);
  gradyk(si(9))    = yk/a4*ind(9);
  gradyk(si(10))   = gradyk(si(10)) + yk*log(X(13))*ind(10); 
  gradyk           = gradyk + b4*yk/X(13)*gradX(13,:);

  gradyh           = zeros(1,np);
  gradyh(si(9))    = -yh/(1-a4)*ind(9);
  gradyh(si(10))   = gradyh(si(10)) + yh*(X(4)+log(X(14)))*ind(10);
  gradyh           = gradyh + b4*yh/X(14)*gradX(14,:)+b4*yh*gradX(4,:);

  grady            = zeros(1,np);
  grady(si(10))    = -y*log(yk+yh)/b4/b4*ind(10);
  grady            = grady + y/(b4*(yk+yh))*(gradyk+gradyh);

  gradFp           = Suy*gradF;
  gradGp           = zeros(ny*no,np);
  gradGp(13:36,:)  = -gradFp(13:36,:);
  gradGp(61:72,:)  = gradFp(1:12,:); 

  % derivatives of G(4,:)
  tem1             = grady/y+gradyh/yh-(gradyk+gradyh)/(yk+yh);
  tem2             = tem1+gradyk/yk-gradyh/yh;
  g41              = [0,-y*yh*X(4)/(yk+yh),0,y*yh/(yk+yh),zeros(1,8)];
  g42              = -y*yk/(yk+yh)/X(13);
  g43              = -y*yh/(yk+yh)/X(14);
  gradGp(37:48,:)  = g42*F(1,:)'*(tem2-gradX(13,:)/X(13))+ ...
                     g42*gradFp(1:12,:)+g43*F(2,:)'*(tem1- ...
                     gradX(14,:)/X(14))+g43*gradFp(13:24,:);
  gradGp(38,:)     = gradGp(38,:) + g41(2)*(tem1+gradX(4,:)/X(4));
  gradGp(40,:)     = gradGp(40,:) + g41(4)*tem1;
  
  % derivatives of G(5,:)
  g51              = [0,-y*yh*X(4)*X(3)/(yk+yh)-X(3)*y,y,y*yh*X(3)/(yk+yh), ...
                      zeros(1,8)];
  g52              = -y*yk*X(3)/(yk+yh)/X(13);
  g53              = -y*yh*X(3)/(yk+yh)/X(14);
  gradGp(49:60,:)  = g52*F(1,:)'*(tem2-gradX(13,:)/X(13)+gradX(3,:)/X(3))+ ...
                     g53*F(2,:)'*(tem1-gradX(14,:)/X(14)+gradX(3,:)/X(3))+ ...
                     g52*gradFp(1:12,:)+g53*gradFp(13:24,:);
  gradGp(50,:)     = gradGp(50,:) + (X(3)*y+g51(2))*(tem1+gradX(4,:)/X(4)+ ...
                     gradX(3,:)/X(3))-y*gradX(3,:)-X(3)*grady; 
  gradGp(51,:)     = gradGp(51,:) + grady;
  gradGp(52,:)     = gradGp(52,:) + g51(4)*(tem1+gradX(3,:)/X(3));
  gradG            = Syo*gradGp;
  %-------------------------------------------------------------------
  
  gradX       = gradX(1:ny,:);
  dAo         = zeros(ny);
  dG          = zeros(no,ny);
  dC          = zeros(ny);
  dD          = zeros(no);
  dR          = zeros(no);
  dX          = zeros(ny,1);
  dS          = zeros(ny);
  dK          = zeros(ny,no);
  gradS0      = zeros(ny*ny,np);
  gradX0      = gradX;
  tem         = zeros(ny);
  sum1        = zeros(np,1);
  sum2        = sum1;
  tem0        =-[1;0;1;1;1;1;0;0]/grate;

  % derivative of S0, where  S0 = Ao*S0*Ao'+C*C'
% for j=1:np;
%   dAo(:)     = gradAo(:,j);
%   dC(:)      = gradC(:,j);
%   tem2       = dAo*S0*Ao'+dC*C';
%   tem1       = tem2+tem2';
%   tem(nct,nct)= dlyap(Ao(nct,nct),tem1(nct,nct));
%   gradS0(:,j) = tem(:);
% end

  % derivative of S0, where  S0 = S (from kfilter)
  AA          = Ao-K*Gbar;
  IKG         = Iny-K*G;
  for j=1:np
    dAo(:)       = gradAo(:,j);
    dC(:)        = gradC(:,j);
    dG(:)        = gradG(:,j);
    dD(:)        = gradD(:,j);
    dR(:)        = gradRR(:,j);
    tem2         = IKG*dAo*S*AA'+IKG*dC*C'*IKG'-K*dG*S+K*D*dG*S*AA'+ ...
                   K*dD*G*S*AA'+.5*K*dR*K'; 
    tem1         = tem2+tem2';
    tem(nct,nct) = dlyap(AA(nct,nct),tem1(nct,nct));
    gradS0(:,j)   = tem(:);
  end;


  S1        = S0;
  Omega     = Rbar+Gbar*S1*Gbar';
  Omegai    = inv(Omega);
  OG        = Omegai*Gbar;
  M         = Omegai-Omegai*innov(t0,:)'*innov(t0,:)*Omegai;
  xio       = Xt(t0,:)'*innov(t0,:)*Omegai;
  yio       = Y(t0,:)'*innov(t0,:)*Omegai;
  for j=1:np;
    dX(:)   = gradX0(:,j);
    dS(:)   = gradS0(:,j);
    dAo(:)  = gradAo(:,j);
    dG(:)   = gradG(:,j);
    dC(:)   = gradC(:,j);
    dR(:)   = gradRR(:,j);
    dD(:)   = gradD(:,j);
    sum1(j) = sum1(j)+2*trace(dAo*S1*Gbar'*M*G)+2*trace(dC*C'*G'*M*G) ...
                     +2*trace(dG*(Ao*S1*Gbar'*M-S1*Gbar'*M*D ...
                             +Sigma*G'*M))-2*trace(dD*G*S1*Gbar'*M) ...
                     +trace(dR*M)+trace(dS*Gbar'*M*Gbar);
    sum2(j) = sum2(j)-2*trace(dAo*xio*G)-2*trace(dG*(Ao*xio-xio*D)) ...
                     -2*trace(dD*(yio-G*xio)) ...
                     -2*trace(dX*innov(t0,:)*OG); 
  end
  for i=t0+1:T0-1;
    K1        = (Ao*S1*Gbar'+Sigma*G')*Omegai;
    AA        = Ao-K1*Gbar;
    IKG       = Iny-K1*G;
    tem1      = Ao*S1*Ao'+Sigma-K1*Gbar*S1*Ao'-K1*G*Sigma;
    gradK     = kron(Omegai*G*C,IKG)*gradC ...
               +kron(Omegai*G,IKG*C)*Syy*gradC ...
               +kron(OG*S1,IKG)*gradAo ...
               +kron(Omegai*G,AA*S1)*Syy*gradAo ...
               +kron(OG,AA)*gradS0 ...
               +kron(OG*S1*G',K1)*gradD ...
               -kron(Omegai,AA*S1*G')*Soo*gradD ...
               -kron(K1',K1)*gradG ...
               +kron(OG*S1,K1*D)*gradG ...
               -kron(Omegai*D,AA*S1)*gradGp ...
               +kron(Omegai,tem1)*gradGp ...
               -kron(Omegai,K1)*gradRR;
    gradX0    = AA*gradX0 +kron(Xt(i-1,:),IKG)*gradAo ...
                +kron(innov(i-1,:),Iny)*gradK ...
                -kron(Xt(i-1,:)*Ao',K1)*Syo*gradGp ...
                +kron(Xt(i-1,:),K1*D)*Syo*gradGp ...
                +kron(Xt(i-1,:)*G',K1)*gradD ...
                -kron(Y(i-1,:),K1)*gradD;
    S0        = S1;
    S1        = Ao*S1*Ao'+Sigma-(Ao*S1*Gbar'+Sigma*G')*Omegai* ...
               (Gbar*S1*Ao'+G*Sigma);
    Omega     = Rbar+Gbar*S1*Gbar';
    Omegai    = inv(Omega);
    OG        = Omegai*Gbar;
    M         = Omegai-Omegai*innov(i,:)'*innov(i,:)*Omegai;
    xio       = Xt(i,:)'*innov(i,:)*Omegai;
    yio       = Y(i,:)'*innov(i,:)*Omegai;
    %tem2      = kron(AA*S0,IKG)*gradAo ...
    %            +kron(C-K1*G*C,IKG)*gradC ...
    %            -kron(AA'*S0*Ao'+Sigma*IKG',K1)*Syo*gradGp ...
    %            +kron(AA*S0,K1*D)*Syo*gradGp ...
    %            +kron(AA*S0*G',K1)*gradD ...
    %            +0.5*kron(K1,K1)*gradRR;
    %gradS0    = kron(AA,AA)*gradS0+tem2+Syy*tem2;
    for j=1:np;
      dX(:)   = gradX0(:,j);
      dS(:)   = gradS0(:,j);
      dAo(:)  = gradAo(:,j);
      dG(:)   = gradG(:,j);
      dC(:)   = gradC(:,j);
      dR(:)   = gradRR(:,j);
      dD(:)   = gradD(:,j);
      tem2    = dAo*S0*Ao'+dC*C'-dC*C'*G'*K1'-K1*G*dC*C'-K1*dG*C*C' ...
                -dAo*S0*Gbar'*K1'-K1*dG*Ao*S0*Ao'-K1*G*dAo*S0*Ao' ...
                +K1*dD*G*S0*Ao'+K1*D*dG*S0*Ao'+.5*K1*dR*K1' ...
                +K1*dG*Ao*S0*Gbar'*K1'+K1*G*dAo*S0*Gbar'*K1' ...
                -K1*dD*G*S0*Gbar'*K1' ...
                -K1*D*dG*S0*Gbar'*K1'+K1*dG*C*C'*G'*K1'+K1*G*dC*C'*G'*K1';
      dS      = AA*dS*AA'+tem2+tem2';
      gradS0(:,j) = dS(:);

      sum1(j) = sum1(j)+2*trace(dAo*S1*Gbar'*M*G)+2*trace(dC*C'*G'*M*G) ...
                       +2*trace(dG*(Ao*S1*Gbar'*M-S1*Gbar'*M*D ...
                               +Sigma*G'*M))-2*trace(dD*G*S1*Gbar'*M) ...
                       +trace(dR*M)+trace(dS*Gbar'*M*Gbar);
      sum2(j) = sum2(j)-2*trace(dAo*xio*G)-2*trace(dG*(Ao*xio-xio*D)) ...
                       -2*trace(dD*(yio-G*xio)) ...
                       -2*trace(dX*innov(i,:)*OG); 
    end
  end
  dL          = dL+sum1+sum2; 
  AA          = Ao-K*Gbar;
  IKG         = Iny-K*G;
  Omega       = Rbar+Gbar*S*Gbar';
  Omegai      = inv(Omega);
  OG          = Omegai*Gbar;
  for j=1:np
    dAo(:)       = gradAo(:,j);
    dC(:)        = gradC(:,j);
    dG(:)        = gradG(:,j);
    dD(:)        = gradD(:,j);
    dR(:)        = gradRR(:,j);
    tem2         = IKG*dAo*S*AA'+IKG*dC*C'*IKG'-K*dG*S+K*D*dG*S*AA'+ ...
                   K*dD*G*S*AA'+.5*K*dR*K'; 
    tem1         = tem2+tem2';
    tem(nct,nct) = dlyap(AA(nct,nct),tem1(nct,nct));
    gradS0(:,j)   = tem(:);
  end;
  tem1        = Ao*S*Ao'+Sigma-K*Gbar*S*Ao'-K*G*Sigma;
  gradK       = kron(Omegai*G*C,IKG)*gradC ...
               +kron(Omegai*G,IKG*C)*Syy*gradC ...
               +kron(OG*S,IKG)*gradAo ...
               +kron(Omegai*G,AA*S)*Syy*gradAo ...
               +kron(OG,AA)*gradS0 ...
               +kron(OG*S*G',K)*gradD ...
               -kron(Omegai,AA*S*G')*Soo*gradD ...
               -kron(K',K)*gradG ...
               +kron(OG*S,K*D)*gradG ...
               -kron(Omegai*D,AA*S)*gradGp ...
               +kron(Omegai,tem1)*gradGp ...
               -kron(Omegai,K)*gradRR;
  gradX0      = AA*gradX0 +kron(Xt(T0-1,:),IKG)*gradAo ...
               +kron(innov(T0-1,:),Iny)*gradK ...
               -kron(Xt(T0-1,:)*Ao',K)*Syo*gradGp ...
               +kron(Xt(T0-1,:),K*D)*Syo*gradGp ...
               +kron(Xt(T0-1,:)*G',K)*gradD ...
               -kron(Y(T0-1,:),K)*gradD;
  lamb(T,:)   = innov(T,:)*OG;
  for i=T-1:-1:T0;
    lamb(i,:) = lamb(i+1,:)*AA+innov(i,:)*OG;  
  end;
  sumxi       = Xt(T0:T,:)'*innov(T0:T,:)/Nobs;
  sumxl       = Xt(T0:T-1,:)'*lamb(T0+1:T,:)/Nobs;
  sumil       = innov(T0:T-1,:)'*lamb(T0+1:T,:)/Nobs;
  sumyi       = Y(T0:T,:)'*innov(T0:T,:)/Nobs;
  sumyl       = Y(T0:T-1,:)'*lamb(T0+1:T,:)/Nobs;
  M           = Omegai-Omegai*sumii*Omegai;
  H           = Gbar'*M*Gbar-2*OG'*sumil*AA;
  Pi          = zeros(ny); 
  Pi(nct,nct) = dlyap(AA(nct,nct)',0.5*(H(nct,nct)+H(nct,nct)'));
  N           = M+K'*Pi*K;
  tem1        = 2*(S*Gbar'*M*G-sumxi*Omegai*G-sumxl*IKG-S*OG'* ...
                   sumil*IKG-S*AA'*sumil'*Omegai*G+S*AA'*Pi*IKG);
  tem2        = 2*C'*(G'*M*G-G'*Omegai*sumil*IKG-IKG'*sumil'*Omegai*G+ ...
                   IKG'*Pi*IKG);
  tem3        = 2*(Ao*S*Gbar'*M-S*Gbar'*M*D+Sigma*G'*M-Ao*sumxi*Omegai+ ...
                   sumxi*Omegai*D+Ao*sumxl*K-sumxl*K*D-Sigma*IKG'*sumil'* ...
                   Omegai+Sigma*G'*Omegai*sumil*K-Ao*S*AA'*sumil'*Omegai+ ...
                   S*AA'*sumil'*Omegai*D+Ao*S*OG'*sumil*K-S*OG'*sumil*K*D- ...
                   Ao*S*AA'*Pi*K+S*AA'*Pi*K*D-Sigma*IKG'*Pi*K);
  tem4        = 2*(-G*S*Gbar'*M-(sumyi-G*sumxi)*Omegai+(sumyl-G*sumxl)*K+ ...
                   G*S*AA'*sumil'*Omegai-G*S*OG'*sumil*K+G*S*AA'*Pi*K);
  tem5        = M+K'*Pi*K+2*Omegai*sumil*K;
  for j=1:np;
    dAo(:)  = gradAo(:,j);
    dC(:)   = gradC(:,j);
    dG(:)   = gradG(:,j);
    dD(:)   = gradD(:,j);
    dR(:)   = gradRR(:,j);
    dL(j,1) = dL(j,1)+ Nobs*(trace(dAo*tem1+dC*tem2)+ ...
              trace(dG*tem3+dD*tem4+dR*tem5)- ...
              2*trace(gradX0(:,j)*lamb(T0,:)/Nobs));
  end; 
  %-------------------------------------------------------------------
  % 15. Add derivatives of Y and Jacobian with respect to grate.
  %

  %-------------------------------------------------------------------
  dL=diag(Tsize)*dL/Lsize;
end;
