diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
index 7199e8aedad634ab42c37d16cd30d54c4a12a6fc..2bbb1da3f429a6c56fe2af14ad36484f55bf3b6f 100644
--- a/demos/mcapi_pov_estimation.cpp
+++ b/demos/mcapi_pov_estimation.cpp
@@ -646,66 +646,62 @@ int main (int argc, char **argv) {
     VectorComposite bi_bias_est;
     for (auto& elt: problem->getTrajectory()->getFrameMap()){
         auto kf = elt.second;
-        if (kf->isKey()){
-            kf_state = kf->getState();
-            cap_imu = kf->getCaptureOf(sen_imu); 
-            bi_bias_est = cap_imu->getState();
-
-            file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                     << kf->getTimeStamp().get() << ","
-                     << kf_state['P'](0) << ","
-                     << kf_state['P'](1) << ","
-                     << kf_state['P'](2) << "," 
-                     << kf_state['O'](0) << "," 
-                     << kf_state['O'](1) << "," 
-                     << kf_state['O'](2) << "," 
-                     << kf_state['O'](3) << "," 
-                     << kf_state['V'](0) << "," 
-                     << kf_state['V'](1) << "," 
-                     << kf_state['V'](2) << ","
-                     << bi_bias_est['I'](0) << ","
-                     << bi_bias_est['I'](1) << ","
-                     << bi_bias_est['I'](2) << ","
-                     << bi_bias_est['I'](3) << ","
-                     << bi_bias_est['I'](4) << ","
-                     << bi_bias_est['I'](5)
-                     << "\n"; 
-
-            // compute covariances of KF and capture stateblocks
-            Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
-            Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
-            
-            solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
-            problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
-            problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
-            problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
-
-            solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
-            problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
-            file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                         << kf->getTimeStamp().get() << ","
-                         << Qp(0,0) << ","
-                         << Qp(1,1) << ","
-                         << Qp(2,2) << ","
-                         << Qo(0,0) << ","
-                         << Qo(1,1) << ","
-                         << Qo(2,2) << ","
-                         << Qv(0,0) << ","
-                         << Qv(1,1) << ","
-                         << Qv(2,2) << ","
-                         << Qbi(0,0) << ","
-                         << Qbi(1,1) << ","
-                         << Qbi(2,2) << ","
-                         << Qbi(3,3) << ","
-                         << Qbi(4,4) << ","
-                         << Qbi(5,5)                    
-                         << "\n"; 
-
-
-        }
+        kf_state = kf->getState();
+        cap_imu = kf->getCaptureOf(sen_imu); 
+        bi_bias_est = cap_imu->getState();
+
+        file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                    << kf->getTimeStamp().get() << ","
+                    << kf_state['P'](0) << ","
+                    << kf_state['P'](1) << ","
+                    << kf_state['P'](2) << "," 
+                    << kf_state['O'](0) << "," 
+                    << kf_state['O'](1) << "," 
+                    << kf_state['O'](2) << "," 
+                    << kf_state['O'](3) << "," 
+                    << kf_state['V'](0) << "," 
+                    << kf_state['V'](1) << "," 
+                    << kf_state['V'](2) << ","
+                    << bi_bias_est['I'](0) << ","
+                    << bi_bias_est['I'](1) << ","
+                    << bi_bias_est['I'](2) << ","
+                    << bi_bias_est['I'](3) << ","
+                    << bi_bias_est['I'](4) << ","
+                    << bi_bias_est['I'](5)
+                    << "\n"; 
+
+        // compute covariances of KF and capture stateblocks
+        Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
+        Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
+        
+        solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
+        problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+        problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+        problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+        solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+        file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                        << kf->getTimeStamp().get() << ","
+                        << Qp(0,0) << ","
+                        << Qp(1,1) << ","
+                        << Qp(2,2) << ","
+                        << Qo(0,0) << ","
+                        << Qo(1,1) << ","
+                        << Qo(2,2) << ","
+                        << Qv(0,0) << ","
+                        << Qv(1,1) << ","
+                        << Qv(2,2) << ","
+                        << Qbi(0,0) << ","
+                        << Qbi(1,1) << ","
+                        << Qbi(2,2) << ","
+                        << Qbi(3,3) << ","
+                        << Qbi(4,4) << ","
+                        << Qbi(5,5)                    
+                        << "\n"; 
     }
 
     file_gtr.close();
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 38206e6ba37667f515d51fcd02ace7a85f585395..6a1e908bff0d6abffdb7ae0b2e7a5edd994af928 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -474,7 +474,6 @@ int main (int argc, char **argv) {
     params_sen_ft->dist_traveled = 20000.0;
     params_sen_ft->angle_turned = 1000;
     params_sen_ft->voting_active = false;
-    params_sen_ft->voting_aux_active = false;
     ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
     // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
     ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
@@ -501,7 +500,7 @@ int main (int argc, char **argv) {
     P_origin.block<3,3>(0,0)   = pow(std_prior_p, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(3,3)   = pow(std_prior_o, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(6,6)   = pow(std_prior_v, 2) * Matrix3d::Identity();
-    FrameBasePtr KF1 = problem->emplaceKeyFrame(t0, x_origin);
+    FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
     // Prior pose factor
     CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
     pose_prior_capture->emplaceFeatureAndFactor();
@@ -845,103 +844,99 @@ int main (int argc, char **argv) {
     VectorComposite bi_bias_est;
     for (auto& elt: problem->getTrajectory()->getFrameMap()){
         auto kf = elt.second;
-        if (kf->isKey()){
-            kf_state = kf->getState();
-            cap_ikin = kf->getCaptureOf(sen_ikin); 
-            bp_bias_est = cap_ikin->getState();
-            cap_imu = kf->getCaptureOf(sen_imu); 
-            bi_bias_est = cap_imu->getState();
-
-            file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                     << kf->getTimeStamp().get() << ","
-                     << kf_state['P'](0) << ","
-                     << kf_state['P'](1) << ","
-                     << kf_state['P'](2) << "," 
-                     << kf_state['O'](0) << "," 
-                     << kf_state['O'](1) << "," 
-                     << kf_state['O'](2) << "," 
-                     << kf_state['O'](3) << "," 
-                     << kf_state['V'](0) << "," 
-                     << kf_state['V'](1) << "," 
-                     << kf_state['V'](2) << "," 
-                     << bi_bias_est['I'](0) << ","
-                     << bi_bias_est['I'](1) << ","
-                     << bi_bias_est['I'](2) << ","
-                     << bi_bias_est['I'](3) << ","
-                     << bi_bias_est['I'](4) << ","
-                     << bi_bias_est['I'](5) << ","
-                     << kf_state['C'](0) << "," 
-                     << kf_state['C'](1) << "," 
-                     << kf_state['C'](2) << "," 
-                     << kf_state['D'](0) << "," 
-                     << kf_state['D'](1) << "," 
-                     << kf_state['D'](2) << "," 
-                     << kf_state['L'](0) << "," 
-                     << kf_state['L'](1) << "," 
-                     << kf_state['L'](2) << ","
-                     << bp_bias_est['I'](0) << ","
-                     << bp_bias_est['I'](1) << ","
-                     << bp_bias_est['I'](2)
-                     << "\n"; 
-
-            // compute covariances of KF and capture stateblocks
-            Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
-            Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
-            Eigen::MatrixXd Qc =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qd =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd QL =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
-            // solver->computeCovariances(kf->getStateBlockVec());     // !! does not work as intended...
-            
-            solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
-            problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
-            problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
-            problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
-            problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
-            problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
-            problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
-
-            solver->computeCovariances(cap_ikin->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
-            problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
-            // std::cout << "Qbp\n" << Qbp << std::endl;
-            solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
-            problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
-            file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                         << kf->getTimeStamp().get() << ","
-                         << Qp(0,0) << ","
-                         << Qp(1,1) << ","
-                         << Qp(2,2) << ","
-                         << Qo(0,0) << ","
-                         << Qo(1,1) << ","
-                         << Qo(2,2) << ","
-                         << Qv(0,0) << ","
-                         << Qv(1,1) << ","
-                         << Qv(2,2) << ","
-                         << Qbi(0,0) << ","
-                         << Qbi(1,1) << ","
-                         << Qbi(2,2) << ","
-                         << Qbi(3,3) << ","
-                         << Qbi(4,4) << ","
-                         << Qbi(5,5) << ","
-                         << Qc(0,0) << ","
-                         << Qc(1,1) << ","
-                         << Qc(2,2) << ","
-                         << Qd(0,0) << ","
-                         << Qd(1,1) << ","
-                         << Qd(2,2) << ","
-                         << QL(0,0) << ","
-                         << QL(1,1) << ","
-                         << QL(2,2) << ","
-                         << Qbp(0,0) << ","
-                         << Qbp(1,1) << ","
-                         << Qbp(2,2)
-                         << "\n"; 
-
-
-        }
+        kf_state = kf->getState();
+        cap_ikin = kf->getCaptureOf(sen_ikin); 
+        bp_bias_est = cap_ikin->getState();
+        cap_imu = kf->getCaptureOf(sen_imu); 
+        bi_bias_est = cap_imu->getState();
+
+        file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                    << kf->getTimeStamp().get() << ","
+                    << kf_state['P'](0) << ","
+                    << kf_state['P'](1) << ","
+                    << kf_state['P'](2) << "," 
+                    << kf_state['O'](0) << "," 
+                    << kf_state['O'](1) << "," 
+                    << kf_state['O'](2) << "," 
+                    << kf_state['O'](3) << "," 
+                    << kf_state['V'](0) << "," 
+                    << kf_state['V'](1) << "," 
+                    << kf_state['V'](2) << "," 
+                    << bi_bias_est['I'](0) << ","
+                    << bi_bias_est['I'](1) << ","
+                    << bi_bias_est['I'](2) << ","
+                    << bi_bias_est['I'](3) << ","
+                    << bi_bias_est['I'](4) << ","
+                    << bi_bias_est['I'](5) << ","
+                    << kf_state['C'](0) << "," 
+                    << kf_state['C'](1) << "," 
+                    << kf_state['C'](2) << "," 
+                    << kf_state['D'](0) << "," 
+                    << kf_state['D'](1) << "," 
+                    << kf_state['D'](2) << "," 
+                    << kf_state['L'](0) << "," 
+                    << kf_state['L'](1) << "," 
+                    << kf_state['L'](2) << ","
+                    << bp_bias_est['I'](0) << ","
+                    << bp_bias_est['I'](1) << ","
+                    << bp_bias_est['I'](2)
+                    << "\n"; 
+
+        // compute covariances of KF and capture stateblocks
+        Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
+        Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
+        Eigen::MatrixXd Qc =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qd =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd QL =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
+        // solver->computeCovariances(kf->getStateBlockVec());     // !! does not work as intended...
+        
+        solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
+        problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+        problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+        problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+        problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
+        problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
+        problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
+
+        solver->computeCovariances(cap_ikin->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
+        // std::cout << "Qbp\n" << Qbp << std::endl;
+        solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+        file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                        << kf->getTimeStamp().get() << ","
+                        << Qp(0,0) << ","
+                        << Qp(1,1) << ","
+                        << Qp(2,2) << ","
+                        << Qo(0,0) << ","
+                        << Qo(1,1) << ","
+                        << Qo(2,2) << ","
+                        << Qv(0,0) << ","
+                        << Qv(1,1) << ","
+                        << Qv(2,2) << ","
+                        << Qbi(0,0) << ","
+                        << Qbi(1,1) << ","
+                        << Qbi(2,2) << ","
+                        << Qbi(3,3) << ","
+                        << Qbi(4,4) << ","
+                        << Qbi(5,5) << ","
+                        << Qc(0,0) << ","
+                        << Qc(1,1) << ","
+                        << Qc(2,2) << ","
+                        << Qd(0,0) << ","
+                        << Qd(1,1) << ","
+                        << Qd(2,2) << ","
+                        << QL(0,0) << ","
+                        << QL(1,1) << ","
+                        << QL(2,2) << ","
+                        << Qbp(0,0) << ","
+                        << Qbp(1,1) << ","
+                        << Qbp(2,2)
+                        << "\n"; 
     }
 
     file_gtr.close();
diff --git a/demos/params_tree_manager.yaml b/demos/params_tree_manager.yaml
index 632501863c81180b1d042ae5476855ff30308c54..6355024a0ba3875a0a5981422864e7828eb04f26 100644
--- a/demos/params_tree_manager.yaml
+++ b/demos/params_tree_manager.yaml
@@ -2,6 +2,6 @@ config:
   problem:
     tree_manager:
       fix_first_frame: true
-      n_frames: 10
+      n_frames: 5
       type: TreeManagerSlidingWindow
       viral_remove_empty_parent: true
diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml
index 2092da805973a9bdce6752312bd31cd935a8ddc9..9ff74a3a27de6086489f87597829377cc7c6db36 100644
--- a/demos/processor_imu_solo12.yaml
+++ b/demos/processor_imu_solo12.yaml
@@ -8,5 +8,4 @@ keyframe_vote:
     max_buff_length:    100000000000   # motion deltas
     dist_traveled:      20000.0   # meters
     angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      true
-    voting_aux_active:  false
\ No newline at end of file
+    voting_active:      true
\ No newline at end of file
diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
index d3633287a618fed2899767c6c99e5f548176d0f3..950b77442fd54510ac688dd8b4e1d884d6888d9d 100644
--- a/demos/processor_odom_3d.yaml
+++ b/demos/processor_odom_3d.yaml
@@ -5,7 +5,6 @@ unmeasured_perturbation_std: 0.0001
 
 keyframe_vote:
   voting_active:        false
-  voting_aux_active:    false
   max_time_span:          100   # seconds
   max_buff_length:        10    # motion deltas
   dist_traveled:          0.5   # meters
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 5ecd84218ab80f5682f4cf481297571d138163fa..0f0397f27b765e02e14f35d9a7c07342a50636b9 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -279,7 +279,6 @@ int main (int argc, char **argv) {
     params_sen_ft->dist_traveled = 20000.0;
     params_sen_ft->angle_turned = 1000;
     params_sen_ft->voting_active = false;
-    params_sen_ft->voting_aux_active = false;
     ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
     // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
     ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
@@ -313,7 +312,7 @@ int main (int argc, char **argv) {
     P_origin.block<3,3>(0,0)   = pow(std_prior_p, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(3,3)   = pow(std_prior_o, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(6,6)   = pow(std_prior_v, 2) * Matrix3d::Identity();
-    FrameBasePtr KF1 = problem->emplaceKeyFrame(t0, x_origin);
+    FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
     // Prior pose factor
     CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
     pose_prior_capture->emplaceFeatureAndFactor();
@@ -723,103 +722,99 @@ int main (int argc, char **argv) {
     VectorComposite bi_bias_est;
     for (auto& elt: problem->getTrajectory()->getFrameMap()){
         auto kf = elt.second;
-        if (kf->isKey()){
-            kf_state = kf->getState();
-            cap_ikin = kf->getCaptureOf(sen_ikin); 
-            bp_bias_est = cap_ikin->getState();
-            cap_imu = kf->getCaptureOf(sen_imu); 
-            bi_bias_est = cap_imu->getState();
-
-            file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                     << kf->getTimeStamp().get() << ","
-                     << kf_state['P'](0) << ","
-                     << kf_state['P'](1) << ","
-                     << kf_state['P'](2) << "," 
-                     << kf_state['O'](0) << "," 
-                     << kf_state['O'](1) << "," 
-                     << kf_state['O'](2) << "," 
-                     << kf_state['O'](3) << "," 
-                     << kf_state['V'](0) << "," 
-                     << kf_state['V'](1) << "," 
-                     << kf_state['V'](2) << "," 
-                     << bi_bias_est['I'](0) << ","
-                     << bi_bias_est['I'](1) << ","
-                     << bi_bias_est['I'](2) << ","
-                     << bi_bias_est['I'](3) << ","
-                     << bi_bias_est['I'](4) << ","
-                     << bi_bias_est['I'](5) << ","
-                     << kf_state['C'](0) << "," 
-                     << kf_state['C'](1) << "," 
-                     << kf_state['C'](2) << "," 
-                     << kf_state['D'](0) << "," 
-                     << kf_state['D'](1) << "," 
-                     << kf_state['D'](2) << "," 
-                     << kf_state['L'](0) << "," 
-                     << kf_state['L'](1) << "," 
-                     << kf_state['L'](2) << ","
-                     << bp_bias_est['I'](0) << ","
-                     << bp_bias_est['I'](1) << ","
-                     << bp_bias_est['I'](2)
-                     << "\n"; 
-
-            // compute covariances of KF and capture stateblocks
-            Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
-            Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
-            Eigen::MatrixXd Qc =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qd =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd QL =  Eigen::Matrix3d::Identity();
-            Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
-            // solver->computeCovariances(kf->getStateBlockVec());     // !! does not work as intended...
-            
-            solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
-            problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
-            problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
-            problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
-            problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
-            problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
-            problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
-
-            solver->computeCovariances(cap_ikin->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
-            problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
-            // std::cout << "Qbp\n" << Qbp << std::endl;
-            solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
-            problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
-
-            file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-                         << kf->getTimeStamp().get() << ","
-                         << Qp(0,0) << ","
-                         << Qp(1,1) << ","
-                         << Qp(2,2) << ","
-                         << Qo(0,0) << ","
-                         << Qo(1,1) << ","
-                         << Qo(2,2) << ","
-                         << Qv(0,0) << ","
-                         << Qv(1,1) << ","
-                         << Qv(2,2) << ","
-                         << Qbi(0,0) << ","
-                         << Qbi(1,1) << ","
-                         << Qbi(2,2) << ","
-                         << Qbi(3,3) << ","
-                         << Qbi(4,4) << ","
-                         << Qbi(5,5) << ","
-                         << Qc(0,0) << ","
-                         << Qc(1,1) << ","
-                         << Qc(2,2) << ","
-                         << Qd(0,0) << ","
-                         << Qd(1,1) << ","
-                         << Qd(2,2) << ","
-                         << QL(0,0) << ","
-                         << QL(1,1) << ","
-                         << QL(2,2) << ","
-                         << Qbp(0,0) << ","
-                         << Qbp(1,1) << ","
-                         << Qbp(2,2)
-                         << "\n"; 
-
-
-        }
+        kf_state = kf->getState();
+        cap_ikin = kf->getCaptureOf(sen_ikin); 
+        bp_bias_est = cap_ikin->getState();
+        cap_imu = kf->getCaptureOf(sen_imu); 
+        bi_bias_est = cap_imu->getState();
+
+        file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                    << kf->getTimeStamp().get() << ","
+                    << kf_state['P'](0) << ","
+                    << kf_state['P'](1) << ","
+                    << kf_state['P'](2) << "," 
+                    << kf_state['O'](0) << "," 
+                    << kf_state['O'](1) << "," 
+                    << kf_state['O'](2) << "," 
+                    << kf_state['O'](3) << "," 
+                    << kf_state['V'](0) << "," 
+                    << kf_state['V'](1) << "," 
+                    << kf_state['V'](2) << "," 
+                    << bi_bias_est['I'](0) << ","
+                    << bi_bias_est['I'](1) << ","
+                    << bi_bias_est['I'](2) << ","
+                    << bi_bias_est['I'](3) << ","
+                    << bi_bias_est['I'](4) << ","
+                    << bi_bias_est['I'](5) << ","
+                    << kf_state['C'](0) << "," 
+                    << kf_state['C'](1) << "," 
+                    << kf_state['C'](2) << "," 
+                    << kf_state['D'](0) << "," 
+                    << kf_state['D'](1) << "," 
+                    << kf_state['D'](2) << "," 
+                    << kf_state['L'](0) << "," 
+                    << kf_state['L'](1) << "," 
+                    << kf_state['L'](2) << ","
+                    << bp_bias_est['I'](0) << ","
+                    << bp_bias_est['I'](1) << ","
+                    << bp_bias_est['I'](2)
+                    << "\n"; 
+
+        // compute covariances of KF and capture stateblocks
+        Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
+        Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbi =  Eigen::Matrix6d::Identity();
+        Eigen::MatrixXd Qc =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qd =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd QL =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
+        // solver->computeCovariances(kf->getStateBlockVec());     // !! does not work as intended...
+        
+        solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
+        problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+        problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+        problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+        problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
+        problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
+        problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
+
+        solver->computeCovariances(cap_ikin->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
+        // std::cout << "Qbp\n" << Qbp << std::endl;
+        solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+        file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+                        << kf->getTimeStamp().get() << ","
+                        << Qp(0,0) << ","
+                        << Qp(1,1) << ","
+                        << Qp(2,2) << ","
+                        << Qo(0,0) << ","
+                        << Qo(1,1) << ","
+                        << Qo(2,2) << ","
+                        << Qv(0,0) << ","
+                        << Qv(1,1) << ","
+                        << Qv(2,2) << ","
+                        << Qbi(0,0) << ","
+                        << Qbi(1,1) << ","
+                        << Qbi(2,2) << ","
+                        << Qbi(3,3) << ","
+                        << Qbi(4,4) << ","
+                        << Qbi(5,5) << ","
+                        << Qc(0,0) << ","
+                        << Qc(1,1) << ","
+                        << Qc(2,2) << ","
+                        << Qd(0,0) << ","
+                        << Qd(1,1) << ","
+                        << Qd(2,2) << ","
+                        << QL(0,0) << ","
+                        << QL(1,1) << ","
+                        << QL(2,2) << ","
+                        << Qbp(0,0) << ","
+                        << Qbp(1,1) << ","
+                        << Qbp(2,2)
+                        << "\n"; 
     }
 
     for (unsigned int i=0; i < N; i++) { 
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
index af69cf52af0f12ff73149729d7409b5665f2d2f3..46eac65e5d406bea477078112ebfd5e202488240 100644
--- a/test/gtest_processor_point_feet_nomove.cpp
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -136,10 +136,10 @@ TEST_F(PointFeetCaptures, process_all_capture_first)
     CPF6_->process(); // one last capture to create the factor
 
     // factor creation due to keyFrameCallback
-    FrameBasePtr KF1 = problem_->emplaceKeyFrame(2.0, x_origin_);
+    FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
     proc_pfnm_base_->keyFrameCallback(KF1, 0.001);
 
-    FrameBasePtr KF2 = problem_->emplaceKeyFrame(3.0, x_origin_);
+    FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
     proc_pfnm_base_->keyFrameCallback(KF2, 0.001);
 
     problem_->print(4,1,1,1);
@@ -149,10 +149,10 @@ TEST_F(PointFeetCaptures, process_all_capture_first)
 TEST_F(PointFeetCaptures, process_all_capture_last)
 {
     // First, get the key frames from the keyFrameCallback
-    FrameBasePtr KF1 = problem_->emplaceKeyFrame(2.0, x_origin_);
+    FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
     proc_pfnm_base_->keyFrameCallback(KF1, 0.001);
 
-    FrameBasePtr KF2 = problem_->emplaceKeyFrame(3.0, x_origin_);
+    FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
     proc_pfnm_base_->keyFrameCallback(KF2, 0.001);
 
     // Then process the captures
diff --git a/test/processor_imu.yaml b/test/processor_imu.yaml
index 9cf58bfe5574a81713cd2496dd3a48738a2aa7c9..468ed456e8fff451dc495eeb2e48b1eabaf104be 100644
--- a/test/processor_imu.yaml
+++ b/test/processor_imu.yaml
@@ -7,5 +7,4 @@ keyframe_vote:
     max_buff_length:    500   # motion deltas
     dist_traveled:      20000.0   # meters
     angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      true
-    voting_aux_active:  false
\ No newline at end of file
+    voting_active:      true
\ No newline at end of file