function [L,dL,xt,Gbar,dy]=lqmle(Theta,dummy);
%LQMLE Maximum likelihood estimation for Hansen-Sargent LQ economies 
%      [L,dL]=lqmle(Theta) has user-defined sections and thus must
%      be edited.  In lqmle.m, the class of economies is described
%      and instructions for the user are provided.  

%      Ellen R. McGrattan,  6-17-90
%      Revised, ERM, 1-30-93 

%      Model: max  E[sum beta^t x[t]'Q x[t]+u[t]'R u[t]+2x[t]'W u[t]) | x[0]]
%            {u[t]} t=0:inf 
%
%                  subject to  x[t+1]=A*x[t]+B*u[t]+C*e[t+1],  e ~ N(0,I)
%                              x[0]=x0 given
%
%      where  [Q  W]  = -.5*(G'*G+H'*H) 
%             [W' R]
%
%             G = ug*[phic phig]^{-1}[0, gamma, ud, -phii] 
%
%             H = [lambda, phih*uc*[phic phig]^{-1}[gamma, ud, -phii] ]
%                 - [0, 0, ub, 0]
%
%             A = [deltah thetah*uc*[phic phig]^{-1}*[gamma, ud]  ]
%                 [  0           deltak                       0   ]
%                 [  0             0                         a22  ]
%
%             B = [ -thetah*uc*[phic phig]^{-1} ]
%                 [           thetak            ]  
%                 [             0               ]
%
%             C = [ 0  ]
%                 [ 0  ]
%                 [ c2 ]    
%
%             x(t) = [h(t-1)',k(t-1)',z(t)']'  
%             u(t) = i(t) 
%
%             h(t) = household capital stocks  (nh x 1)
%             k(t) = market capital stocks  (nk x 1) 
%             z(t) = taste (b(t)) and technology shocks (d(t))  (nz x 1)
%             i(t) = investment goods   (ni x 1)
%
%      Optimal Controls:
%             u[t] = -F x[t],   F = inv(R+beta*B'*P*B)*(beta*B'*P*A+W')
%                               P = Q+beta*A'*P*A-(beta*A'*P*B+W)*F 
%
%      Transition function for States:
%             x[t+1] = (A-B*F) x[t] + C*e[t+1]
%                    =  Ao x[t] + C*e[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[ w[t] w[s]'] = R, t=s
%                            = 0  t~=s
%
%      Gaussian Likelihood Function:
%             L= T*( ln(|Sigmaz| + trace(inv(Sigmaz)*Sum) ) + jacobian
%
%             Sigmaz    = E[y[t]-yhat[t]][y[t]-yhat[t]]'
%             yhat[t]   = E[y[t]|y[t-1],y[t-2],...,y[1],x[0]]
%             Sum       = sum(innov(t)*innov(t)')/T
%                        t=1:T
%             jacobian ~= 0 if g(z[t],Theta) is a nontrivial function in Theta
%
%
%      References:
%      [1] Hansen, Lars P. and Thomas J. Sargent, "Recursive Linear
%          Models of Dynamic Economies," manuscript.
%      [2] McGrattan, Ellen, "The Macroeconomic Effects of Distortionary
%          Taxation," mimeo, Duke University, 1990.
%      [3] 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. 
%      [4] Zadrozny, Peter, "Analytic Derivatives for Estimation of
%          Discrete-Time, Linear-Quadratic, Dynamic, Optimization
%          Models," Econometrica, 56:467-472, 1988. 
%
%      Reference [1] describes the models in detail. References
%      [2]-[4] describe the calculation of derivatives of F,P, and L.
%
%      User Instructions: Fill in required specifications in sections
%                         enclosed by (----) markings.  There are 13
%                         sections in all;  2 and 5 are optional. 

global ZVAR
Theta=Theta(:);

%---------------------------------------------------------------------
% 1. Provide default values of the parameters underlying the following
%    matrices:     deltak (nk x nk),thetak (nk x ni), phic (nd x nc),
%                  phig (nd x ng), phii (nd x ni), gamma (nd x nk),
%                  deltah (nh x nh), lambda (nb x nh), thetah (nh x nc),
%                  pih (nb x nc), a22 (nz x nz), c2 (nz x nw), x0 (nx x 1)
%    and values for the selector matrices:    ub (nb x nz), ud (nd x nz)
%    and a growth rate: grate
%    and a coefficient matrix for measurement error process: D
%    and a covariance matrix for measurement error process:  RR

% default values for parameters

alpha0 = 500;
alpha1 = 0.5;
beta   = 0.909;
beta   = 0.96;
delta  = 0.1;
delta  = 0.0;
gamma0 = 0.4;
gamma1 = 0.7;
f1     = 0.0001;
f3     = 0.0001;
f5     = 0.0001;
f7     = 0.0001;
g      = 0.85;
rho1   = 0.0;
rho2   = 0.0;
rho3   = 0.0;
sigma1 = 0;
sigma2 = 0;
sigma3 = 0;
muh    = 50.0;
muh    = 36.741;
mum    = 100.0;
mum    = 62.834;
x0     = zeros(8,1);
x0(2:5)= [17.56*ones(3,1);1];

no     = 3;
grate  = 1;
varm   = 1e-7*ones(no,1);
D      = zeros(no);


if no>2; varm(3)= 1e-7; end

%    Note: make sure the user provided derivatives are consistent with
%    the defined matrices here.  
%---------------------------------------------------------------------


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

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


%---------------------------------------------------------------------
% 3. Assign elements of Theta to underlying parameters.  Use the vector
%    ind to indicate which parameters are to be estimated and which
%    are to be fixed in estimation.  For example, if the vector of 
%    parameters is [beta,gamma,delta] and gamma is to be fixed during
%    estimation then set:
%       beta  = Theta(1);
%       delta = Theta(2);
%       ind   = [1,0,1];
%  
 
%    For the current example, the ordering of the parameter vector is:
%       alpha0,alpha1,beta,delta,gamma0,gamma1,f1,f3,f5,f7,
%       g,rho1,rho2,rho3,sigma1,sigma2,sigma3,muh,mum,
%       grate,varm
%         1      2     3     4     5      6    7  8  9  10 
%       11 12   13   14    15     16     17   18  19
%        20    23 
%

ind    = [1,1,0,0,1,1, 0,0,0,0, 1, 0,1,1, 0,1,1, 0,0, 0, 1,1,0];

alpha0 = Theta(1);
alpha1 = Theta(2);
gamma0 = Theta(3);
gamma1 = Theta(4);
g      = Theta(5);
rho2   = Theta(6);
rho3   = Theta(7);
sigma2 = Theta(8);
sigma3 = Theta(9);
varm(1)= Theta(10);
varm(2)= Theta(11);

f2     = 0.5*gamma0*g/f1;   
f4     = 0.5*gamma1*g/f3;  
f6     = 0.5/f5;
f8     = 0.5/f7;
a22    = [1 0 0 0; 0 rho1 0 0; 0 0 rho2 0; 0 0 0 rho3];
c2     = [0 0 0;sigma1 0 0; 0 sigma2 0; 0 0 sigma3];
deltah = 0.0;
deltak = [(1-delta),0,g;1,0,0;0,1,0];
gamma  = [0 0 0; f5*(1-delta) 0 g*f5; f1 0 0; 0 f3 0; 0 0 0];
lambda = 0.0;
phic   = [1 f5 0 0 -f7]';
phig   = [0 0 0 0; 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
phii   = [1 0 0 0 0]';
pih    = sqrt(1/alpha1);
thetah = 0.0;
thetak = [1 0 0]';
ub     = [pih*alpha0 0 0 pih];
ud     = [0 0 0 0; [f6;f2;f4]*[muh,1,0,0]; f8*[mum,0,1,0]];
RR     = diag(varm);

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


%---------------------------------------------------------------------
% 4. The position of the discount factor (beta) and the growth rate 
%    (grate) are set so that their derivatives can be computed directly.
%    Set betain = i   if the ith element of Theta corresponds to beta 
%               = 0   if beta is not to be estimated               
%    and do the same for gratein.

if ind(3);  betain=sum(ind(1:3));   else; betain=0;  end;
if ind(20); gratein=sum(ind(1:20)); else; gratein=0; end;
%---------------------------------------------------------------------



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

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

% ordering: alpha0,alpha1,beta,delta,gamma0,gamma1,f1,f3,f5,f7,
%           g,rho1,rho2,rho3,sigma1,sigma2,sigma3,muh,mum,
%           grate,varm
Ub = [10000;100;.999;1;10;10;1.;1.;1.;1.;1;.999;.999;.999;1000;1000;
      1000;5000;5000;1.1;10000;10000;10000];
Lb = [0;.00000001;.001;0;0;0;0.0000001;0.0000001;0.0000001;0.0000001;0;
     -.999;-.999;-.999;0;0;0;0;0;.999;0;0;0];
Ub = Ub(ind);
Lb = Lb(ind);
if any(Theta>Ub) | any(Theta<Lb);
  L = 1e+20; dL=1e+20*ones(size(Theta));
  return
end; 

%---------------------------------------------------------------------
% 6. Specify element in x corresponding to x(t)=1, all t: ct.  
              
ct=5;
%---------------------------------------------------------------------

[nb,nh] = size(lambda);
[nd,nc] = size(phic);
[nz,nw] = size(c2);
[tem,ng]= size(phig);
[nk,ni] = size(thetak);
nx      = nh+nk+nz;
n       = ni+nx;

phiin   = inv([phic phig]);
phiinc  = [eye(nc) zeros(nc,ng)]*phiin;
phiing  = [zeros(ng,nc) eye(ng)]*phiin;

a1  = thetah*phiinc*gamma;
a12 = [thetah*phiinc*ud; zeros(nk,nz)];
a11 = [deltah a1 ; zeros(nk,nh) deltak];
b11 = -thetah*phiinc*phii;
b1  = [b11; thetak];
C   = [zeros(nk+nh,nw);c2];
Cp  = C';
G1  = phiing*[zeros(nd,nh) gamma ud];
G2  = -phiing*phii;
H1  = [lambda pih*phiinc*gamma -ub+pih*phiinc*ud];
H2  = -pih*phiinc*phii; 
Q   = -.5*(G1'*G1 + H1'*H1);
R   = -.5*(G2'*G2 + H2'*H2);
W   = -.5*(G1'*G2 + H1'*H2);
A   = [a11 a12; [zeros(nz,nk+nh) a22]];
B   = [b1; zeros(nz,ni)];
a   = sqrt(beta)*(A-B*(R\W'));
b   = sqrt(beta)*B;
q   = Q-W*(R\W');

%---------------------------------------------------------------------
% 7. Use one of the following methods for computing F,P,Ao=A-B*F:
%    doubling algorithm, Vaughan's method, direct iteration.
%    Comment out or delete sections not to be used (after checking
%    that all algorithms provide same result).

%=======================
% 7a. Doubling Algorithm
%=======================

%v = eye(nx);
%a0= a;
%b0= b*(R\b');
%P = q;
%k0= a'*P*b/(b'*P*b+R);
%dd=1;
%it=1;
%maxit=1000;
%tol=1e-5;                         % reset tol if necessary
%while (dd>tol & it<=maxit);
%  v1 = (v+b0*P);
%  vv = v1\a0;
%  a1 = a0*vv;
%  b0 = b0+a0*(v1\(b0*a0'));
%  P  = P+a0'*P*vv;
%  k1 = a'*P*b/(b'*P*b+R);
%  dd = norm(k1-k0);
%  it = it+1;
%  a0 = a1;
%  k0 = k1;
%end
%F = k1'+R\W';

%=====================
%% 7b. Vaughan's method
%=====================

%[v,d] = eig([a+b/R*b'/a'*q  -b/R*b'/a'; -a'\q  inv(a)']);
%d     = diag(d);
%[d,index] = sort(abs(d));	 % sort on magnitude of eigenvalues
%if (~((d(nx) < 1) & (d(nx+1)>1)))
%  error('Can''t order eigenvalues, (A,B) may be uncontrollable.')
%end
%%select vectors with eigenvalues inside unit circle
%v11 = v(1:nx,index(1:nx));
%v21 = v((nx+1):(2*nx),index(1:nx));
%P   = real(v21/v11);
%F   = (R+b'*P*b)\b'*P*a+R\W';

%==================================================
%% 7c. Vaughan's method when A-B*R\W' not invertible
%==================================================

tem1 = [eye(nx),b/R*b';zeros(nx),a'];
tem2 = [a,zeros(nx);-q,eye(nx)];
[v,d]=eig(tem2,tem1);
d = diag(d);
[d,index] = sort(abs(d));        % sort on magnitude of eigenvalues
if (~((d(nx) < 1) & (d(nx+1) > 1)))
        error('Can''t order eigenvalues, (A,B) may be uncontrollable.')
end
% select vectors with eigenvalues inside unit circle
v11 = v(1:nx,index(1:nx));
v21 = v((nx+1):(2*nx),index(1:nx));
P   = real(v21/v11);
F   = (R+b'*P*b)\b'*P*a+R\W';

%=====================
%% 7d. Direct Iteration 
%=====================
% 
%P = -eye(nx);
%F0= (R+beta*B'*P*B)\(beta*B'*P*A+W');
%d=1;
%tol=1e-8;       % reset tolerance if necessary 
%while (d > tol)
%  P  = beta*A'*P*A + Q -(beta*A'*P*B+W)*F0;
%  F1 = (R+beta*B'*P*B)\(beta*B'*P*A+W');
%  d  = norm(F1-F0);
%  F0 = F1;
%end;
%F  = F1;
%---------------------------------------------------------------------

Ao = A-B*F;


%---------------------------------------------------------------------
% 8. Specify observables (y). (Use global statement on ZVAR instead of 
%    loading the dataset each time.)

%data y(t)=[total stock(t),breeding stock(t),slaughter rate(t),price(t)]
T   = length(ZVAR);
tem = grate.^([1:T]');  

y   = [ZVAR(:,1)./tem,ZVAR(:,3)./tem];
if no>2; 
  y = [y,ZVAR(:,4)+100-ZVAR(1,4)];
end;
%---------------------------------------------------------------------

T   = length(y);
dy  = y(2:T,:)-y(1:T-1,:)*D'; 
T   = T-1;



%---------------------------------------------------------------------
% 9. Specify D which relates observables to states (e.g.,y = D x + v).   

G  = [[0,1-delta+g,g,g,0,0,0,0]-F;
      F];
skl = [zeros(nk,nh),eye(nk),zeros(nk,nz)];
sc  = phiinc*(gamma*skl+[zeros(nd,nh+nk),ud]+phii*F);
mc  = 2*beta*thetah'*[eye(nh),zeros(nh,nk+nz)]*P*Ao+ ...
      pih'*([zeros(nb,nh+nk),ub]-lambda* ...
      [eye(nh),zeros(nh,nk+nz)]-pih*sc);
if no>2; G= [G;mc]; end;
%---------------------------------------------------------------------

Gbar       = G*Ao-D*G;
Rbar       = RR+G*C*C'*G';
%if we start with Sigma0 = Sigma_infty:
%[K,Sigma0]= kfilter(Ao,Gbar,C*C',Rbar,C*C'*G');
%
%if we start with Sigma0 = Ao*Sigma0*Ao'+C*C':
Sigma0     = dlyap(Ao,C*C');
Sigma      = Sigma0;
xt(1,:)    = x0';
innov(1,:) = dy(1,:)-x0'*Gbar';
L          = 0;
for i=1:T-1;
  Omega        = Rbar+Gbar*Sigma*Gbar';
  Omegai       = inv(Omega);
  K            = (Ao*Sigma*Gbar'+C*C'*G')*Omegai;
  Sigma        = Ao*Sigma*Ao'+C*C'-K*Omega*K';
  xt(i+1,:)    = xt(i,:)*Ao'+innov(i,:)*K';
  innov(i+1,:) = dy(i+1,:)-xt(i+1,:)*Gbar';
  L            = L + log(det(Omega))+innov(i,:)*Omegai*innov(i,:)';
end;
L              = L + log(det(Omega))+innov(T,:)*Omegai*innov(T,:)';

%-----------------------------------------------------------------------
% 10. Set jacobian resulting from observations being a function of Theta,
%     e.g., if n elements of y have a geometric trend removed then add
%     n*log(grate)*T*(T+1) to L.

if gratein;
  L = L + 3*log(grate)*T*(T+1);
end;
%-----------------------------------------------------------------------

L=L/Lsize;

if nargout > 1;

  %-------------------------------------------------------------------
  % 11. Specify grada2,gradc2,graddh,graddk,gradg,gradl,gradpc,gradpg,
  %             gradpi,gradph,gradth,gradtk,gradub,gradud,gradx0,
  %             gradD,gradRR  
  %     which are gradients for
  %             a22,c2,deltah,deltak,gamma,lambda,phic,phig,phii,pih,
  %             thetah,thetak,ub,ud,x0,D,RR,
  %     e.g., gradA(:,i) = vec(dA/dTheta(i)).
  %    
  p      = length(ind);
  grada2 = zeros(nz*nz,p);    grada2(6,12)=1; grada2(11,13)=1; grada2(16,14)=1; 
  gradc2 = zeros(nz*nw,p);    gradc2(2,15)=1; gradc2(7,16)=1;  gradc2(12,17)=1; 
  graddh = zeros(nh*nh,p);   
  graddk = zeros(nk*nk,p);    graddk(1,4)=-1; graddk(7,11)=1;
  gradg  = zeros(nd*nk,p);    gradg(2,4)=-f5; gradg(2,9)=1-delta; 
                              gradg(3,7)=1;   gradg(9,8)=1;    gradg(12,9)=g;
                              gradg(12,11)=f5;
  gradl  = zeros(nb*nh,p);    
  gradpc = zeros(nd*nc,p);    gradpc(2,9)=1;  gradpc(5,10)=-1;
  gradpg = zeros(nd*ng,p); 
  gradpi = zeros(nd*ni,p); 
  gradph = zeros(nb*nc,p);    gradph(1,2)=-0.5*alpha1^(-1.5);
  gradth = zeros(nh*nc,p);    
  gradtk = zeros(nk*ni,p);    
  gradub = zeros(nb*nz,p);    gradub(1,1)=pih;
                              gradub(1,2)=-0.5*alpha1^(-1.5)*alpha0;
                              gradub(4,2)=-0.5*alpha1^(-1.5);
  gradud = zeros(nd*nz,p);    gradud(2,9)=-f6*muh/f5;          
                              gradud(2,18)=f6;
                              gradud(7,9)=-f6/f5;         
                              gradud(3,5)= f2*muh/gamma0;    
                              gradud(3,7)=-f2*muh/f1;          
                              gradud(3,11)=f2*muh/g;
                              gradud(3,18)=f2;
                              gradud(8,5)= f2/gamma0;         
                              gradud(8,7)=-f2/f1;         
                              gradud(8,11)=f2/g;
                              gradud(4,6)= f4*muh/gamma1;    
                              gradud(4,8)=-f4*muh/f3;          
                              gradud(4,11)=f4*muh/g;
                              gradud(4,18)=f4;
                              gradud(9,6)= f4/gamma1;         
                              gradud(9,8)=-f4/f3;         
                              gradud(9,11)=f4/g;
                              gradud(5,10)=-f8*mum/f7;          
                              gradud(5,19)=f8;
                              gradud(15,10)=-f8/f7;         
  gradx0 = zeros(nx,p);
  gradD  = zeros(no*no,p);
  gradRR  = zeros(no*no,p);   gradRR(1,21)=1; gradRR(no+2,22)=1; 
                              if no>2; gradRR(2*no+3,23)=1; end;
  %-------------------------------------------------------------------

  grada2 = grada2(:,ind);
  gradc2 = gradc2(:,ind);
  graddh = graddh(:,ind);
  graddk = graddk(:,ind);
  gradg  = gradg(:,ind);
  gradl  = gradl(:,ind);
  gradpc = gradpc(:,ind);
  gradpg = gradpg(:,ind);
  gradpi = gradpi(:,ind);
  gradph = gradph(:,ind);
  gradth = gradth(:,ind);
  gradtk = gradtk(:,ind);
  gradub = gradub(:,ind);
  gradud = gradud(:,ind);
  gradx0 = gradx0(:,ind);
  gradD  = gradD(:,ind);
  gradRR = gradRR(:,ind);
  gradp  = [gradpc;gradpg];
  p      = sum(ind);

  tem1   = zeros(nx,ni);
  nxi    = nx*ni;
  tem1(:)= [1:nxi]';
  tem2   = tem1';
  tem    = zeros(nxi*nxi,1);
  tem((tem2(:)-1)*nxi+tem1(:)) = ones(nxi,1);
  Kxi    = zeros(nxi);
  Kxi(:) = tem;
  tem1   = zeros(no,no);
  noo    = no*no;
  tem1(:)= [1:noo]';
  tem2   = tem1';
  tem    = zeros(noo*noo,1);
  tem((tem2(:)-1)*noo+tem1(:)) = ones(noo,1);
  Koo    = zeros(noo);
  Koo(:) = tem;
  tem1   = zeros(nx,no);
  nxo    = nx*no;
  tem1(:)= [1:nxo]';
  tem2   = tem1';
  tem    = zeros(nxo*nxo,1);
  tem((tem2(:)-1)*nxo+tem1(:)) = ones(nxo,1);
  Kxo    = zeros(nxo);
  Kxo(:) = tem;
  tem1   = zeros(nw,nx);
  nxw    = nx*nw;
  tem1(:)= [1:nxw]';
  tem2   = tem1';
  tem    = zeros(nxw*nxw,1);
  tem((tem2(:)-1)*nxw+tem1(:)) = ones(nxw,1);
  Kwx    = zeros(nxw);
  Kwx(:) = tem;
  tem1   = zeros(nx);
  tem1   = zeros(ni,nx);
  tem1(:)= [1:nxi]';
  tem2   = tem1';
  tem    = zeros(nxi*nxi,1);
  tem((tem2(:)-1)*nxi+tem1(:)) = ones(nxi,1);
  Kix    = zeros(nxi);
  Kix(:) = tem;
  tem1   = zeros(nx);
  nxx    = nx*nx;
  tem1(:)= [1:nxx]';
  tem2   = tem1';
  tem    = zeros(nxx*nxx,1);
  tem((tem2(:)-1)*nxx+tem1(:)) = ones(nxx,1);
  Kxx    = zeros(nxx);
  Kxx(:) = tem;
  tem1   = zeros(ni);
  nii    = ni*ni;
  tem1(:)= [1:nii]';
  tem2   = tem1';
  tem    = zeros(nii*nii,1);
  tem((tem2(:)-1)*nii+tem1(:)) = ones(nii,1);
  Kii    = zeros(nii);
  Kii(:) = tem;
  tem1   = zeros(nh,nd);
  nhd    = nh*nd;
  tem1(:)= [1:nhd]';
  tem2   = tem1';
  tem    = zeros(nhd*nhd,1);
  tem((tem2(:)-1)*nhd+tem1(:)) = ones(nhd,1);
  Khd    = zeros(nhd);
  Khd(:) = tem;
  tem1   = zeros(nz,nw);
  nzw    = nz*nw;
  tem1(:)= [1:nzw]';
  tem2   = tem1';
  tem    = zeros(nzw*nzw,1);
  tem((tem2(:)-1)*nzw+tem1(:)) = ones(nzw,1);
  Kzw    = zeros(nzw);
  Kzw(:) = tem;
  tem1   = zeros(nz,nw);
  nzw    = nz*nw;
  tem1(:)= [1:nzw]';
  tem2   = tem1';
  tem    = zeros(nzw*nzw,1);
  tem((tem2(:)-1)*nzw+tem1(:)) = ones(nzw,1);
  Kzw    = zeros(nzw);
  Kzw(:) = tem;
  tem1   = zeros(nk,ni);
  nki    = nk*ni;
  tem1(:)= [1:nki]';
  tem2   = tem1';
  tem    = zeros(nki*nki,1);
  tem((tem2(:)-1)*nki+tem1(:)) = ones(nki,1);
  Kki    = zeros(nki);
  Kki(:) = tem;
  tem1   = zeros(ng,nx);
  ngx    = ng*nx;
  tem1(:)= [1:ngx]';
  tem2   = tem1';
  tem    = zeros(ngx*ngx,1);
  tem((tem2(:)-1)*ngx+tem1(:)) = ones(ngx,1);
  Kgx    = zeros(ngx);
  Kgx(:) = tem;
  tem1   = zeros(nb,nx);
  nbx    = nb*nx;
  tem1(:)= [1:nbx]';
  tem2   = tem1';
  tem    = zeros(nbx*nbx,1);
  tem((tem2(:)-1)*nbx+tem1(:)) = ones(nbx,1);
  Kbx    = zeros(nbx);
  Kbx(:) = tem;
  tem1   = zeros(nd,nx);
  ndx    = nd*nx;
  tem1(:)= [1:ndx]';
  tem2   = tem1';
  tem    = zeros(ndx*ndx,1);
  tem((tem2(:)-1)*ndx+tem1(:)) = ones(ndx,1);
  Kdx    = zeros(ndx);
  Kdx(:) = tem;
  tem1   = zeros(ng,ni);
  ngi    = ng*ni;
  tem1(:)= [1:ngi]';
  tem2   = tem1';
  tem    = zeros(ngi*ngi,1);
  tem((tem2(:)-1)*ngi+tem1(:)) = ones(ngi,1);
  Kgi    = zeros(ngi);
  Kgi(:) = tem;
  tem1   = zeros(nb,ni);
  nbi    = nb*ni;
  tem1(:)= [1:nbi]';
  tem2   = tem1';
  tem    = zeros(nbi*nbi,1);
  tem((tem2(:)-1)*nbi+tem1(:)) = ones(nbi,1);
  Kbi    = zeros(nbi);
  Kbi(:) = tem;
  tem1   = zeros(nc,nh);
  nch    = nc*nh;
  tem1(:)= [1:nch]';
  tem2   = tem1';
  tem    = zeros(nch*nch,1);
  tem((tem2(:)-1)*nch+tem1(:)) = ones(nch,1);
  Kch    = zeros(nch);
  Kch(:) = tem;
  tem1   = zeros(nc,nb);
  ncb    = nc*nb;
  tem1(:)= [1:ncb]';
  tem2   = tem1';
  tem    = zeros(ncb*ncb,1);
  tem((tem2(:)-1)*ncb+tem1(:)) = ones(ncb,1);
  Kcb    = zeros(ncb);
  Kcb(:) = tem;
  tem = kron(phiinc',eye(nh))*gradth-kron(phiin',thetah*phiinc)*gradp;
  grada12 = kron(gamma',eye(nh))*tem+kron(eye(nk),thetah*phiinc)*gradg;
  grada13 = kron(ud',eye(nh))*tem+kron(eye(nz),thetah*phiinc)*gradud;
  gradBp  = [-kron(thetah*phiinc,eye(ni))*Kii*gradpi-kron(eye(nh),phii')* ...
               Khd*tem; Kki*gradtk; zeros(nz*ni,p)];
  gradCp  = [zeros((nk+nh)*nw,p);Kzw*gradc2];
  gradC   = Kwx*gradCp;
  tem1 = zeros(nx);
  tem2 = zeros(nx);
  tem3 = zeros(nh);
  tem4 = zeros(nh,nk);
  tem5 = zeros(nh,nz);
  tem6 = zeros(nk);
  tem7 = zeros(nz);
  for i=1:p;
    tem3(:) = graddh(:,i);
    tem4(:) = grada12(:,i);
    tem5(:) = grada13(:,i);
    tem6(:) = graddk(:,i);
    tem7(:) = grada2(:,i);
    tem1(1:nh,1:nh)             = tem3;
    tem1(1:nh,nh+1:nh+nk)       = tem4;
    tem1(1:nh,nh+nk+1:nx)       = tem5;
    tem1(nh+1:nh+nk,nh+1:nh+nk) = tem6;
    tem1(nh+nk+1:nx,nh+nk+1:nx) = tem7;
    gradA(:,i)  = vec(tem1);
  end;
  gradB = Kix*gradBp;
  gradG1=-kron( [zeros(nd,nh),gamma,ud]'*phiin',phiing)*gradp+ ...
          kron(eye(nx),phiing)*[zeros(nd*nh,p);gradg;gradud];
  gradG2= kron(phii'*phiin',phiing)*gradp-kron(eye(ni),phiing)* ...
          gradpi;
  gradH1=[gradl;
          kron(gamma'*phiinc',eye(nb))*gradph-kron(gamma'*phiin', ...
             pih*phiinc)*gradp+kron(eye(nk),pih*phiinc)*gradg;
          kron(ud'*phiinc',eye(nb))*gradph-kron(ud'*phiin', ...
             pih*phiinc)*gradp-gradub+kron(eye(nz),pih*phiinc)*gradud];
  gradH2=-kron(phii'*phiinc',eye(nb))*gradph+kron(phii'*phiin', ...
          pih*phiinc)*gradp-kron(eye(ni),pih*phiinc)*gradpi;
  gradQ=-.5*( (kron(G1',eye(nx))*Kgx+kron(eye(nx),G1'))*gradG1+ ...
              (kron(H1',eye(nx))*Kbx+kron(eye(nx),H1'))*gradH1 );
  gradR=-.5*( (kron(G2',eye(ni))*Kgi+kron(eye(ni),G2'))*gradG2+ ...
              (kron(H2',eye(ni))*Kbi+kron(eye(nc),H2'))*gradH2 );
  gradW=-.5*(  kron(G2',eye(nx))*Kgx*gradG1+kron(eye(ni),G1')*gradG2+ ...
               kron(H2',eye(nx))*Kbx*gradH1+kron(eye(ni),H1')*gradH2 );
  ri=inv(R+beta*B'*P*B);
  ai=inv(eye(nx*nx)-beta*kron(Ao',Ao'));
  gradP = ai*(gradQ+(beta*kron(eye(nx),Ao'*P)+beta*kron(Ao'*P,eye(nx)) ...
         *Kxx)*gradA-beta*kron(F',Ao'*P)*gradB-beta*kron(Ao'*P,F')*gradBp);
  gradP = gradP+ai*(kron(F',F')*gradR-(kron(F',eye(nx))+kron(eye(nx),F')* ...
          Kxi)*gradW);
  gradF = beta*kron(eye(nx),ri*B'*P)*gradA-beta*kron(F',ri*B'*P)* ...
          gradB+beta*kron(Ao'*P,ri)*gradBp-kron(F',ri)*gradR+ ...
          kron(eye(nx),ri)*Kxi*gradW+ beta*kron(Ao',ri*B')*gradP;
  if betain;
    gradF(:,betain)=gradF(:,betain)-beta*kron(Ao',ri*B')*gradP(:,betain) ...
                   +vec(ri*B'*P*Ao);
    gradP(:,betain)=gradP(:,betain)+ai*vec(Ao'*P*Ao);
    gradF(:,betain)=gradF(:,betain)+beta*kron(Ao',ri*B')*gradP(:,betain);
  end;
  gradAo=gradA-kron(F',eye(nx))*gradB-kron(eye(nx),B)*gradF;

  %-------------------------------------------------------------------
  % 12. Specify gradGp where Gp = G'.   In doing so, you may find
  %     the following formulas useful:  
  %
  %     vec(A*B*C) = kron(C',A)*vec(B)
  %     gradA(:,i) = vec(dA/dTheta(i)).

  gradGp=zeros(16,p);
  if ind(4); 
    gradGp(2,sum(ind(1:4)))=-1; 
  end;
  if ind(11);
    gradGp([2,3,4],sum(ind(1:11)))=ones(3,1);
  end;
  gradGp=gradGp+kron([-1;1],eye(nx))*Kxi*gradF;
  tem   =phiin*(gamma*skl+[zeros(nd,nh+nk),ud]+phii*F);
  gradsc=-kron(tem',phiinc)*gradp+kron(skl',phiinc)*gradg+ ...
         kron(eye(nx),phiinc)*[zeros(nd*nh+nd*nk,p);gradud]+ ...
         kron(F',phiinc)*gradpi+kron(eye(nx),phiinc*phii)*gradF;
  tem   =[eye(nh),zeros(nh,nk+nz)];
  gradmc=2*beta*(kron(Ao'*P'*tem',eye(nc))*Kch*gradth+ ...
         kron(Ao',thetah'*tem)*gradP+kron(eye(nx),thetah'*tem*P)*gradAo);
  tem   =[zeros(nb,nh+nk),ub]-lambda*[eye(nh),zeros(nh,nk+nz)]-pih*sc;
  gradmc=gradmc+kron(tem',eye(nc))*Kcb*gradph+kron(eye(nx),pih')* ...
         [zeros(nb*nh+nb*nk,p);gradub]- ...
         kron([eye(nh),zeros(nh,nk+nz)]',eye(nb))*gradl;
  gradmc=gradmc-kron(sc',pih')*gradph-kron(eye(nx),pih'*pih)*gradsc;
  if no>2;
    gradGp(17:24,:)=gradmc;
    if betain;
      tem   =[eye(nh),zeros(nh,nk+nz)];
      gradGp(17:24,betain) = gradGp(17:24,betain)+2*vec(thetah'*tem*P*Ao);
    end;
  end;
  gradG = Kxo*gradGp;
  
  %-------------------------------------------------------------------

  dAo        = zeros(nx);
  dG         = zeros(no,nx); 
  dC         = zeros(nx,nw); 
  dD         = zeros(no); 
  dR         = zeros(no); 
  dx         = zeros(nx,1); 
  dS         = zeros(nx); 
  dK         = zeros(nx,no); 
  %if we start from Sigma0 = Sigma_infty, use the following lines: 
  %AA         = Ao-K*Gbar;
  %tem2       = kron(AA*Sigma,eye(nx)-K*G)*gradAo ...
  %             +kron(C-K*G*C,eye(nx)-K*G)*gradC ...
  %             -kron(Sigma,K)*gradG+kron(AA*Sigma,K*D)*gradG ...
  %             +kron(AA*Sigma*G',K)*gradD ...
  %             +0.5*kron(K,K)*gradRR;
  %gradS0     = (eye(nx*nx)-kron(AA,AA))\(tem2+Kxx*tem2);
  % 
  %if we start from Sigma0 = Ao * Sigma0 Ao' + C*C'
  for j=1:p;
    dAo(:)      = gradAo(:,j); 
    dC(:)       = gradC(:,j); 
    dS          = dlyap(Ao,dAo*Sigma0*Ao'+Ao*Sigma0*dAo'+dC*C'+C*dC');
    gradS0(:,j) = dS(:); 
  end;
  %
  Sigma      = Sigma0;
  gradx      = gradx0;
  gradS      = gradS0;
  sum1       = zeros(p,1);
  sum2       = zeros(p,1);
  for i=1:T;
    Omega     = Rbar+Gbar*Sigma*Gbar';
    Omegai    = inv(Omega);
    K         = (Ao*Sigma*Gbar'+C*C'*G')*Omegai;
    AA        = Ao-K*Gbar;
    M         = Omegai-Omegai*innov(i,:)'*innov(i,:)*Omegai;
    tem1      = Ao*Sigma*Ao'+C*C'-K*Omega*K';
    gradK     = kron(Omegai*G*C,eye(nx)-K*G)*gradC ...
                +kron(Omegai*G,C-K*G*C)*gradCp ...
                +kron(Omegai*Gbar*Sigma,eye(nx)-K*G)*gradAo ...
                +kron(Omegai*G,AA*Sigma)*Kxx*gradAo ...
                +kron(Omegai*Gbar,AA)*gradS ...
                +kron(Omegai*Gbar*Sigma*G',K)*gradD ...
                -kron(Omegai,AA*Sigma*G')*Koo*gradD ...
                -kron(K',K)*gradG ...
                +kron(Omegai*Gbar*Sigma,K*D)*gradG ...
                -kron(Omegai*D,AA*Sigma)*gradGp ...
                +kron(Omegai,tem1)*gradGp ...
                -kron(Omegai,K)*gradRR;
    xuo       = xt(i,:)'*innov(i,:)*Omegai;
    yuo       = y(i,:)'*innov(i,:)*Omegai;
    for j=1:p;
      dK(:)   = gradK(:,j);
      dx(:)   = gradx(:,j);
      dS(:)   = gradS(:,j);
      dAo(:)  = gradAo(:,j); 
      dG(:)   = gradG(:,j); 
      dC(:)   = gradC(:,j); 
      dR(:)   = gradRR(:,j); 
      dD(:)   = gradD(:,j); 
      sum1(j) = sum1(j)+2*trace(dAo*Sigma*Gbar'*M*G)+2*trace(dC*C'*G'*M*G) ...
                       +2*trace(dG*(Ao*Sigma*Gbar'*M-Sigma*Gbar'*M*D ...
                               +C*C'*G'*M))-2*trace(dD*G*Sigma*Gbar'*M) ...
                       +trace(dR*M)+trace(dS*Gbar'*M*Gbar);
      sum2(j) = sum2(j)-2*trace(dAo*xuo*G)-2*trace(dG*(Ao*xuo-xuo*D)) ...
                       -2*trace(dD*(yuo-G*xuo)) ...
                       -2*trace(dx*innov(i,:)*Omegai*Gbar);  % plus z terms
    end;
    gradx     = AA*gradx +kron(xt(i,:),eye(nx)-K*G)*gradAo ...
                +kron(innov(i,:),eye(nx))*gradK-kron(xt(i,:)*Ao',K)*gradG ...
                +kron(xt(i,:),K*D)*gradG+kron(xt(i,:)*G',K)*gradD; % plus z
    tem2      = kron(AA*Sigma,eye(nx)-K*G)*gradAo ...
                +kron(C-K*G*C,eye(nx)-K*G)*gradC ...
                -kron(tem1,K)*gradG+kron(AA*Sigma,K*D)*gradG ...
                +kron(AA*Sigma*G',K)*gradD ...
                +0.5*kron(K,K)*gradRR;
    gradS     = kron(AA,AA)*gradS+tem2+Kxx*tem2;
    Sigma     = tem1;
  end;
  dL   = sum1+sum2; 
  dL   = diag(Tsize)*dL/Lsize;
end;

