diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ddda01d422e8492b5a10e2f212dccf48a7814afa..7cb79904da5050e6ea09e195031f2c42e12183d9 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -997,7 +997,7 @@ void Problem::setPriorOptions(const std::string& _mode,
             for (const auto& ckey : prior_options_->structure)
             {
                 const auto& key = string(1,ckey); // ckey is char
-                auto cov_blk = (_sigma.at(key).array() * _sigma.at(key).array()).matrix();
+                auto cov_blk = (_sigma.at(key).array() * _sigma.at(key).array()).matrix().asDiagonal();
                 Q.emplace(key,key,cov_blk);
             }
             WOLF_TRACE("prior covariance:\n"    , Q);
@@ -1065,39 +1065,6 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
 
             }
 
-
-//            for (auto sb : prior_keyframe->getStateBlockVec())
-//            {
-//                assert(sb != nullptr);
-//                assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
-//                assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
-//
-//                // state block segment
-//                Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
-//                Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
-//
-//                // feature
-//                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
-//
-//                // factor
-//                if (sb->hasLocalParametrization())
-//                {
-//                    if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
-//                        auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
-//                    else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
-//                        auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
-//                    else
-//                        throw std::runtime_error("not implemented...!");
-//                }
-//                else
-//                {
-//                    auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
-//                }
-//                state_idx += sb->getSize();
-//                cov_idx += sb->getLocalSize();
-//            }
-//            assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
-//            assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
         }
         else
             assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");