diff --git a/matlab/DsgeSmoother.m b/matlab/DsgeSmoother.m
index 74faa49e5..0140ecf12 100644
--- a/matlab/DsgeSmoother.m
+++ b/matlab/DsgeSmoother.m
@@ -147,7 +147,9 @@ elseif options_.lik_init == 3           % Diffuse Kalman filter
     if kalman_algo ~= 4
         kalman_algo = 3;
     end
-    [Z,ST,R1,QT,Pstar,Pinf] = schur_statespace_transformation(mf,T,R,Q,options_.qz_criterium);
+    [Z,ST,R1,QT,Pstar,Pinf] = schur_statespace_transformation(mf,T,R,Q,options_.qz_criterium,oo_.dr.restrict_var_list);
+    Pinf = QT*Pinf*QT';
+    Pstar = QT*Pstar*QT';
 elseif options_.lik_init == 4           % Start from the solution of the Riccati equation.
     [err, Pstar] = kalman_steady_state(transpose(T),R*Q*transpose(R),transpose(build_selection_matrix(mf,np,vobs)),H);
     mexErrCheck('kalman_steady_state',err);
@@ -196,13 +198,11 @@ if ~missing_value
     end
 end
 
-if kalman_algo == 1 || kalman_algo == 2
-    ST = T;
-    R1 = R;
-    Z = zeros(vobs,size(T,2));
-    for i=1:vobs
-        Z(i,mf(i)) = 1;
-    end
+ST = T;
+R1 = R;
+Z = zeros(vobs,size(T,2));
+for i=1:vobs
+    Z(i,mf(i)) = 1;
 end
 
 if kalman_algo == 1 || kalman_algo == 3
@@ -240,25 +240,6 @@ if kalman_algo == 2 || kalman_algo == 4
                                                       options_.filter_decomposition);
 end
 
-if kalman_algo == 3 || kalman_algo == 4
-    alphahat = QT*alphahat;
-    ahat = QT*ahat;
-    nk = options_.nk;
-    for jnk=1:nk
-        aK(jnk,:,:) = QT*dynare_squeeze(aK(jnk,:,:));
-        for i=1:size(PK,4)
-            PK(jnk,:,:,i) = QT*dynare_squeeze(PK(jnk,:,:,i))*QT';
-        end
-        if options_.filter_decomposition
-            for i=1:size(decomp,4)
-                decomp(jnk,:,:,i) = QT*dynare_squeeze(decomp(jnk,:,:,i));
-            end
-        end
-    end
-    for i=1:size(P,4)
-        P(:,:,i) = QT*dynare_squeeze(P(:,:,i))*QT';
-    end
-end
 
 if estim_params_.ncn && (kalman_algo == 2 || kalman_algo == 4)
     % extracting measurement errors
diff --git a/matlab/dsge_likelihood.m b/matlab/dsge_likelihood.m
index fa9996f87..59198bfda 100644
--- a/matlab/dsge_likelihood.m
+++ b/matlab/dsge_likelihood.m
@@ -398,11 +398,20 @@ switch DynareOptions.lik_init
             error(['The model requires Diffuse filter, but you specified a different Kalman filter. You must set options_.kalman_algo ' ...
                    'to 0 (default), 3 or 4'])
     end
-    [Z,T,R,QT,Pstar,Pinf] = schur_statespace_transformation(Z,T,R,Q,DynareOptions.qz_criterium);
+    [Ztmp,Ttmp,Rtmp,QT,Pstar,Pinf] = schur_statespace_transformation(Z,T,R,Q,DynareOptions.qz_criterium,[1:length(T)]);
+    Pinf = QT*Pinf*QT';
+    Pstar = QT*Pstar*QT';
+    Z1=Ztmp*0;
+    for jz=1:length(Z)
+        Z1(jz,Z(jz))=1;
+    end
+    Z=Z1;
+    clear Ztmp Z1
     Zflag = 1;
     % Run diffuse kalman filter on first periods.
     if (kalman_algo==3)
         % Multivariate Diffuse Kalman Filter
+        Pstar0 = Pstar; % store Pstar
         if no_missing_data_flag
             [dLIK,dlik,a,Pstar] = kalman_filter_d(Y, 1, size(Y,2), ...
                                                        zeros(mm,1), Pinf, Pstar, ...
@@ -419,6 +428,7 @@ switch DynareOptions.lik_init
         if isinf(dLIK)
             % Go to univariate diffuse filter if singularity problem.
             singular_diffuse_filter = 1;
+            Pstar = Pstar0;
         end
     end
     if singular_diffuse_filter || (kalman_algo==4)
@@ -693,7 +703,11 @@ if ((kalman_algo==1) || (kalman_algo==3))% Multivariate Kalman Filter
         if DynareOptions.lik_init==3
             LIK = LIK + dLIK;
             if analytic_derivation==0 && nargout==2,
-                lik = [dlik; lik];
+                if ~singular_diffuse_filter,
+                    lik = [dlik; lik];
+                else
+                    lik = [sum(dlik,2); lik];
+                end
             end
         end
     end
diff --git a/matlab/kalman/likelihood/kalman_filter.m b/matlab/kalman/likelihood/kalman_filter.m
index 8dee95f06..14208f471 100644
--- a/matlab/kalman/likelihood/kalman_filter.m
+++ b/matlab/kalman/likelihood/kalman_filter.m
@@ -169,7 +169,8 @@ while notsteady && t<=last
         v  = Y(:,t)-a(Z);
         F  = P(Z,Z) + H;
     end
-    if rcond(F) < kalman_tol
+    sig=sqrt(diag(F));
+    if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
         if ~all(abs(F(:))<kalman_tol)
             return
         else
@@ -178,9 +179,9 @@ while notsteady && t<=last
         end
     else
         F_singular = 0;
-        dF      = det(F);
-        iF      = inv(F);
-        likk(s) = log(dF)+transpose(v)*iF*v;
+        log_dF      = log(det(F./(sig*sig')))+2*sum(log(sig));
+        iF      = inv(F./(sig*sig'))./(sig*sig');
+        likk(s) = log_dF+transpose(v)*iF*v;
         if Zflag
             K = P*Z'*iF;
             Ptmp = T*(P-K*Z*P)*transpose(T)+QQ;
@@ -245,7 +246,7 @@ if t <= last
             Hess = Hess + tmp{3};
         end
     else
-        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log_dF,Z,pp,Zflag);
     end
 end
 
diff --git a/matlab/kalman/likelihood/kalman_filter_fast.m b/matlab/kalman/likelihood/kalman_filter_fast.m
index d6a7ba257..6eeb90f76 100644
--- a/matlab/kalman/likelihood/kalman_filter_fast.m
+++ b/matlab/kalman/likelihood/kalman_filter_fast.m
@@ -221,7 +221,7 @@ if t <= last
             Hess = Hess + tmp{3};
         end
     else
-        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log(dF),Z,pp,Zflag);
     end
 end
 
diff --git a/matlab/kalman/likelihood/kalman_filter_ss.m b/matlab/kalman/likelihood/kalman_filter_ss.m
index 43493e4fa..8e5f92733 100644
--- a/matlab/kalman/likelihood/kalman_filter_ss.m
+++ b/matlab/kalman/likelihood/kalman_filter_ss.m
@@ -1,4 +1,4 @@
-function [LIK, likk, a] = kalman_filter_ss(Y,start,last,a,T,K,iF,dF,Z,pp,Zflag,analytic_derivation,Da,DT,DYss,D2a,D2T,D2Yss)
+function [LIK, likk, a] = kalman_filter_ss(Y,start,last,a,T,K,iF,log_dF,Z,pp,Zflag,analytic_derivation,Da,DT,DYss,D2a,D2T,D2Yss)
 % Computes the likelihood of a stationnary state space model (steady state kalman filter).
 
 %@info:
@@ -130,7 +130,7 @@ while t <= last
 end
 
 % Adding constant determinant of F (prediction error covariance matrix)
-likk = likk + log(dF);
+likk = likk + log_dF;
 
 % Add log-likelihhod constants and divide by two
 likk = .5*(likk + pp*log(2*pi));
diff --git a/matlab/kalman/likelihood/missing_observations_kalman_filter.m b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
index 577d57beb..4c64bd28d 100644
--- a/matlab/kalman/likelihood/missing_observations_kalman_filter.m
+++ b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
@@ -102,7 +102,8 @@ while notsteady & t<=last
             v = Y(d_index,t) - a(z);
             F = P(z,z) + H(d_index,d_index);
         end
-        if rcond(F) < kalman_tol
+        sig=sqrt(diag(F));
+        if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
             if ~all(abs(F(:))<kalman_tol)
                 return
             else
@@ -111,9 +112,9 @@ while notsteady & t<=last
             end
         else
             F_singular = 0;
-            dF     = det(F);
-            iF     = inv(F);
-            lik(s) = log(dF) + transpose(v)*iF*v + length(d_index)*log(2*pi);
+            log_dF      = log(det(F./(sig*sig')))+2*sum(log(sig));
+            iF      = inv(F./(sig*sig'))./(sig*sig');
+            lik(s) = log_dF + transpose(v)*iF*v + length(d_index)*log(2*pi);
             if Zflag
                 K = P*z'*iF;
                 P = T*(P-K*z*P)*transpose(T)+QQ;
@@ -140,7 +141,7 @@ lik(1:s) = .5*lik(1:s);
 
 % Call steady state Kalman filter if needed.
 if t<=last
-    [tmp, lik(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+    [tmp, lik(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log_dF,Z,pp,Zflag);
 end
 
 % Compute minus the log-likelihood.
diff --git a/matlab/kalman/likelihood/univariate_kalman_filter_d.m b/matlab/kalman/likelihood/univariate_kalman_filter_d.m
index 0782f9507..2383dc602 100644
--- a/matlab/kalman/likelihood/univariate_kalman_filter_d.m
+++ b/matlab/kalman/likelihood/univariate_kalman_filter_d.m
@@ -122,7 +122,7 @@ while newRank && (t<=last)
         Fstar = Zi*Pstar*Zi' + H(d_index(i));
         Finf  = Zi*Pinf*Zi';
         Kstar = Pstar*Zi';
-        if Finf>kalman_tol && newRank
+        if Finf>diffuse_kalman_tol && newRank
             Kinf   = Pinf*Zi';
             Kinf_Finf = Kinf/Finf;
             a         = a + Kinf_Finf*prediction_error;
diff --git a/matlab/missing_DiffuseKalmanSmootherH1_Z.m b/matlab/missing_DiffuseKalmanSmootherH1_Z.m
index 9fc09eb30..b50416072 100644
--- a/matlab/missing_DiffuseKalmanSmootherH1_Z.m
+++ b/matlab/missing_DiffuseKalmanSmootherH1_Z.m
@@ -168,11 +168,13 @@ while notsteady && t<smpl
         ZZ = Z(di,:);
         v(di,t)      = Y(di,t) - ZZ*a(:,t);
         F = ZZ*P(:,:,t)*ZZ' + H(di,di);
-        if rcond(F) < kalman_tol
+        sig=sqrt(diag(F));
+
+        if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
             alphahat = Inf;
             return
         end    
-        iF(di,di,t)   = inv(F);
+        iF(di,di,t)   = inv(F./(sig*sig'))./(sig*sig');
         PZI         = P(:,:,t)*ZZ'*iF(di,di,t);
         atilde(:,t) = a(:,t) + PZI*v(di,t);
         K(:,di,t)    = T*PZI;
diff --git a/matlab/missing_DiffuseKalmanSmootherH3_Z.m b/matlab/missing_DiffuseKalmanSmootherH3_Z.m
index ddd6159b7..dd6b5603c 100644
--- a/matlab/missing_DiffuseKalmanSmootherH3_Z.m
+++ b/matlab/missing_DiffuseKalmanSmootherH3_Z.m
@@ -105,7 +105,7 @@ while newRank && t < smpl
         Fstar(i,t)  = Zi*Pstar(:,:,t)*Zi' +H(i);
         Finf(i,t)   = Zi*Pinf(:,:,t)*Zi';
         Kstar(:,i,t) = Pstar(:,:,t)*Zi';
-        if Finf(i,t) > kalman_tol && newRank
+        if Finf(i,t) > diffuse_kalman_tol && newRank
             icc=icc+1;
             Kinf(:,i,t)       = Pinf(:,:,t)*Zi';
             Kinf_Finf         = Kinf(:,i,t)/Finf(i,t);
@@ -233,7 +233,7 @@ if d
     for t = d:-1:1
         di = flipud(data_index{t})';
         for i = di
-            if Finf(i,t) > kalman_tol 
+            if Finf(i,t) > diffuse_kalman_tol 
                 r1(:,t) = Z(i,:)'*v(i,t)/Finf(i,t) + ...
                           (Kinf(:,i,t)'*Fstar(i,t)/Finf(i,t)-Kstar(:,i,t)')*r0(:,t)/Finf(i,t)*Z(i,:)' + ...
                           r1(:,t)-Kinf(:,i,t)'*r1(:,t)/Finf(i,t)*Z(i,:)';