diff --git a/src/pruning/init_odb.h b/src/pruning/init_odb.h
index 439fd0fa3337accca71f6bbb16626f411dd2af57..5fe3321499ffdafb126a58b6f242a379beec1d91 100644
--- a/src/pruning/init_odb.h
+++ b/src/pruning/init_odb.h
@@ -8,65 +8,113 @@
 #ifndef SRC_PRUNING_INIT_ODB_H_
 #define SRC_PRUNING_INIT_ODB_H_
 
+void makePSD(Eigen::MatrixXs& M)
+{
+    // impose symmetry
+    Eigen::MatrixXs m = M.selfadjointView<Eigen::Upper>();
+
+    // NaN
+    Scalar epsilon = Constants::EPS;
+    while (m.hasNaN() && epsilon < 1e6)
+    {
+        WOLF_WARN("matrix containing NaN, trying to fix it adding Identity matrix");
+        std::cout << m << std::endl;
+        m = m + Eigen::MatrixXs::Identity(m.rows(),m.cols()) * epsilon;
+        epsilon *=10;
+    }
+    assert(!m.hasNaN() && "couldn't fix it...");
+    while (!m.allFinite())
+    {
+        WOLF_WARN("matrix containing Inf, trying to fix it");
+        m = m.cwiseMin(1e12);
+    }
+    assert(m.allFinite() && "couldn't fix it...");
+
+    // eigen decomposition
+    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> eigensolver(m);
+
+    if (eigensolver.info() == Eigen::Success)
+    {
+        // All eigenvalues must be >= 0:
+        epsilon = Constants::EPS;
+        while ((eigensolver.eigenvalues().array() < Constants::EPS).any() || m.determinant() <= 0)
+        {
+            std::cout << "----- any negative eigenvalue or too close to zero\n";
+            std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+            std::cout << "previous determinant: " << m.determinant() << std::endl;
+            m = eigensolver.eigenvectors() *
+                eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
+                eigensolver.eigenvectors().transpose();
+            eigensolver.compute(m);
+            std::cout << "epsilon used: " << epsilon << std::endl;
+            std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+            std::cout << "posterior determinant: " << m.determinant() << std::endl;
+            epsilon *=10;
+        }
+    }
+    else
+    {
+        std::string info;
+        if (eigensolver.info() == Eigen::NumericalIssue)
+            info = "numerical issue";
+        else if (eigensolver.info() == Eigen::NoConvergence)
+            info = "No Convergence";
+        else if (eigensolver.info() == Eigen::InvalidInput)
+            info = "Invalid Input";
+        else
+            info = "unknown";
+
+        WOLF_ERROR("Couldn't compute covariance eigen decomposition: ", info);
+    }
+
+    WOLF_ASSERT_COVARIANCE_MATRIX(m);
+
+    // impose symmetry
+    M = m.selfadjointView<Eigen::Upper>();
+}
+
 Eigen::MatrixXs computeClosestSymPSD(const Eigen::MatrixXs& A)
 {
     assert(A.rows() == A.cols() && "matrix must be square!");
 
+    if (A.hasNaN())
+    {
+        WOLF_WARN("matrix A containing NaN");
+        std::cout << A << std::endl;
+    }
+
     // symmetrize A into B
     Eigen::MatrixXs B = (A + A.transpose()) / 2;
 
+    if (B.hasNaN())
+    {
+        WOLF_WARN("matrix B containing NaN");
+        std::cout << B << std::endl;
+    }
     // Compute the symmetric polar factor of B. Call it H (Clearly H is itself SPD)
     //[U,Sigma,V] = svd(B);
     Eigen::JacobiSVD<Eigen::MatrixXs> svd(B, Eigen::ComputeFullU | Eigen::ComputeFullV);
     Eigen::MatrixXs Sigma = svd.singularValues().asDiagonal();
     Eigen::MatrixXs V = svd.matrixV();
     Eigen::MatrixXs H = V*Sigma*V.transpose();
-
+    if (H.hasNaN())
+    {
+        WOLF_WARN("matrix H containing NaN");
+        std::cout << H << std::endl;
+    }
     //get Ahat in the above formula
     Eigen::MatrixXs Ahat = (B+H)/2;
+    if (Ahat.hasNaN())
+    {
+        WOLF_WARN("matrix Ahat containing NaN");
+        std::cout << Ahat << std::endl;
+    }
 
     // ensure symmetry
-    Eigen::MatrixXs Ahat_sym = (Ahat + Ahat.transpose())/2;
+    Eigen::MatrixXs Ahat_sym = Ahat.selfadjointView<Eigen::Upper>();
 
     // Ensure pos semi def
-    Scalar epsilon = 1e-12;
-    if(!(Ahat_sym.determinant() > 0))
-    {
-        Eigen::MatrixXs Ahat_sym_fixed(Ahat_sym);
-        Eigen::EigenSolver<Eigen::MatrixXs> es(Ahat_sym_fixed);
-        Eigen::VectorXs eval = es.eigenvalues().real();
-
-        //std::cout << "!! Closest sym PSD not PD det = " << Ahat_sym_fixed.determinant() << std::endl;
-        //std::cout << "Matrix:" << std::endl << Ahat_sym_fixed << std::endl;
-        //std::cout << "eval"  << std::endl << eval.transpose() << std::endl;
-
-        eval = eval.array().max(epsilon).matrix();
-        Ahat_sym_fixed = es.eigenvectors().real() * eval.asDiagonal() * es.eigenvectors().real().transpose();
-
-        //std::cout << "eval pos def"  << std::endl << eval.transpose() << std::endl;
-        //std::cout << "Fixed Matrix:" << std::endl << Ahat_sym << std::endl;
-
-        if (Ahat_sym_fixed.hasNaN())
-            Ahat_sym_fixed = Ahat_sym + Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon;
-
-        while (!(Ahat_sym.determinant() > 0))
-        {
-            //std::cout << "Still not PD Ahat_sym.determinant() = "  << Ahat_sym.determinant() << std::endl;
-            epsilon *=2;
-            //eval = eval.array().max(epsilon).matrix();
-            //Ahat_sym = es.eigenvectors().real() * eval.asDiagonal() * es.eigenvectors().real().inverse() + Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon;
-            Ahat_sym += Eigen::MatrixXs::Identity(Ahat_sym.rows(),Ahat_sym.cols()) * epsilon;
-            //std::cout << "FIXED Ahat_sym.determinant() = "  << Ahat_sym.determinant() << std::endl;
-
-            if (epsilon > 1)
-            {
-                std::cout << "FAILED TO find closes sym PSD\n";
-                break;
-            }
-        }
-
-    }
-    assert(Ahat_sym.determinant() >= 0);
+    makePSD(Ahat_sym);
 
     return Ahat_sym;
 
diff --git a/src/pruning/problem_pruning.cpp b/src/pruning/problem_pruning.cpp
index 20b1e7c654dab32fbcd6cf60eb8887c3130e91a0..871675fece542fa13b465a0976a69727612839da 100644
--- a/src/pruning/problem_pruning.cpp
+++ b/src/pruning/problem_pruning.cpp
@@ -281,16 +281,15 @@ unsigned int computeStateBlockMap(const FrameBaseList& _fr_list, std::map<StateB
     //std::cout << "computeStateBlockMap:\n";
     unsigned int size = 0;
     for (auto fr_ptr : _fr_list)
-        if (!fr_ptr->isFixed())
-        {
-            fr_2_col_[fr_ptr] = size;
-            for (auto sb_ptr : fr_ptr->getStateBlockVec())
-                if (sb_ptr)
-                {
-                    sb_2_col_[sb_ptr] = size;
-                    size += sb_ptr->getSize();
-                }
-        }
+    {
+        fr_2_col_[fr_ptr] = size;
+        for (auto sb_ptr : fr_ptr->getStateBlockVec())
+            if (sb_ptr)
+            {
+                sb_2_col_[sb_ptr] = size;
+                size += sb_ptr->getSize();
+            }
+    }
     return size;
 }