diff --git a/mex/sources/estimation/KalmanFilter.cc b/mex/sources/estimation/KalmanFilter.cc
index 5791f9cab..e208ccd7d 100644
--- a/mex/sources/estimation/KalmanFilter.cc
+++ b/mex/sources/estimation/KalmanFilter.cc
@@ -39,9 +39,9 @@ KalmanFilter::KalmanFilter(const std::string &modName, size_t n_endo, size_t n_e
                            double riccati_tol_in, double lyapunov_tol, int &info) :
   Z(varobs_arg.size(), riv.size()), T(riv.size()), R(riv.size(), n_exo),
   Pstar(riv.size(), riv.size()), Pinf(riv.size(), riv.size()), RQRt(riv.size(), riv.size()),
-  Pold(riv.size(), riv.size()), Ptmp(riv.size(), riv.size()), F(varobs_arg.size(), varobs_arg.size()),
-  Finv(varobs_arg.size(), varobs_arg.size()), K(riv.size(), varobs_arg.size()),
-  KFinv(riv.size(), varobs_arg.size()), Finverter(varobs_arg.size()), a_init(riv.size(), 1),
+  Ptmp(riv.size(), riv.size()), F(varobs_arg.size(), varobs_arg.size()),
+  Finv(varobs_arg.size(), varobs_arg.size()), K(riv.size(), varobs_arg.size()), KFinv(riv.size(), varobs_arg.size()),
+  oldKFinv(riv.size(), varobs_arg.size()), Finverter(varobs_arg.size()), a_init(riv.size(), 1),
   a_new(riv.size(), 1), vt(varobs_arg.size(), 1), vtFinv(1, varobs_arg.size()), vtFinvVt(1), riccati_tol(riccati_tol_in),
   initKalmanFilter(modName, n_endo, n_exo, zeta_fwrd_arg, zeta_back_arg, zeta_mixed_arg,
                    zeta_static_arg, ll_incidence, qz_criterium, order_var, inv_order_var, riv, ric, lyapunov_tol, info)
@@ -94,7 +94,7 @@ KalmanFilter::filter(const Matrix &dataView,  const Matrix &H, Vector &vll, size
           // Finv=inv(F)
           mat::set_identity(Finv);
           Finverter.invMult("N", F, Finv); // F now contains its LU decomposition!
-          // KFinv
+          // KFinv gain matrix
           KFinv.setAll(0.0);
           blas::gemm("N", "N", 1.0, K, Finv, 1.0, KFinv);
           // deteminant of F:
@@ -104,7 +104,6 @@ KalmanFilter::filter(const Matrix &dataView,  const Matrix &H, Vector &vll, size
 
           logFdet=log(fabs(Fdet));
 
-          Pold = Pstar;
           Ptmp = Pstar;
           // Pt+1= T(Pt - KFinvK')T' +RQR'
           // 1) Ptmp= Pt - K*FinvK'
@@ -116,7 +115,9 @@ KalmanFilter::filter(const Matrix &dataView,  const Matrix &H, Vector &vll, size
           // 3) Pt+1= Ptmp*T' +RQR'
           Pstar = RQRt;
           blas::gemm("N", "T", 1.0, Ptmp, T, 1.0, Pstar);
-          nonstationary = mat::isDiffSym(Pstar, Pold, riccati_tol);
+          if (t>0)
+            nonstationary = mat::isDiff(KFinv, oldKFinv, riccati_tol);
+          oldKFinv=KFinv;
         }
 
       // err= Yt - Za
diff --git a/mex/sources/estimation/KalmanFilter.hh b/mex/sources/estimation/KalmanFilter.hh
index dbdc62528..fab350f35 100644
--- a/mex/sources/estimation/KalmanFilter.hh
+++ b/mex/sources/estimation/KalmanFilter.hh
@@ -68,9 +68,9 @@ private:
   Matrix Pstar; //mm*mm variance-covariance matrix of stationary variables
   Matrix Pinf;  //mm*mm variance-covariance matrix of diffuse variables
   // allocate space for intermediary matrices
-  Matrix RQRt, Pold, Ptmp;  //mm*mm variance-covariance matrix of variable disturbances
+  Matrix RQRt, Ptmp;  //mm*mm variance-covariance matrix of variable disturbances
   Matrix F, Finv;  // nob*nob F=ZPZt +H an inv(F)
-  Matrix K, KFinv; // mm*nobs K=PZt and nm*nm K*Finv
+  Matrix K,  KFinv, oldKFinv; // mm*nobs K=PZt and K*Finv gain matrices
   LUSolver Finverter; // matrix inversion algorithm
   Matrix a_init, a_new; // state vector
   Matrix vt; // current observation error vectors
diff --git a/mex/sources/estimation/ModelSolution.cc b/mex/sources/estimation/ModelSolution.cc
index 71621744e..1d0a79f50 100644
--- a/mex/sources/estimation/ModelSolution.cc
+++ b/mex/sources/estimation/ModelSolution.cc
@@ -71,17 +71,10 @@ ModelSolution::ComputeModelSolution(Vector& steadyState, const Vector& deepParam
           llXsteadyState(((size_t) ll_incidence(ll_row, i))-1) = steadyState(i);
       }
     }
-#ifdef DEBUG
-    //    std::cout << "Vector llXsteadyState: " << std::endl << llXsteadyState << std::endl;
-    mexPrintf(" get jacobian \n");
-#endif
+
     //get jacobian 
     dynamicDLLp.eval(llXsteadyState, Mx, &deepParams,  1,  residual, &jacobian, NULL, NULL);
 
-#ifdef DEBUG
-    std::cout << "jacobian: " << std::endl << jacobian << std::endl;
-    mexPrintf(" compute rules \n");
-#endif
     //compute rules
     decisionRules.compute(jacobian,ghx, ghu);
 }
diff --git a/mex/sources/estimation/tests/testInitKalman.cc b/mex/sources/estimation/tests/testInitKalman.cc
index 576a42311..ef42863cf 100644
--- a/mex/sources/estimation/tests/testInitKalman.cc
+++ b/mex/sources/estimation/tests/testInitKalman.cc
@@ -49,10 +49,6 @@ main(int argc, char **argv)
     0.3167, 0.8610, 1.0085, 0.9917,
     1.3559, 1.0085, 0.9929
   };
-  double vcov1[] = {
-    0.1960e-3, 0.0,
-    0.0, 0.0250e-3
-  };
 
   double vcov[] = {
     0.0013,         0,
diff --git a/mex/sources/estimation/tests/testKalman.cc b/mex/sources/estimation/tests/testKalman.cc
index bed2ee4a5..b95504e85 100644
--- a/mex/sources/estimation/tests/testKalman.cc
+++ b/mex/sources/estimation/tests/testKalman.cc
@@ -49,10 +49,6 @@ main(int argc, char **argv)
     0.3167, 0.8610, 1.0085, 0.9917,
     1.3559, 1.0085, 0.9929
   };
-  double vcov1[] = {
-    0.1960e-3, 0.0,
-    0.0, 0.0250e-3
-  };
 
   double vcov[] = {
     0.0013,         0,