diff --git a/matlab/DiffuseKalmanSmoother1_Z.m b/matlab/DiffuseKalmanSmoother1_Z.m
index 807ec74ef..f474afdd2 100644
--- a/matlab/DiffuseKalmanSmoother1_Z.m
+++ b/matlab/DiffuseKalmanSmoother1_Z.m
@@ -63,6 +63,7 @@ global options_
 d = 0;
 decomp = [];
 nk = options_.nk;
+kalman_tol = options_.kalman_tol;
 spinf           = size(Pinf1);
 spstar          = size(Pstar1);
 v               = zeros(pp,smpl);
@@ -91,28 +92,51 @@ etahat          = zeros(rr,smpl);
 r               = zeros(mm,smpl+1);
 
 t = 0;
-while rank(Pinf(:,:,t+1),crit1) & t<smpl
+while rank(Pinf(:,:,t+1),kalman_tol) && (t<smpl)
     t = t+1;
     v(:,t)= Y(:,t) - Z*a(:,t);
-    F = Z*Pinf(:,:,t)*Z';
-    if rcond(F) < crit
-        return          
+    Finf  = Z*Pinf(:,:,t)*Z' ;
+    if rcond(Finf) < kalman_tol
+        if ~all(abs(Finf(:)) < kalman_tol)
+            % The univariate diffuse kalman filter should be used.
+            return
+        else
+            Fstar(:,:,t)  = Z*Pstar(:,:,t)*Z' + H;
+            if rcond(Fstar(:,:,t)) < kalman_tol
+                if ~all(abs(Fstar(:,:,t))<kalman_tol)
+                    % The univariate diffuse kalman filter should be used.
+                    return
+                else
+                    a(:,:,t+1) = T*a(:,:,t);
+                    Pstar(:,:,t+1) = T*Pstar(:,:,t)*transpose(T)+QQ;
+                    Pinf(:,:,t+1)  = T*Pinf(:,:,t)*transpose(T);
+                end
+            else
+                iFstar = inv(Fstar(:,:,t));
+                Kstar(:,:,t)  = Pstar(:,:,t)*Z'*iFstar(:,:,t);
+                Pinf(:,:,t+1)   = T*Pinf(:,:,t)*transpose(T);
+                Pstar(:,:,t+1)  = T*(Pstar(:,:,t)-Pstar(:,:,t)*Z'*Kstar(:,:,t)')*T'+QQ;
+                a(:,:,t+1)        = T*(a(:,:,t)+Kstar(:,:,t)*v(:,t));
+            end
+        end
+    else
+        iFinf(:,:,t)        = inv(Finf);
+        PZI                 = Pinf(:,:,t)*Z'*iFinf(:,:,t);
+        atilde(:,t)         = a(:,t) + PZI*v(:,t);
+        Kinf(:,:,t)         = T*PZI;
+        a(:,t+1)            = T*atilde(:,t);
+        aK(1,:,t+1)         = a(:,t+1);
+        % isn't a meaningless as long as we are in the diffuse part? MJ
+        for jnk=2:nk
+            aK(jnk,:,t+jnk) = T*dynare_squeeze(aK(jnk-1,:,t+jnk-1));
+        end
+        Linf(:,:,t)         = T - Kinf(:,:,t)*Z;
+        Fstar(:,:,t)        = Z*Pstar(:,:,t)*Z';
+        Kstar(:,:,t)        = (T*Pstar(:,:,t)*Z'-Kinf(:,:,t)*Fstar(:,:,t))*iFinf(:,:,t);
+        Pstar(:,:,t+1)      = T*Pstar(:,:,t)*T'-T*Pstar(:,:,t)*Z'*Kinf(:,:,t)'-Kinf(:,:,t)*F*Kstar(:,:,t)' + QQ;
+        Pinf(:,:,t+1)       = T*Pinf(:,:,t)*T'-T*Pinf(:,:,t)*Z'*Kinf(:,:,t)';
     end
-    iFinf(:,:,t)        = inv(F);
-    PZI                 = Pinf(:,:,t)*Z'*iFinf(:,:,t);
-    atilde(:,t)         = a(:,t) + PZI*v(:,t);
-    Kinf(:,:,t)         = T*PZI;
-    a(:,t+1)            = T*atilde(:,t);
-    aK(1,:,t+1)         = a(:,t+1);
-    % isn't a meaningless as long as we are in the diffuse part? MJ
-    for jnk=2:nk
-        aK(jnk,:,t+jnk) = T*dynare_squeeze(aK(jnk-1,:,t+jnk-1));
-    end
-    Linf(:,:,t)         = T - Kinf(:,:,t)*Z;
-    Fstar(:,:,t)        = Z*Pstar(:,:,t)*Z';
-    Kstar(:,:,t)        = (T*Pstar(:,:,t)*Z'-Kinf(:,:,t)*Fstar(:,:,t))*iFinf(:,:,t);
-    Pstar(:,:,t+1)      = T*Pstar(:,:,t)*T'-T*Pstar(:,:,t)*Z'*Kinf(:,:,t)'-Kinf(:,:,t)*F*Kstar(:,:,t)' + QQ;
-    Pinf(:,:,t+1)       = T*Pinf(:,:,t)*T'-T*Pinf(:,:,t)*Z'*Kinf(:,:,t)';
+
 end
 d = t;
 P(:,:,d+1) = Pstar(:,:,d+1);
diff --git a/matlab/DsgeSmoother.m b/matlab/DsgeSmoother.m
index e33dec1fe..7cf0cc0d3 100644
--- a/matlab/DsgeSmoother.m
+++ b/matlab/DsgeSmoother.m
@@ -203,7 +203,7 @@ if any(any(H ~= 0))   % should be replaced by a flag
                                                 nobs,np,smpl,mf);
             end
         end
-    elseif options_.kalman_algo == 2
+    elseif kalman_algo == 2
         if ~estim_params_.ncn
             [alphahat,epsilonhat,etahat,ahat,aK] = ...
                 DiffuseKalmanSmootherH3(T,R,Q,H,Pinf,Pstar,Y,trend,nobs,np,smpl,mf);
@@ -289,7 +289,7 @@ else
                                                               data1,nobs,np,smpl);
         end
         if all(alphahat(:)==0)
-            options_.kalman_algo = 4;
+            kalman_algo = 4;
         end
     end
     if kalman_algo == 4