diff --git a/matlab/DsgeLikelihood_hh.m b/matlab/DsgeLikelihood_hh.m
index e68370f11..9f6cbd2ca 100644
--- a/matlab/DsgeLikelihood_hh.m
+++ b/matlab/DsgeLikelihood_hh.m
@@ -266,4 +266,6 @@ end
 lnprior = priordens(xparam1,bayestopt_.pshape,bayestopt_.p6,bayestopt_.p7,bayestopt_.p3,bayestopt_.p4);
 fval    = (likelihood-lnprior);
 options_.kalman_algo = kalman_algo;
-llik=[-lnprior; lik(start:end)];
+lik=lik(start:end,:);
+llik=[-lnprior; lik(:)];
+% llik=[-lnprior; lik(start:end)];
diff --git a/matlab/ReshapeMatFiles.m b/matlab/ReshapeMatFiles.m
index d15371dfb..ad44331c7 100644
--- a/matlab/ReshapeMatFiles.m
+++ b/matlab/ReshapeMatFiles.m
@@ -115,7 +115,7 @@ switch TYPEarray
             eval(['STOCK_' CAPtype ' = zeros(NumberOfPeriodsPerTYPEfiles,TYPEsize(2),TYPEsize(3),B);'])
             for f2 = 1:NumberOfTYPEfiles
                 load([MhDirectoryName M_.fname '_' type int2str(f2) '.mat']);
-                eval(['STOCK_' CAPtype '(:,:,:,idx+1:idx+size(stock_' type ',4))=stock_' ...
+                eval(['STOCK_' CAPtype '(:,:,1:+size(stock_' type ',3),idx+1:idx+size(stock_' type ',4))=stock_' ...
                       type '(jdx+1:jdx+NumberOfPeriodsPerTYPEfiles,:,:,:);'])
                 eval(['idx = idx + size(stock_' type ',4);'])
             end
diff --git a/matlab/kalman/likelihood/univariate_diffuse_kalman_filter.m b/matlab/kalman/likelihood/univariate_diffuse_kalman_filter.m
index 03d74c1f3..a7fac2699 100644
--- a/matlab/kalman/likelihood/univariate_diffuse_kalman_filter.m
+++ b/matlab/kalman/likelihood/univariate_diffuse_kalman_filter.m
@@ -1,4 +1,4 @@
-function [LIK, lik] = univariate_diffuse_kalman_filter(T,R,Q,H,Pinf,Pstar,Y,start,Z,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
+function [LIK, llik] = univariate_diffuse_kalman_filter(T,R,Q,H,Pinf,Pstar,Y,start,Z,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
 % Computes the likelihood of a stationnary state space model (univariate approach).
 %
 % INPUTS
@@ -52,6 +52,7 @@ a  = zeros(mm,1);
 QQ = R*Q*transpose(R);
 t  = 0;
 lik = zeros(smpl,1);
+llik=zeros(smpl,pp);
 notsteady = 1;
 crit = 1.e-6;
 newRank = rank(Pinf,crit);
@@ -74,10 +75,12 @@ while newRank && (t<smpl)
             Pstar  = Pstar + Kinf*(Kinf_Finf'*(Fstar/Finf)) - Kstar*Kinf_Finf' ...
                      - Kinf_Finf*Kstar';
             Pinf   = Pinf - Kinf*Kinf_Finf';
-            lik(t) = lik(t) + log(Finf) + l2pi;
+            llik(t,i) = log(Finf) + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         elseif Fstar>kalman_tol
-            lik(t) = lik(t) + log(Fstar) + prediction_error* ...
+            llik(t,i) = log(Fstar) + prediction_error* ...
                      prediction_error/Fstar + l2pi;
+            lik(t) = lik(t) + llik(t,i);
             a = a + Kstar*(prediction_error/Fstar);
             Pstar = Pstar - Kstar*(Kstar'/Fstar);
         end
@@ -116,8 +119,9 @@ while notsteady && (t<smpl)
         if Fi > kalman_tol
             a      = a + Ki*(prediction_error/Fi);
             Pstar  = Pstar - Ki*(Ki'/Fi);
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end 
     a     = T*a;
@@ -138,13 +142,15 @@ while t < smpl
             Ki     = Pstar*Zi';
             a      = a + Ki*prediction_error/Fi;
             Pstar  = Pstar - Ki*Ki'/Fi;
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end 
     a = T*a;
 end
 
 lik = lik/2;
+llik = llik/2;
 
-LIK = sum(lik(start:end));
\ No newline at end of file
+LIK = sum(lik(start:end));
diff --git a/matlab/kalman/likelihood/univariate_diffuse_kalman_filter_corr.m b/matlab/kalman/likelihood/univariate_diffuse_kalman_filter_corr.m
index 7c403a603..21230302c 100644
--- a/matlab/kalman/likelihood/univariate_diffuse_kalman_filter_corr.m
+++ b/matlab/kalman/likelihood/univariate_diffuse_kalman_filter_corr.m
@@ -1,4 +1,4 @@
-function [LIK, lik] = univariate_diffuse_kalman_filter_corr(T,R,Q,H,Pinf,Pstar,Y,start,Z,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
+function [LIK, llik] = univariate_diffuse_kalman_filter_corr(T,R,Q,H,Pinf,Pstar,Y,start,Z,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
 % Computes the likelihood of a stationnary state space model (univariate
 % approach with correlated errors).
 %
@@ -52,6 +52,7 @@ a  = zeros(mm,1);
 QQ = R*Q*transpose(R);
 t  = 0;
 lik = zeros(smpl,1);
+llik = zeros(smpl,pp);
 notsteady = 1;
 crit = 1.e-6;
 newRank = rank(Pinf,crit);
@@ -104,7 +105,8 @@ while newRank && (t<smpl)
             a      = a + Kinf*(prediction_error/Finf);
             Pstar  = Pstar + Kinf*(Kinf'*(Fstar/(Finf*Finf))) - (Kstar*Kinf'+Kinf*Kstar')/Finf;
             Pinf   = Pinf - Kinf*(Kinf'/Finf);
-            lik(t) = lik(t) + log(Finf) + l2pi;
+            llik(t,i) = log(Finf) + l2pi;
+            lik(t) = lik(t) + llik(t,i);
             if ~isempty(options_.diffuse_d)
                 newRank = (icc<options_.diffuse_d);
                 if newRank && (any(diag(Za*Pinf*Za')>kalman_tol)==0 & rank(Pinf,crit)==0); 
@@ -125,8 +127,9 @@ while newRank && (t<smpl)
                 end
             end
         elseif Fstar>kalman_tol
-            lik(t) = lik(t) + log(Fstar) + prediction_error* ...
+            llik(t,i) = log(Fstar) + prediction_error* ...
                      prediction_error/Fstar + l2pi;
+            lik(t) = lik(t) + llik(t,i);
             a = a + Kstar*prediction_error/Fstar;
             Pstar = Pstar - Kstar*Kstar'/Fstar;
         end
@@ -165,8 +168,9 @@ while notsteady && (t<smpl)
             Ki  = Pstar*Zi';
             a      = a + Ki*prediction_error/Fi;
             Pstar  = Pstar - Ki*Ki'/Fi;
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end 
     a     = T*a;
@@ -187,13 +191,15 @@ while t < smpl
             Ki     = Pstar*Zi';
             a      = a + Ki*prediction_error/Fi;
             Pstar  = Pstar - Ki*Ki'/Fi;
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end 
     a = T*a;
 end
 
 lik = lik/2;
+llik = lik/2;
 
 LIK = sum(lik(start:end));
\ No newline at end of file
diff --git a/matlab/kalman/likelihood/univariate_kalman_filter.m b/matlab/kalman/likelihood/univariate_kalman_filter.m
index f8db8fbcc..51380de02 100644
--- a/matlab/kalman/likelihood/univariate_kalman_filter.m
+++ b/matlab/kalman/likelihood/univariate_kalman_filter.m
@@ -1,4 +1,4 @@
-function [LIK, lik] = univariate_kalman_filter(T,R,Q,H,P,Y,start,mf,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
+function [LIK, llik] = univariate_kalman_filter(T,R,Q,H,P,Y,start,mf,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
 % Computes the likelihood of a stationnary state space model (univariate approach).
 %
 % INPUTS
@@ -18,7 +18,7 @@ function [LIK, lik] = univariate_kalman_filter(T,R,Q,H,P,Y,start,mf,kalman_tol,r
 %
 % OUTPUTS
 %    LIK        [double]    scalar, MINUS loglikelihood
-%    lik        [double]    vector, density of observations in each period.
+%    llik        [double]    vector, density of observations in each period.
 %
 % REFERENCES
 %   See "Filtering and Smoothing of State Vector for Diffuse State Space
@@ -52,6 +52,7 @@ a      = zeros(mm,1);                          % Initial condition of the state
 QQ     = R*Q*transpose(R);
 t      = 0;
 lik    = zeros(smpl,1); 
+llik    = zeros(smpl,pp); 
 notsteady   = 1;
 l2pi = log(2*pi);
 
@@ -67,8 +68,9 @@ while notsteady && t<smpl
             Ki     = P(:,MF(i))/Fi;
             a      = a + Ki*prediction_error;
             P      = P - (Fi*Ki)*transpose(Ki);
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end
     a = T*a;
@@ -89,13 +91,15 @@ while t < smpl
             Ki = PP(:,mf(i))/Fi;
             a  = a + Ki*prediction_error;
             PP  = PP - (Fi*Ki)*transpose(Ki);
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end
     a = T*a;
 end
 
 lik = lik/2;
+llik = llik/2;
 
 LIK = sum(lik(start:end));
\ No newline at end of file
diff --git a/matlab/kalman/likelihood/univariate_kalman_filter_corr.m b/matlab/kalman/likelihood/univariate_kalman_filter_corr.m
index 4bbccc2c5..72bf5173c 100644
--- a/matlab/kalman/likelihood/univariate_kalman_filter_corr.m
+++ b/matlab/kalman/likelihood/univariate_kalman_filter_corr.m
@@ -1,4 +1,4 @@
-function [LIK, lik] = ...
+function [LIK, llik] = ...
     univariate_kalman_filter_corr(T,R,Q,H,P,Y,start,mf,kalman_tol,riccati_tol,data_index,number_of_observations,no_more_missing_observations)
 % Computes the likelihood of a stationnary state space model (univariate
 % approach + correlated measurement errors).
@@ -20,7 +20,7 @@ function [LIK, lik] = ...
 %
 % OUTPUTS
 %    LIK        [double]    scalar, MINUS loglikelihood
-%    lik        [double]    vector, density of observations in each period.
+%    llik        [double]    vector, density of observations in each period.
 %
 % REFERENCES
 %   See "Filtering and Smoothing of State Vector for Diffuse State Space
@@ -54,6 +54,7 @@ smpl   = size(Y,2);                        % Number of periods in the dataset.
 a      = zeros(mm+pp,1);                      % Initial condition of the state vector.
 t      = 0;
 lik    = zeros(smpl,1); 
+llik    = zeros(smpl,pp); 
 notsteady   = 1;
 
 TT = zeros(mm+pp);
@@ -82,8 +83,9 @@ while notsteady && t<smpl
         prediction_error = Y(d_index(i),t) - a(MF(i)) - a( mm+i );
         Fi = PP(MF(i),MF(i)) + PP(mm+i,mm+i);         
         if Fi > kalman_tol
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
             Ki     = sum(PP(:,[MF(i) mm+i]),2)/Fi;
             a      = a + Ki*prediction_error;
             PP     = PP - (Ki*Fi)*transpose(Ki);
@@ -111,8 +113,9 @@ while t < smpl
             Ki = ( PPPP(:,mf(i)) + PPPP(:,mm+i) )/Fi;
             a  = a + Ki*prediction_error;
             PPPP  = PPPP - (Fi*Ki)*transpose(Ki);
-            lik(t) = lik(t) + log(Fi) + prediction_error*prediction_error/Fi ...
+            llik(t,i) = log(Fi) + prediction_error*prediction_error/Fi ...
                      + l2pi;
+            lik(t) = lik(t) + llik(t,i);
         end
     end
     a(1:mm) = T*a(1:mm);
@@ -120,5 +123,6 @@ while t < smpl
 end
 
 lik = lik/2;
+llik = llik/2;
 
 LIK = sum(lik(start:end));
\ No newline at end of file
diff --git a/matlab/mr_gstep.m b/matlab/mr_gstep.m
index 56d178d65..2f5196a90 100644
--- a/matlab/mr_gstep.m
+++ b/matlab/mr_gstep.m
@@ -27,7 +27,8 @@ n=size(x,1);
 
 if init,
     gstep_ = options_.gstep;
-    h1=max(abs(x),sqrt(gstep_)*ones(n,1))*eps^(1/4);
+%     h1=max(abs(x),sqrt(gstep_)*ones(n,1))*eps^(1/4);
+    h1=options_.gradient_epsilon*ones(n,1);
     return
 end
 if nargin<4, 
@@ -71,7 +72,8 @@ while i<n,
     
     icount = 0;
     h0=h1(i);
-    while (abs(dx(it))<0.5*htol | abs(dx(it))>(2*htol)) & icount<10 & ic==0,
+%     while (abs(dx(it))<0.5*htol | abs(dx(it))>(2*htol)) & icount<10 & ic==0,
+    while (abs(dx(it))<0.5*htol) & icount<10 & ic==0,
         %while abs(dx(it))<0.5*htol & icount< 10 & ic==0,
         icount=icount+1;
         if abs(dx(it)) ~= 0,
@@ -86,10 +88,10 @@ while i<n,
                 %                     ic=1;
                 %                 end  
             end
-            if abs(dx(it))>(2*htol),
-                h1(i)= htol/abs(dx(it))*h1(i);
-                xh1(i)=x(i)+h1(i);
-            end
+%             if abs(dx(it))>(2*htol),
+%                 h1(i)= htol/abs(dx(it))*h1(i);
+%                 xh1(i)=x(i)+h1(i);
+%             end
             try
                 fx = feval(func,xh1,varargin{:});
             catch
diff --git a/matlab/mr_hessian.m b/matlab/mr_hessian.m
index a652b3e86..2c6e4b731 100644
--- a/matlab/mr_hessian.m
+++ b/matlab/mr_hessian.m
@@ -50,7 +50,8 @@ if init,
     htol = 1.e-4; 
     %h1=max(abs(x),gstep_*ones(n,1))*eps^(1/3);
     %h1=max(abs(x),sqrt(gstep_)*ones(n,1))*eps^(1/6);
-    h1=max(abs(x),sqrt(gstep_)*ones(n,1))*eps^(1/4);
+%     h1=max(abs(x),sqrt(gstep_)*ones(n,1))*eps^(1/4);
+    h1=options_.gradient_epsilon*ones(n,1);
     return,
 end
 func = str2func(func);
@@ -102,7 +103,8 @@ while i<n,
     
     icount = 0;
     h0=h1(i);
-    while (abs(dx(it))<0.5*htol | abs(dx(it))>(2*htol)) & icount<10 & ic==0,
+%     while (abs(dx(it))<0.5*htol | abs(dx(it))>(2*htol)) & icount<10 & ic==0,
+    while (abs(dx(it))<0.5*htol) & icount<10 & ic==0,
         %while abs(dx(it))<0.5*htol & icount< 10 & ic==0,
         icount=icount+1;
         %if abs(dx(it)) ~= 0,
@@ -127,21 +129,21 @@ while i<n,
                 fx=1.e8;
             end
         end
-        if abs(dx(it))>(2*htol),
-            h1(i)= htol/abs(dx(it))*h1(i);
-            xh1(i)=x(i)+h1(i);
-            try
-                [fx, ffx]=feval(func,xh1,varargin{:});
-            catch
-                fx=1.e8;
-            end
-            while (fx-f0)==0,
-                h1(i)= h1(i)*2;
-                xh1(i)=x(i)+h1(i);
-                [fx, ffx]=feval(func,xh1,varargin{:});
-                ic=1;
-            end
-        end
+%         if abs(dx(it))>(2*htol),
+%             h1(i)= htol/abs(dx(it))*h1(i);
+%             xh1(i)=x(i)+h1(i);
+%             try
+%                 [fx, ffx]=feval(func,xh1,varargin{:});
+%             catch
+%                 fx=1.e8;
+%             end
+%             while (fx-f0)==0,
+%                 h1(i)= h1(i)*2;
+%                 xh1(i)=x(i)+h1(i);
+%                 [fx, ffx]=feval(func,xh1,varargin{:});
+%                 ic=1;
+%             end
+%         end
         it=it+1;
         dx(it)=(fx-f0);
         h0(it)=h1(i);
@@ -171,7 +173,7 @@ while i<n,
     else
         ff1=ffx;
     end
-    if hflag,  % two point based derivatives
+%     if hflag,  % two point based derivatives
         xh1(i)=x(i)-h1(i);
         %         c=mr_nlincon(xh1,varargin{:});
         %        ic=0;
@@ -193,9 +195,9 @@ while i<n,
         %             [f1(:,i), ff1]=feval(func,xh1,varargin{:});
         %         end
         ggh(:,i)=(ff1-ff_1)./(2.*h1(i));
-    else
-        ggh(:,i)=(ff1-ff0)./h1(i);
-    end
+%     else
+%         ggh(:,i)=(ff1-ff0)./h1(i);
+%     end
     xh1(i)=x(i);
     if hcheck & htol<1,
         htol=min(1,max(min(abs(dx))*2,htol*10));
@@ -209,11 +211,11 @@ h_1=h1;
 xh1=x;
 xh_1=xh1;
 
-if hflag,
+% if hflag,
     gg=(f1'-f_1')./(2.*h1);
-else
-    gg=(f1'-f0)./h1;
-end
+% else
+%     gg=(f1'-f0)./h1;
+% end
 
 if hflag==2,
     gg=(f1'-f_1')./(2.*h1);
diff --git a/matlab/newrat.m b/matlab/newrat.m
index 95f0d9f16..c346ff6b4 100644
--- a/matlab/newrat.m
+++ b/matlab/newrat.m
@@ -82,6 +82,7 @@ else
     hhg=hh;
     igg=inv(hh);
 end
+H = igg;
 disp(['Gradient norm ',num2str(norm(gg))])
 ee=eig(hh);
 disp(['Minimum Hessian eigenvalue ',num2str(min(ee))])
@@ -105,7 +106,7 @@ while norm(gg)>gtol & check==0 & jit<nit,
     bayestopt_.penalty = fval0(icount);
     disp([' '])
     disp(['Iteration ',num2str(icount)])
-    [fval x0 fc retcode] = csminit(func0,xparam1,fval0(icount),gg,0,igg,varargin{:});
+    [fval x0 fc retcode] = csminit(func0,xparam1,fval0(icount),gg,0,H,varargin{:});
     if igrad,
         [fval1 x01 fc retcode1] = csminit(func0,x0,fval,gg,0,inx,varargin{:});
         if (fval-fval1)>1, %(fval0(icount)-fval),
@@ -116,7 +117,7 @@ while norm(gg)>gtol & check==0 & jit<nit,
         fval=fval1;
         x0=x01;        
     end
-    if (fval0(icount)-fval)<1.e-2*(gg'*(igg*gg))/2 & igibbs,
+    if (fval0(icount)-fval)<1.e-2*(gg'*(H*gg))/2 & igibbs,
         if length(find(ig))<nx,
             ggx=ggx*0;
             ggx(find(ig))=gg(find(ig));
@@ -126,6 +127,9 @@ while norm(gg)>gtol & check==0 & jit<nit,
             [fvala x0 fc retcode] = csminit(func0,x0,fval,ggx,0,iggx,varargin{:});
         end
         [fvala, x0, ig] = mr_gstep(0,x0,func0,htol,varargin{:});
+        % if length(find(ig))==0,
+            % [fvala, x0, ig] = mr_gstep(0,x0,func0,htol/10,varargin{:});
+        % end
         nig=[nig ig];
         if (fval-fvala)<gibbstol*(fval0(icount)-fval),
             igibbs=0;
@@ -160,6 +164,8 @@ while norm(gg)>gtol & check==0 & jit<nit,
     x(:,icount+1)=xparam1;
     fval0(icount+1)=fval;
     %if (fval0(icount)-fval)<ftol*ftol & flagg==1;,
+    mr_gstep(1,x);
+    mr_hessian(1,x);
     if (fval0(icount)-fval)<ftol,
         disp('No further improvement is possible!')
         check=1;
@@ -228,6 +234,7 @@ while norm(gg)>gtol & check==0 & jit<nit,
                 end
             end
         end
+
         disp(['Gradient norm  ',num2str(norm(gg))])
         ee=eig(hh);
         disp(['Minimum Hessian eigenvalue ',num2str(min(ee))])
@@ -237,7 +244,9 @@ while norm(gg)>gtol & check==0 & jit<nit,
         disp(['Elapsed time for iteration ',num2str(t),' s.'])
         
         g(:,icount+1)=gg;
-        save m1.mat x hh g hhg igg fval0 nig
+%         H = bfgsi(H,g(:,end)-g(:,end-1),x(:,end)-x(:,end-1));
+        H = igg;
+        save m1.mat x hh g hhg igg fval0 nig H
     end
 end