diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml
index 3ef24102e36a6aafa04839f277ec6eb8dac69c4b..d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8 100644
--- a/demos/processor_ft_preint.yaml
+++ b/demos/processor_ft_preint.yaml
@@ -9,5 +9,4 @@ keyframe_vote:
     max_buff_length:    50000   # motion deltas
     dist_traveled:      20000.0   # meters
     angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
-    voting_active:      false
-    voting_aux_active:  false
\ No newline at end of file
+    voting_active:      false
\ No newline at end of file
diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml
index 9ff74a3a27de6086489f87597829377cc7c6db36..f92b74c06167c96ef4294e00600c7270bd0d20e2 100644
--- a/demos/processor_imu_solo12.yaml
+++ b/demos/processor_imu_solo12.yaml
@@ -1,11 +1,10 @@
-type: "ProcessorImu"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "Main imu"             # This is ignored. The name provided to the SensorFactory prevails
-time_tolerance: 0.0005         # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.000001
 keyframe_vote:
-    max_time_span:      0.05  # seconds
-    # max_time_span:      0.075  # slower walking
-    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
\ No newline at end of file
+  angle_turned: 1000
+  dist_traveled: 20000.0
+  max_buff_length: 100000000000
+  max_time_span: 0.19999
+  voting_active: true
+name: Main imu
+time_tolerance: 0.0005
+type: ProcessorImu
+unmeasured_perturbation_std: 1.0e-06
diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml
index 4bc4fb3e78808ae3d254454cf1e6c490d6ae8ac7..62258179f3d786b1093ca314d2e9895bf765b7c3 100644
--- a/demos/solo_real_estimation.yaml
+++ b/demos/solo_real_estimation.yaml
@@ -40,6 +40,9 @@ std_bp_drift: 0.005     # small drift
 
 std_pose_p: 0.001
 std_pose_o_deg: 1
+std_mocap_extr_o_deg: 0.1
+std_mocap_extr_p: 10
+unfix_extr_sensor_pose: true
 
 bias_imu_prior: [0,0,0, 0,0,0]
 # bias_imu_prior: [0,0,-0.011,-0.000079251,  0.00439540765, -0.0002120267]
@@ -71,6 +74,9 @@ artificial_bias_p: [0.0, 0.0, 0.0]
 # artificial_bias_p: [3, 6, 4]
 
 
+
+
+
 # Data files
 # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz"
 # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz"
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
index 7e52c950c1179cdd14f38ceb4e734d1a22e1818f..310f973c0346d4da0db46f17f120f376750d50c7 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_real_pov_estimation.cpp
@@ -32,6 +32,9 @@
 #include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
 #include "core/yaml/parser_yaml.h"
 
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
 // IMU
 #include "imu/sensor/sensor_imu.h"
 #include "imu/capture/capture_imu.h"
@@ -107,6 +110,15 @@ int main (int argc, char **argv) {
     Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
     double fz_thresh = config["fz_thresh"].as<double>();
 
+    // MOCAP
+    double std_pose_p = config["std_pose_p"].as<double>();
+    double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+    double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+    double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+    //
+
+    bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+
     std::string data_file_path = config["data_file_path"].as<std::string>();
     std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
 
@@ -114,6 +126,8 @@ int main (int argc, char **argv) {
     unsigned int nbc = ee_names.size();
     // Base to IMU transform
     Quaterniond b_q_i(b_qvec_i);
+    assert(b_q_i.normalized().isApprox(b_q_i));
+    Quaterniond i_q_b = b_q_i.conjugate();
     SE3 b_T_i(b_q_i, b_p_bi);
     SE3 i_T_b = b_T_i.inverse();
     Matrix3d b_R_i =  b_T_i.rotation();
@@ -139,6 +153,9 @@ int main (int argc, char **argv) {
     // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
     cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
     cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+    // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+    // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
     // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
     double* t_arr = t_npyarr.data<double>();
     double* imu_acc_arr = imu_acc_npyarr.data<double>();
@@ -155,6 +172,10 @@ int main (int argc, char **argv) {
     // double* o_R_i_arr = o_R_i_npyarr.data<double>();
     double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
     double* w_q_m_arr = w_q_m_npyarr.data<double>();
+    
+    // double* contact_arr = contact_npyarr.data<double>();
+    // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
     // double* w_R_m_arr = w_R_m_npyarr.data<double>();
     unsigned int N = t_npyarr.shape[0];
     if (max_t > 0){
@@ -184,7 +205,7 @@ int main (int argc, char **argv) {
     ProblemPtr problem = Problem::create("POV", 3);
 
     // add a tree manager for sliding window optimization 
-    ParserYaml parser = ParserYaml("demos/params_tree_manager.yaml", bodydynamics_root_dir);
+    ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
     ParamsServer server = ParamsServer(parser.getParams());
     auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
     problem->setTreeManager(tree_manager);
@@ -212,19 +233,41 @@ int main (int argc, char **argv) {
 
     //===================================================== INITIALIZATION
     // SENSOR + PROCESSOR IMU
-    SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+    SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
     SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
     ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
     ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
 
-    // SENSOR + PROCESSOR POINT FEET NOMOVE
-    ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
-    intr_pfn->std_foot_nomove_ = std_odom3d_est;
-    VectorXd extr;  // default size for dynamic vector is 0-0 -> what we need
-    SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
-    SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
-    ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
-    ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+    // // SENSOR + PROCESSOR POINT FEET NOMOVE
+    // ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+    // intr_pfn->std_foot_nomove_ = std_odom3d_est;
+    // VectorXd extr;  // default size for dynamic vector is 0-0 -> what we need
+    // SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+    // SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+    // ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+    // ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+    // SENSOR + PROCESSOR POSE (for mocap)
+    // pose sensor and proc (to get extrinsics in the prb)
+    auto intr_sp = std::make_shared<ParamsSensorPose>();
+    intr_sp->std_p = std_pose_p;
+    intr_sp->std_o = toRad(std_pose_o_deg);
+    Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+    auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+    auto params_proc = std::make_shared<ParamsProcessorPose>();
+    params_proc->time_tolerance = 0.005;
+    auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+    // somehow by default the sensor extrinsics is fixed
+    Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
+    Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+    sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
+    sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
+    if (unfix_extr_sensor_pose){
+        sensor_pose->unfixExtrinsics();
+    }
+    else{
+        sensor_pose->fixExtrinsics();
+    }
 
 
 
@@ -233,15 +276,14 @@ int main (int argc, char **argv) {
     // - Manually create the first KF and its pose and velocity priors
     // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
 
-    // VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
-    Vector4d q_identity; q_identity << 0,0,0,1;
-    VectorComposite x_origin("POV", {Vector3d::Zero(), q_identity, Vector3d::Zero()});
+    VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
     VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
-    FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+    FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005);  // if mocap used
     
     proc_imu->setOrigin(KF1);
 
-    ////////////////////////////////////////////
+    //////////////////////////////////////////
     // INITIAL BIAS FACTORS
     // Add absolute factor on Imu biases to simulate a fix()
     Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
@@ -249,8 +291,9 @@ int main (int argc, char **argv) {
     CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
     FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
     FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);   
-
     sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+
+
     ///////////////////////////////////////////
     // process measurements in sequential order
     double ts_since_last_kf = 0;
@@ -259,32 +302,27 @@ int main (int argc, char **argv) {
     std::vector<Vector4d> i_qvec_l_vec_prev;
     std::vector<bool> feet_in_contact_prev;
 
-    // for point feet factor
-    std::vector<Vector3d> i_p_il_vec_origin;
-    FrameBasePtr KF_origin = KF1;
-
     //////////////////////////////////////////
     // Vectors to store estimates at the time of data processing 
     // fbk -> feedback: to check if the estimation is stable enough for feedback control
     std::vector<Vector3d> p_ob_fbk_v; 
     std::vector<Vector4d> q_ob_fbk_v; 
     std::vector<Vector3d> v_ob_fbk_v; 
-    double o_p_ob_carr[3*N];
-    double o_q_b_carr[4*N];
-    double o_v_ob_carr[3*N];
-    double imu_bias_carr[6*N];
+    double o_p_ob_fbk_carr[3*N];
+    double o_q_b_fbk_carr[4*N];
+    double o_v_ob_fbk_carr[3*N];
+    double imu_bias_fbk_carr[6*N];
+    double extr_mocap_fbk_carr[7*N];
 
     std::vector<Vector3d> i_omg_oi_v; 
     //////////////////////////////////////////
 
-
-
     unsigned int nb_kf = 1;
     for (unsigned int i=0; i < N; i++){
         TimeStamp ts(t_arr[i]);
 
         ////////////////////////////////////
-        // Retrieve current state 
+        // Retrieve current state
         VectorComposite curr_state = problem->getState();
         Vector4d o_qvec_i_curr = curr_state['O'];
         Quaterniond o_q_i_curr(o_qvec_i_curr); 
@@ -299,51 +337,54 @@ int main (int argc, char **argv) {
         Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
         Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
         Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+        // Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i);  // int conversion does not work!
+        // Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i);  // int conversion does not work!
+        Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+        Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+        w_qvec_wm.normalize();  // not close enough to wolf eps sometimes
 
         // store i_omg_oi for later computation of o_v_ob from o_v_oi
         i_omg_oi_v.push_back(i_omg_oi);
 
-        Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
-        // std::cout << "bias_acc_gyro: " << bias_acc_gyro.transpose() << std::endl;
-        Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.tail<3>());
+        Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+        Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
 
 
         ////////////////////////////////////
         // IMU
         Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
-        // Vector6d acc_gyr_meas; acc_gyr_meas << 0,0,9.806, 0,0,0;
         Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
 
         ////////////////////////////////////
         // Kinematics + forces
-        SE3 o_T_i(o_q_i_curr, Vector3d::Zero());
-        Matrix3d o_R_i = o_T_i.rotation();
-        Matrix3d i_R_o = o_R_i.transpose();
-        Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr; 
-        Vector3d i_acc = imu_acc + i_R_o * gravity();
-        Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr);
-
-        VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
-        VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa;
-        VectorXd ddq(model.nv); ddq.setZero();
-        ddq.head<3>() = i_asp_i;
-        // TODO: add other terms to compute forces (ddqa...)
-
-        // update and retrieve kinematics
-        forwardKinematics(model, data, q);
-        updateFramePlacements(model, data);
-
-        // compute all linear jacobians in feet frames (only for quadruped)
-        MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
-        for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
-            MatrixXd Jee(6, model.nv); Jee.setZero();
-            computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
-            Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
-        }
+        // SE3 o_T_i(o_q_i_curr, Vector3d::Zero());
+        // Matrix3d o_R_i = o_T_i.rotation();
+        // Matrix3d i_R_o = o_R_i.transpose();
+        // Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr; 
+        // Vector3d i_acc = imu_acc + i_R_o * gravity();
+        // Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr);
+
+        // VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
+        // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa;
+        // VectorXd ddq(model.nv); ddq.setZero();
+        // ddq.head<3>() = i_asp_i;
+        // // TODO: add other terms to compute forces (ddqa...)
+
+        // // update and retrieve kinematics
+        // forwardKinematics(model, data, q);
+        // updateFramePlacements(model, data);
+
+        // // compute all linear jacobians in feet frames (only for quadruped)
+        // MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
+        // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+        //     MatrixXd Jee(6, model.nv); Jee.setZero();
+        //     computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
+        //     Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+        // }
 
-        // estimate forces from torques
-        VectorXd tau_cf = rnea(model, data, q, dq, ddq);  // compute total torques applied on the joints (and free flyer)
-        tau_cf.tail(model.nv-6) -= tau;  // remove measured joint torque (not on the free flyer)
+        // // estimate forces from torques
+        // VectorXd tau_cf = rnea(model, data, q, dq, ddq);  // compute total torques applied on the joints (and free flyer)
+        // tau_cf.tail(model.nv-6) -= tau;  // remove measured joint torque (not on the free flyer)
 
         // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat 
         // Solve in Least square sense (3 ways):
@@ -352,187 +393,86 @@ int main (int argc, char **argv) {
         // (A.transpose() * A).ldlt().solve(A.transpose() * b)  // solve the normal equation (twice less accurate than other 2)
 
         // Or else, retrieve from preprocessed dataset
-        Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
-
-        Vector3d o_f_tot = Vector3d::Zero();
-        std::vector<Vector3d> l_f_l_vec;
-        std::vector<Vector3d> o_f_l_vec;
-        std::vector<Vector3d> i_p_il_vec;
-        std::vector<Vector4d> i_qvec_l_vec;
-        // needing relative kinematics measurements
-        VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
-        VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
-        forwardKinematics(model, data, q_static, dq_static);
-        updateFramePlacements(model, data);
-        for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
-            auto b_T_l = data.oMf[ee_ids[i_ee]];
-            auto i_T_l = i_T_b * b_T_l;
-            Vector3d i_p_il = i_T_l.translation();                       // meas
-            Matrix3d i_R_l = i_T_l.rotation();
-            Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();  // meas
-            i_p_il = i_p_il +  i_R_l*l_p_lg;
-
-            Vector3d l_f_l = l_forces.segment(3*i_ee, 3);  // meas
-            Vector3d o_f_l = o_R_i*i_R_l * l_f_l;     // for contact test
-            o_f_tot += o_f_l;
-
-            l_f_l_vec.push_back(l_f_l);
-            o_f_l_vec.push_back(o_f_l);
-            i_p_il_vec.push_back(i_p_il);
-            i_qvec_l_vec.push_back(i_qvec_l);
-        }
-
-        // initialization for point feet factor
-        if (i == 0){
-            i_p_il_vec_origin = i_p_il_vec;
-        }
-        // std::cout << "o_f_tot:       " << o_f_tot.transpose() << std::endl;
-        // std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
+        // Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+        // Vector3d o_f_tot = Vector3d::Zero();
+        // std::vector<Vector3d> l_f_l_vec;
+        // std::vector<Vector3d> o_f_l_vec;
+        // std::vector<Vector3d> i_p_il_vec;
+        // std::vector<Vector4d> i_qvec_l_vec;
+        // // needing relative kinematics measurements
+        // VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+        // VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
+        // forwardKinematics(model, data, q_static, dq_static);
+        // updateFramePlacements(model, data);
+        // // std::cout << "" << std::endl;
+        // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+        //     // std::cout << i_ee << std::endl;
+        //     auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+        //     // overides with value from mocap
+        //     // std::cout << b_T_l.translation().transpose() << std::endl;
+        //     b_T_l.translation(b_p_bl_mocap.segment<3>(3*i_ee));
+        //     // std::cout << b_T_l.translation().transpose() << std::endl;
+
+        //     auto i_T_l = i_T_b * b_T_l;
+        //     Vector3d i_p_il = i_T_l.translation();                       // meas
+        //     Matrix3d i_R_l = i_T_l.rotation();
+        //     Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();  // meas
+        //     i_p_il = i_p_il +  i_R_l*l_p_lg;
+
+        //     Vector3d l_f_l = l_forces.segment(3*i_ee, 3);  // meas
+        //     // Vector3d o_f_l = o_R_i*i_R_l * l_f_l;     // for contact test
+
+        //     l_f_l_vec.push_back(l_f_l);
+        //     // o_f_l_vec.push_back(o_f_l);
+        //     i_p_il_vec.push_back(i_p_il);
+        //     i_qvec_l_vec.push_back(i_qvec_l);
+        // }
 
         CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
         CImu->process();
 
-
-        ////////////////////////////////////////
-        ////////////////////////////////////////
-        ////////////////////////////////////////
-        // Integrate leg odom in 4 separate factors
-        ////////////////////////////////////////
-        // 2) fill the leg odometry data matrix, depending on the contact dimension
-        // forwardKinematics and updateFramePlacements have been called on b centered q and dq
-
-        // gyro 
-        // // .) nothing
-        // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
-        // ..) direct gyro
-        // data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0;
-        // ...) gyro compensated
-        // for (int i_ee=0; i_ee < 4; i_ee++)
-        // {
-        //     Eigen::MatrixXd data_legodom(10,2);            
-        //     // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
-        //     data_legodom.rightCols<1>() << i_omg_oi_unbiased, 0,0,0,0, 0,0,0;
-
-        //     Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee]).linear();
-        //     Vector3d l_v_il = l_v_bl;
-        //     data_legodom.col(0) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_il;
-        //     // data_legodom.col(0) << 0,0,0, 0,0,0,1, 0,0,0;
-
-        //     // 3) process the data and its cov
-        //     Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity(); 
-        //     CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d_lst[i_ee], data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
-        //     CLO->process();
-        // }
-
-        ////////////////////////////////////////
-        ////////////////////////////////////////
-
         // 1) detect feet in contact
-        std::vector<int> feet_in_contact;
-        int nb_feet_in_contact = 0;
-        std::unordered_map<int, Vector3d> kin_incontact;
-        for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
-            // basic contact detection based on normal foot vel, things break if no foot is in contact
-            // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
-            // nb_feet: just for debugging/testing kinematic factor
-            if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
-                feet_in_contact.push_back(i_ee);
-                nb_feet_in_contact++;
-                kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
-            }
-        }
-
-        CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
-        CPF->process();
-
-
-        // // 2) fill the leg odometry data matrix, depending on the contact dimension
-        // // forwardKinematics and updateFramePlacements have been called on b centered q and dq
-
-        // // Option a) Differential kinematics
-        // Eigen::MatrixXd data_legodom(10,feet_in_contact.size()+1);            
-        // // gyro 
-        // // // .) nothing
-        // // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
-        // // ..) direct gyro
-        // data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0;
-        // // ...) gyro compensated
-        // // Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
-        // // data_legodom.rightCols<1>() << (acc_gyr_meas.tail<3>()-bias_acc_gyro.tail<3>()), 0,0,0,0, 0,0,0;
-
-        // unsigned int j = 0;
-        // for (int i_ee_c: feet_in_contact){
-        //     Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee_c]).linear();
-        //     // data_legodom.col(j) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_bl;
-        //     data_legodom.col(j) << 0,0,0, 0,0,0,1, 0,0,0;
-        //     j++;
-        // }
-        // // 3) process the data and its cov
-        // Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity(); 
-        // CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
-        // CLO->process();
-
-
-
-
-        // // Option b) Relative kinematics
-        // if (i > 0){
-        //     // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
-        //     Eigen::MatrixXd data_legodom(3,2*nb_feet_in_contact+1);            
- 
-        //     // data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
-        //     // data_legodom.rightCols<1>() = Vector3d::Zero();
-        //     // Compensate bias
-        //     Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
-        //     data_legodom.rightCols<1>() = (acc_gyr_meas.tail<3>() - bias_acc_gyro.tail<3>());
-
-        //     for (unsigned int j=0; j < feet_in_contact.size(); j++){
-        //             data_legodom.col(2*j  ) << i_p_il_vec_prev[j];
-        //             data_legodom.col(2*j+1) << i_p_il_vec[j];
-        //             // data_legodom.col(2*j  ) << 0,0,0;
-        //             // data_legodom.col(2*j+1) << 0,0,0;
+        // int nb_feet_in_contact = 0;
+        // std::unordered_map<int, Vector3d> kin_incontact;
+        // for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+        //     // basic contact detection based on normal foot vel, things break if no foot is in contact
+        //     // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
+        //     // nb_feet: just for debugging/testing kinematic factor
+
+        //     // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+        //     if (contact_gtr(i_ee)){
+        //         nb_feet_in_contact++;
+        //         kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
         //     }
+        // }
 
-        //     // 3) process the data and its cov
-        //     Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity(); 
-        //     CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
-        //     CLO->process();
+        // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+        // CPF->process();
+        
+        // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+        Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+        CapturePosePtr CP = std::make_shared<CapturePose>(ts, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+        CP->process();
+
+        // ts_since_last_kf += dt;
+        // if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+        //     // problem->print(4,true,true,true);
+        //     auto kf = problem->emplaceFrame(ts);
+        //     problem->keyFrameCallback(kf, nullptr, 0.0005);
+        //     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        //     std::cout << report << std::endl;
+        //     ts_since_last_kf = 0;
+        //     // std::cout << "Extr SPose: " << sensor_pose->getP()->getState().transpose() << " " << sensor_pose->getO()->getState().transpose() << std::endl;
         // }
-        // i_p_il_vec_prev = i_p_il_vec;
-        // i_qvec_l_vec_prev = i_qvec_l_vec;
-
-
-        // add zero vel artificial factor
-        if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
-            auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
-            std::cout << "New KF " << kf_pair->first << std::endl;
-            auto kf = kf_pair->second;
-
-            // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
-            // FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
-            // FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
-            
-            // point feet factor for each foot (short cut to avoid using a processor...)
-            for (int i_ee=0; i_ee < 4; i_ee++)
-            {
-                Vector6d meas; meas << i_p_il_vec_origin[i_ee], i_p_il_vec[i_ee];
-                Matrix3d cov = pow(std_odom3d_est, 2)*Matrix3d::Identity();
-                // CaptureBasePtr cap  = CaptureBase::emplace<CaptureBase>(kf,  "PointFeet", kf_pair->first);
-                // FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "PointFeet", meas, cov);
-                // FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, KF_origin, nullptr, false);
-            }
-            i_p_il_vec_origin = i_p_il_vec;
-
-            nb_kf++;
-            KF_origin = kf;
-        }
 
-        ts_since_last_kf += dt;
-        if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
-            // problem->print(4,true,true,true);
+        // solve every new KF
+        if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
             std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+            std::cout << ts << "  ";
             std::cout << report << std::endl;
-            ts_since_last_kf = 0;
+            nb_kf = problem->getTrajectory()->getFrameMap().size();
         }
 
         // Store current state estimation
@@ -542,18 +482,23 @@ int main (int argc, char **argv) {
         Vector3d o_v_oi = state_fbk['V'];
 
         // IMU to base frame 
-        o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
         Matrix3d o_R_b = o_R_i * i_R_b;
         Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
-        Vector3d o_p_ob = o_p_oi +  o_R_i * i_p_ib;
+        Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
         Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi);
+        // Vector3d o_p_ob = o_p_oi;
+        // Vector3d o_v_ob = o_v_oi;
 
-        Vector6d imu_bias = sen_imu->getIntrinsic()->getState();
+        imu_bias = sen_imu->getIntrinsic()->getState();
+        Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
 
-        mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),   3*sizeof(double));
-        mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
-        mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),   3*sizeof(double));
-        mempcpy(imu_bias_carr+6*i, imu_bias.data(),   6*sizeof(double));
+        mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(),   3*sizeof(double));
+        mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+        mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(),   3*sizeof(double));
+        mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(),   6*sizeof(double));
+        mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(),   7*sizeof(double));
 
         p_ob_fbk_v.push_back(o_p_ob);
         q_ob_fbk_v.push_back(o_qvec_b);
@@ -565,159 +510,40 @@ int main (int argc, char **argv) {
     ///////////////////////////////////////////
     //////////////// SOLVING //////////////////
     ///////////////////////////////////////////
-    problem->print(4,true,true,true);
     if (solve_end){
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
         problem->print(4,true,true,true);
         std::cout << report << std::endl;
     }
 
-    //////////////////////////////////////
-    //////////////////////////////////////
-    //       STORE DATA OFFLINE         //
-    // NOT WORKING WITH SLIDING WINDOW  //
-    //////////////////////////////////////
-    //////////////////////////////////////
-
-    // std::fstream file_est; 
-    // std::fstream file_fbk; 
-    // std::fstream file_kfs; 
-    // std::fstream file_cov; 
-    // file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out); 
-    // file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out); 
-    // file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out); 
-    // file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out); 
-    // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
-    // file_est << header_est;
-    // file_fbk << header_est;
-    // std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz\n";
-    // file_kfs << header_kfs;
-    // std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz\n";
-    // file_cov << header_cov; 
-
-    // VectorComposite state_est;
-    // double o_p_ob_carr[3*N];
-    // double o_q_b_carr[4*N];
-    // double o_v_ob_carr[3*N];
-    // for (unsigned int i=0; i < N; i++) { 
-    //     state_est = problem->getState(t_arr[i]);
-    //     Vector3d i_omg_oi = i_omg_oi_v[i];
-    //     Vector3d o_p_oi = state_est['P'];
-    //     Vector4d o_qvec_i = state_est['O'];
-    //     Vector3d o_v_oi = state_est['V'];
-
-    //     Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
-    //     Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.tail<3>());
-
-    //     Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
-    //     Matrix3d o_R_b = o_R_i * i_R_b;
-    //     Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
-    //     Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
-    //     Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
-
-    //     mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),   3*sizeof(double));
-    //     mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
-    //     mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),   3*sizeof(double));
-
-    //     file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-    //              << t_arr[i] << ","
-    //              << o_p_ob(0) << ","
-    //              << o_p_ob(1) << ","
-    //              << o_p_ob(2) << "," 
-    //              << o_qvec_b(0) << "," 
-    //              << o_qvec_b(1) << "," 
-    //              << o_qvec_b(2) << "," 
-    //              << o_qvec_b(3) << "," 
-    //              << o_v_oi(0) << "," 
-    //              << o_v_oi(1) << "," 
-    //              << o_v_oi(2)
-    //              << "\n"; 
-    // }
-
-    // VectorComposite kf_state;
-    // CaptureBasePtr cap_imu;
-    // 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"; 
-    //     }
-    // }
-
-    // for (unsigned int i=0; i < N; i++) { 
-    //     file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
-    //              << t_arr[i] << ","
-    //              << p_ob_fbk_v[i](0) << ","
-    //              << p_ob_fbk_v[i](1) << ","
-    //              << p_ob_fbk_v[i](2) << "," 
-    //              << q_ob_fbk_v[i](0) << "," 
-    //              << q_ob_fbk_v[i](1) << "," 
-    //              << q_ob_fbk_v[i](2) << "," 
-    //              << q_ob_fbk_v[i](3) << "," 
-    //              << v_ob_fbk_v[i](0) << "," 
-    //              << v_ob_fbk_v[i](1) << "," 
-    //              << v_ob_fbk_v[i](2)
-    //              << "\n";    
-    // }
-
-    // file_est.close();
-    // file_kfs.close();
-    // file_cov.close();
+    double o_p_ob_carr[3*N];
+    double o_q_b_carr[4*N];
+    double o_v_ob_carr[3*N];
+    double imu_bias_carr[6*N];
+    for (unsigned int i=0; i < N; i++) { 
+        VectorComposite state_est = problem->getState(t_arr[i]);
+        Vector3d i_omg_oi = i_omg_oi_v[i];
+        Vector3d o_p_oi = state_est['P'];
+        Vector4d o_qvec_i = state_est['O'];
+        Vector3d o_v_oi = state_est['V'];
+
+        Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+        std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
+        Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+        Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        Matrix3d o_R_b = o_R_i * i_R_b;
+        Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+        Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+        Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
+        // Vector3d o_p_ob = o_p_oi;
+        // Vector3d o_v_ob = o_v_oi;
+
+        mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),   3*sizeof(double));
+        mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
+        mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),   3*sizeof(double));
+        mempcpy(imu_bias_carr+3*i, imu_bias.data(),   3*sizeof(double));
+    }
 
 
     // Save trajectories in npz file
@@ -731,12 +557,16 @@ int main (int argc, char **argv) {
     cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
 
     // save estimates
+    cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_q_b_fbk",  o_q_b_fbk_carr,  {N,4}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
     cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
     cnpy::npz_save(out_npz_file_path, "o_q_b",  o_q_b_carr,  {N,4}, "a");
     cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
     
-    // and biases
-    
-    cnpy::npz_save(out_npz_file_path, "imu_biases", imu_bias_carr, {N,6}, "a");
+    // and biases/extrinsics
+    cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
 
 }
diff --git a/demos/params_tree_manager.yaml b/demos/tree_manager.yaml
similarity index 66%
rename from demos/params_tree_manager.yaml
rename to demos/tree_manager.yaml
index 6355024a0ba3875a0a5981422864e7828eb04f26..ceb0dd77261e7361210a690b2ba563aaa58e7779 100644
--- a/demos/params_tree_manager.yaml
+++ b/demos/tree_manager.yaml
@@ -1,7 +1,7 @@
 config:
   problem:
     tree_manager:
-      fix_first_frame: true
-      n_frames: 5
+      n_fix_first_frames: 3
+      n_frames: 100000000000
       type: TreeManagerSlidingWindow
       viral_remove_empty_parent: true