function [L,gL,dL]=hsmle(Theta);
% 
% The model is a version of Hansen and Sargent (1987).
%          
%          max E sum{ beta^t (log(c(t)) -a1*(n1(t)-n2(t)) -a2*n2(t)
%          s.t.
%              c(t) + i(t) = xi(t) k^theta {h1*(n1(t)-d/2*(n1(t)-n1(t-1))
%                            ^2)^(1-theta)+h2*n2(t)^(1-theta)} 
%              k(t+1)=(1-delta)/lambda k(t) + (1/lambda) i(t)
%              xi(t+1)=rho0+rho1*xi(t)+rho2*xi(t-1)+epsilon(t+1)
%
%  States:       [1 xi(t) xi(t-1) k(t) n1(t-1)]'
%  Controls:     [n1(t) n2(t) i(t)]'
%  Vars in u(.): [xi(t) k(t) n1(t-1) n1(t) n2(t) i(t)]'
%

% default values of the parameters
d=3.08; 
h1=.432; 
h2=.0639; 
theta=.365;
aa=3.62; 
delta=.0241; 
lambda=1.005186;
rho0=5.20; 
rho1=.7; 
rho2=.277;
sigma=1.64;
beta=.995; 
varm=[1000;.001;.001;1000;1000];
coefm=zeros(5,1);

% parameter vector
param=[d h1 h2 theta aa lambda rho0 rho1 rho2 delta sigma beta]';
param=[param;varm;coefm];

% Adjust by Scale factors 
Thet=Theta;
Theta=Theta(:);
Scale=[ ..
     3.795512916323920e+00
     2.045197208086051e-01
     3.331391289261171e-01
     3.805104703385082e-01
     4.904231484463731e+00
     1.007100000000000e+00
     1.027849608978465e-01
     9.698872701312443e-01
     2.966414894510069e-02
     2.331192393718028e-02
     2.237302236913262e+00
     9.932425200471350e-01
     7.741876614607470e+01
     7.774488719381233e-05
     1.579043861715189e-04
     1.302730396789175e+03
     2.697016189716473e+03
     1.419938856306719e-06
     8.501089002209466e-06
     1.391233398951500e-05
     1.198184489497086e-05
     1.407375015639111e-05];
fsize=1100;

Scale=ones(22,1); fsize=1;
Theta=diag(Scale)*Theta;

lb=[-10;1e-8;1e-8;.2;.01; .99;.0001; .6;-.5;.02;.0075; .96;    1;1e-8;
    1e-8;    1;    1;-.99;-.99;-.99;-.99;-.99];
ub=[ 10;  .8;  .5;.8; 10;1.0073;   10;1.1; .3;.03;   10;.999;10000;  .1;
      .1;10000;10000; .99; .99; .99; .99; .99];
if any(lb>Theta) | any(ub<Theta) | Theta(2)+Theta(3)>1 | Theta(8)+Theta(9)>1;
 % disp('WARNING: in restricted zone with:')
 % disp(Theta)
  L=1e+10; dL=1e+10*ones(Theta);
  return
end;

% parameters to be estimated
ind=[1:22];
d=Theta(1); h1=Theta(2); h2=Theta(3); theta=Theta(4); aa=Theta(5);
lambda=Theta(6);    rho0=Theta(7);     rho1=Theta(8);    rho2=Theta(9); 
delta=Theta(10);    sigma=Theta(11);   beta=Theta(12);   varm=Theta(13:17);
coefm=Theta(18:22);
param(ind)=Theta;
np=length(ind);
betain=12;

% A,B,D,E,Sigma,ct
a1=-aa*log(1-h1);
a2=-aa*log(1-h1-h2);
A=[1     0    0   0      0;
   rho0 rho1 rho2 0      0;
   0     1    0   0      0;
   0     0    0 (1-delta)/lambda  0;
   0     0    0   0      0];
B=[0 0 0;0 0 0;0 0 0;0 0 1/lambda;1 0 0];
D=[0 1 0 0 0;
   0 0 0 1 0;
   0 0 0 0 1;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0];
E=[0 0 0;
   0 0 0;
   0 0 0;
   1 0 0;
   0 1 0;
   0 0 1];
Sigma=zeros(5);
Sigma(2,2)=sigma^2;
ct=1;


% steady states
lbd=(lambda-beta*(1-delta))/(beta*theta);
ti=1/theta;
tt=1-ti;
x(1,1)=rho0/(1-rho1-rho2);
x(2,1)=((h1^ti*a1^tt+h2^ti*(a2-a1)^tt)/lbd)^(-1/tt)*(1-theta)*x(1)^ ..
       (1/(1-theta))/(lbd-lambda+1-delta);
x(3,1)=(a1*(lbd-lambda+1-delta)*x(2)^(1-theta)/(x(1)*h1*(1-theta)))^(-ti);
x(4,1)=x(3);
x(5,1)=(h2*a1/(h1*(a2-a1)))^ti*x(3);
x(6,1)=(lambda-1+delta)*x(2);
X=[1 x(1) x(1) x(2) x(3)]';
u=[x(4) x(5) x(6)]';
ns=length(X);
nc=length(u);
nx=length(x);


% data Y(t)=[k(t),n1(t),n2(t),i(t),c(t)]';
X0=X;
%load /home/erm/data/hsdata3
T=length(zxvar);
tem=lambda.^([1:T]');
%Y=[zxvar(:,1)./tem,zxvar(:,2),zxvar(:,2).*(zxvar(:,3)-13.045*80*h1)./ ..
%   (13.045*80*h2),zxvar(:,5)./tem,zxvar(:,4)./tem];
Y=[zxvar(:,1)./tem,zxvar(:,2),zxvar(:,3),zxvar(:,5)./tem,zxvar(:,4)./tem];
DY=Y(2:T,:)-Y(1:T-1,:)*diag(coefm);
T=T-1;
Omega=diag(varm);
G=diag(coefm);
% see C below

% retrieve b=dr(x,theta)/x 
%          q=d^2r(x,theta)/dx^2
% plus     cub=d^3r(x,theta)/dx^3
%          du=dr(x,theta)/dtheta
%          db=db/dtheta 
%          dq=dq/dtheta   if derivatives required
if nargout > 2;
  [ux,b,q,cub,du,db,dq]=hsu4(x,param(1:5));
  du=[du(:);zeros(np-5,1)];
  db=[db,zeros(6,np-5)];
  dq=[dq,zeros(36,np-5)];
else;
  [ux,b,q]=hsu4(x,param(1:5));
end;
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 at Theta equal:')
  Theta
  return
end;

% compute Riccati matrix and decision rules 
[Fn,P]=double(An',Bn',Qn,R);
Fn=Fn';
Aon=An-Bn*Fn;

% construct matrices for original problem
F=Fn+(R\W');
Ao=A-B*F;

% compute innovations for likelihood function
[tem1,tem2]=hsf4(x,param(1:4));
C=[0,0,0,1,0;
   -F;
   tem2'*(D-E*F)+[tem1-tem2'*x,zeros(1,ns-1)]];


Cbar=C*Ao-G*C;
Obar=Omega+C*Sigma*C';
[K,S]=kfilter(Ao,Cbar,Sigma,Obar,Sigma*C');
Sigmaz=Obar+Cbar*S*Cbar';
Sigmazi=inv(Sigmaz);
t0=3;
Xt(t0,:)=X0';
innov(t0,:)=DY(t0,:)-X0'*Cbar';
sum1=innov(t0,:)'*innov(t0,:);
sum2=Xt(t0,:)'*innov(t0,:);
sum6=Y(t0,:)'*innov(t0,:);
for i=t0+1:T;
  Xt(i,:)=Xt(i-1,:)*Ao'+innov(i-1,:)*K';
  innov(i,:)=DY(i,:)-Xt(i,:)*Cbar';
  sum1=sum1+innov(i,:)'*innov(i,:);
  sum2=sum2+Xt(i,:)'*innov(i,:);
  sum6=sum6+Y(i,:)'*innov(i,:);
end;
sum1=sum1/(T-t0+1);
sum2=sum2/(T-t0+1);
sum6=sum6/(T-t0+1);

junkon=0;
if junkon;
tem=Xt*Cbar';
XX=Cbar*X;
for i=1:5;
  plot([DY(t0:T,i),tem(t0:T,i),ones(T-t0+1,1)*XX(i)]); pause(2)
end;
for i=1:5;
  plot([Xt(t0:T,i),ones(T-t0+1,1)*X(i)]); pause(2)
end;
end;

L= (T-t0+1)*(log(det(Sigmaz))+trace(Sigmazi*sum1))+  ..
   1.5*log(lambda)*(T-t0+1)*(T-t0+2);
L=L/fsize;
gL(1)=h1+h2-1;
gL(2)=rho1+rho2-1;

del=diag(abs(Theta)*1e-7);
[fp1,fp2,fp3,fp4,fp5]=hs2QRW(param+del(:,6));
[fm1,fm2,fm3,fm4,fm5]=hs2QRW(param-del(:,6));
dGii=(fp1-fm1)/(2*del(6,6));
G2=(T-t0+1)*trace(dGii*Sigmazi)/fsize
dC=(fp2-fm2)/(2*del(6,6));
dAo=(fp3-fm3)/(2*del(6,6));
dDY=(fp4-fm4)/(2*del(6,6));
dXt=(fp5-fm5)/(2*del(6,6));

G2a=-2*(T-t0+1)*trace(dC*(Ao*sum2*Sigmazi-sum2*Sigmazi*G))
G2b=-2*(T-t0+1)*trace(dAo*(sum2*Sigmazi*C))
%G2c=2*trace(dY(t0+1:T,:)'*innov(t0:T-1,:)*Sigmazi)
%G2d=-2*trace(dY(t0:T-1,:)'*innov(t0:T-1,:)*Sigmazi*G)
G2c=2*trace(dDY(t0:T,:)'*innov(t0:T,:)*Sigmazi)
G2d=0;
G2e=-2*trace(dXt(t0:T,:)'*innov(t0:T,:)*Sigmazi*Cbar)
[G2,G2a+G2b+G2c+G2d+G2e] 


%dl=(fp3-fm3)/(2*del(6,6))
%dSz=(fp1-fm1)/(2*del(6,6));
%M1=Sigmazi-Sigmazi*sum1*Sigmazi;
%G1=(T-t0+1)*trace(dSz*M1)/fsize 
%G3=1.5*(T-t0+1)*(T-t0+2)/(fsize*lambda)
%[dl,G1+G2+G3]


if nargout > 2;
 
  % user-defined
  gradA=zeros(25,np);
  gradA(2,7)=1;
  gradA(7,8)=1;
  gradA(12,9)=1;
  gradA(19,6)=-(1-delta)/(lambda*lambda);
  gradA(19,10)=-1/lambda;
  gradB=zeros(15,np);
  gradB(14,6)=-1/(lambda*lambda);
  gradS=zeros(25,np);
  gradS(7,11)=2*sigma;
  gradO=zeros(25,np);
  gradO(1,13)=1; 
  gradO(7,14)=1; 
  gradO(13,15)=1; 
  gradO(19,16)=1; 
  gradO(25,17)=1; 
  gradG(1,18)=1; 
  gradG(7,19)=1; 
  gradG(13,20)=1; 
  gradG(19,21)=1; 
  gradG(25,22)=1; 
  gradx=zeros(6,np);
  tem1=grad('hsgradx',param,abs(Theta([2:12]))*1e-5,ind([2:12]));
  gradx(:,2:12)=tem1;
  gradx(1,[6,7:9,12])=[0,x(1)/rho0,x(1)/(1-rho1-rho2),x(1)/(1-rho1-rho2),0];
  gradx(2,[7:9])=x(2)/((1-theta)*x(1))*gradx(1,[7:9]);
  gradx(6,[7:9])=(lambda-1+delta)*gradx(2,[7:9]);
  gradx(2,6)=1/tt*x(2)/lbd/(beta*theta)-x(2)*(1/(beta*theta)-1)/  ..
    (lbd-lambda+1-delta);
  tem2=-(1-delta+lbd*theta)/(beta*theta);    
  gradx(2,12)=1/tt*x(2)*tem2/lbd-x(2)*tem2/(lbd-lambda+1-delta);
  gradx(3,6)=-ti*x(3)/(lbd-lambda+1-delta)*(1/(beta*theta)-1)-ti*(1-theta)* ..
    x(3)/x(2)*gradx(2,6);
  gradx(3,12)=-ti*x(3)/(lbd-lambda+1-delta)*tem2-ti*(1-theta)* ..
    x(3)/x(2)*gradx(2,12);
  gradx(4,[6,12])=gradx(3,[6,12]);
  gradx(5,[6,12])=x(5)/x(3)*gradx(3,[6,12]);
  gradx(6,6)=x(2)+(lambda-1+delta)*gradx(2,6);
  gradx(6,12)=(lambda-1+delta)*gradx(2,12);
  gradX=zeros(5,np);
  gradX(2:5,:)=gradx([1,1:3],:);
  V=eye(5); V(2,2)=0; V(3,3)=0;
  sum8=(t0+1)*V*Y(t0+1,:)'*innov(t0,:);
  sum9=t0*V*Y(t0,:)'*innov(t0,:);
  for i=t0+1:T;
    sum8=sum8+(i+1)*V*Y(i+1,:)'*innov(i,:);
    sum9=sum9+i*V*Y(i,:)'*innov(i,:);
  end; 
  sum8=-sum8/(lambda*(T-t0+1));
  sum9=-sum9/(lambda*(T-t0+1));


  % construct gradb,gradq
  gradb=q*gradx+db;
  gradq=cub*gradx+dq;

  % construct gradQ, etc.
  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;
  tem1=zeros(nc,ns);
  tem1(:)=[1:ns*nc]';
  tem2=tem1'; 
  tem2=tem2(:);
  tem1=tem1(:);
  Kkn=zeros(nc,ns);
  for i=1:ns*nc;
    Kkn(tem1(i),tem2(i))=1;
  end;
  tem=(ns+1)*(ct-1);
  gradQ=[zeros(tem,np);
         du'-x'*gradb+x'*q*gradx+.5*kron(x',x')*gradq;
         zeros((ns*ns)-tem-1,np)];
  tem= [zeros(ns*(ct-1),np);
        .5*D'*gradb-.5*D'*q*gradx-.5*kron(x',D')*gradq;
        zeros(ns*(ns-ct),np)];
  gradQ=gradQ + tem + Knn*tem;
  gradQ=gradQ + .5*kron(D',D')*gradq;
  gradR=kron(E',E')*gradq/2;
  gradWp=[zeros(nc*(ct-1),np);
          (E'*gradb-E'*q*gradx-kron(x',E')*gradq)/2;
          zeros((ns-ct)*nc,np)] +kron(D',E')*gradq/2; 
  gradW=Kkn*gradWp;

  ZO=inv(R+beta*B'*P*B);
  ZB=beta*kron(Ao',ZO*B')*inv(eye(ns*ns)-beta*kron(Ao',Ao'));
  Z1=beta*kron(eye(ns),ZO*B'*P)+ZB*(beta*kron(eye(ns),Ao'*P)+ ..
      beta*kron(Ao'*P,eye(ns))*Knn); 
  Z2=-beta*kron(F',ZO*B'*P)+beta*kron(Ao'*P,ZO)*Knk-ZB*(beta* ..
      kron(F',Ao'*P)+beta*kron(Ao'*P,F')*Knk);
  Z3=ZB;
  Z4=-kron(F',ZO)+ZB*kron(F',F');
  Z5=kron(eye(ns),ZO)*Knk-ZB*(kron(F',eye(ns))+kron(eye(ns),F')*Knk);
  Z6=zeros(ns*nc,np);
  if betain; Z6(:,betain)=vec(ZO*B'*P*Ao)+ZB*vec(Ao'*P*Ao); end;
  gradF=Z1*gradA+ Z2*gradB+ Z3*gradQ+ Z4*gradR +Z5*gradW+ Z6;
  gradAo=gradA-kron(F',eye(ns))*gradB-kron(eye(ns),B)*gradF;

  % user-defined 
  gradFp=Kkn*gradF;
  gradCp=zeros(25,np);
  gradCp(6:5+ns*nc,:)=-gradFp;
  [f,fj,fh,f,df,dfj]=hsf4(x,param(1:4));
  dfj=[dfj,zeros(6,np-4)];
  df=[df(:);zeros(np-4,1)];
  gradCp(21:25,:)=[df'-x'*(fh*gradx+dfj);zeros(4,np)]+ ..
                  (D-E*F)'*(fh*gradx+dfj)-kron(fj'*E,eye(ns))*gradFp;
  gradC=Knn*gradCp;

  % user-defined
  % sum10,sum11 below

  AA=Ao-K*Cbar;
  lamb(T,:)=innov(T,:)*Sigmazi*Cbar;
  sum3=Xt(T-1,:)'*lamb(T,:);
  sum4=DY(T-1,:)'*lamb(T,:);
  sum5=innov(T-1,:)'*lamb(T,:);
  sum7=Y(T-1,:)'*lamb(T,:);
  sum10=(T-1)*V*Y(T-1,:)'*lamb(T,:);
  sum11=T*V*Y(T,:)'*lamb(T,:);
  for i=T-1:-1:t0+1;
    lamb(i,:)=lamb(i+1,:)*AA+innov(i,:)*Sigmazi*Cbar;  
    sum3=sum3+Xt(i-1,:)'*lamb(i,:);
    sum4=sum4+DY(i-1,:)'*lamb(i,:);
    sum5=sum5+innov(i-1,:)'*lamb(i,:);
    sum7=sum7+Y(i-1,:)'*lamb(i,:);
    sum10=sum10+(i-1)*V*Y(i-1,:)'*lamb(i,:);
    sum11=sum11+i*V*Y(i,:)'*lamb(i,:);
  end;
  lamb(t0,:)=lamb(t0+1,:)*AA+innov(t0,:)*Sigmazi*Cbar;
  sum3=sum3/(T-t0+1);
  sum4=sum4/(T-t0+1);
  sum5=sum5/(T-t0+1);
  sum7=sum7/(T-t0+1);
  sum10=-sum10/(lambda*(T-t0+1));
  sum11=-sum11/(lambda*(T-t0+1));

  M1=Sigmazi-Sigmazi*sum1*Sigmazi;
  M2=Cbar'*M1*Cbar-2*Cbar'*Sigmazi*sum5*AA;
  M21=Cbar'*M1*Cbar;
  M22=-2*Cbar'*Sigmazi*sum5*AA;
  M3=dlyap(AA',.5*(M2+M2'));
  M31=dlyap(AA',.5*(M21+M21'));
  M32=dlyap(AA',.5*(M22+M22'));
  tem1=zeros(Ao);
  tem2=zeros(C);
  tem3=zeros(Omega);
  for j=1:np;
    tem1(:)=gradAo(:,j);
    d1(j,1)=2*trace(tem1*((-sum3-S*Cbar'*Sigmazi*sum5)*(eye(S)-K*C) .. 
      +S*Cbar'*M1*C-sum2*Sigmazi*C-S*AA'*sum5'*Sigmazi*C+S*Cbar'*K'* ..
      M3*K*C+S*AA'*M3-S*Ao'*M3*K*C));
    tem2(:)=gradC(:,j);
    d2(j,1)=2*trace(tem2*(Ao*S*Cbar'*M1-S*Cbar'*M1*G+Sigma*C'*M1- ..
      Ao*sum2*Sigmazi+sum2*Sigmazi*G-sum3*K*G+Ao*sum3*K-Sigma* ..
      (eye(S)-C'*K')*sum5'*Sigmazi));
    d2(j,1)=d2(j,1)+2*trace(tem2*(Sigma*C'*Sigmazi*sum5*K-Ao*S*AA'*sum5' ..
      *Sigmazi+Ao*S*AA'*sum5'*Sigmazi*G+Ao*S*Cbar'*Sigmazi*sum5*K-S* ..
      Cbar'*Sigmazi*sum5*K*G-Sigma*M3*K-Ao*S*AA'*M3*K+S*AA'*M3*K*G)); 
    d2(j,1)=d2(j,1)+2*trace(tem2*(Sigma*C'*K'*M3*K));
    tem1(:)=gradS(:,j);  
    d3(j,1)=2*trace(tem1*(.5*C'*M1*C-C'*Sigmazi*sum5*(eye(S)-K*C)+.5*M3- ..
      C'*K'*M3+.5*C'*K'*M3*K*C));
    tem3(:)=gradO(:,j);
    d4(j,1)=trace(tem3*(2*Sigmazi*sum5*K+M1+K'*M3*K));
    d5(j,1)=-2*trace(gradX(:,j)*lamb(t0,:)/(T-t0+1));
    tem1(:)=gradG(:,j);
    d6(j,1)=2*trace(tem1*(-C*S*Cbar'*M1-(sum6-C*sum2)*Sigmazi-C*sum3* ..
      K+C*S*AA'*sum5'*Sigmazi-C*S*Cbar'*Sigmazi*sum5*K+sum7*K+C*S*Ao'* ..
      M3*K-C*S*Cbar'*K'*M3*K));
  end; 


%  G2=(T-t0+1)*trace(dGii*Sigmazi)/fsize


    tem1(:)=gradAo(:,6);
    GG2=2*trace(tem1*((-sum3-S*Cbar'*Sigmazi*sum5)*(eye(S)-K*C) .. 
      -sum2*Sigmazi*C-S*AA'*sum5'*Sigmazi*C+S*Cbar'*K'* ..
      M32*K*C+S*AA'*M32-S*Ao'*M32*K*C));
    tem2(:)=gradC(:,6);
    gg2=2*trace(tem2*(-Ao*sum2*Sigmazi+sum2*Sigmazi*G- ..
      sum3*K*G+Ao*sum3*K-Sigma* ..
      (eye(S)-C'*K')*sum5'*Sigmazi));
    gg2=gg2+2*trace(tem2*(Sigma*C'*Sigmazi*sum5*K-Ao*S*AA'*sum5' ..
      *Sigmazi+Ao*S*AA'*sum5'*Sigmazi*G+Ao*S*Cbar'*Sigmazi*sum5*K-S* ..
      Cbar'*Sigmazi*sum5*K*G-Sigma*M32*K-Ao*S*AA'*M32*K+S*AA'*M32*K*G)); 
    gg2=gg2+2*trace(tem2*(Sigma*C'*K'*M32*K));
(T-t0+1)*[GG2,gg2,-2*trace(K*sum11-K*G*sum10),2*trace(sum8*Sigmazi- ..
        G*sum9*Sigmazi),-2*trace(gradX(:,6)*lamb(t0,:)/(T-t0+1))]

    GG2=GG2+gg2;

    GG2=GG2-2*trace(K*sum11-K*G*sum10)+2*trace(sum8*Sigmazi- ..
        G*sum9*Sigmazi); 
    GG2=GG2-2*trace(gradX(:,6)*lamb(t0,:)/(T-t0+1));

  [G2,GG2*(T-t0+1)]

  dL=d1+d2+d3+d4+d5+d6;
  dL(6)=dL(6)-2*trace(K*sum11-K*G*sum10)+2*trace(sum8*Sigmazi- ..
    G*sum9*Sigmazi)  +1.5*(T-t0+2)/lambda;
  dL=diag(Scale)*(T-t0+1)*dL/fsize;
%  del=eye(22)*1e-6;
%  dL(6)=(hs3mle(Thet+del(:,6))-hs3mle(Thet-del(:,6)))/(2*del(6,6));
end;

%  GG1=2*trace(tem1*(S*Cbar'*M1*C+S*Cbar'*K'*M31*K*C+S*AA'*M31-S*Ao'*M31*K*C));
%    gg1=2*trace(tem2*(Ao*S*Cbar'*M1-S*Cbar'*M1*G+Sigma*C'*M1 ..
%      -Sigma*M31*K-Ao*S*AA'*M31*K+S*AA'*M31*K*G+Sigma*C'*K'*M31*K));
