diff --git a/.gitignore b/.gitignore
index 09710a7b21fdda851139ab38f69dfe0832dab035..88dfba09b20f8c2fba647bf61275d4be289b3098 100644
--- a/.gitignore
+++ b/.gitignore
@@ -41,4 +41,7 @@ compile_commands.json
 .vimspector.json
 est*.csv
 .clang-format
-test/yaml/imu_mocap_fusion/*.csv
+test/yaml/imu_2d_static_init/*.csv
+test/yaml/imu_3d_mocap_fusion/*.csv
+test/yaml/imu_3d_static_init/*.csv
+test/yaml/processor_motion_intrinsics_update/*.csv
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 4ad302cc17068ae8f9471d7d72c105f81a477e5e..0283d709e05c8ee46d06041ddc61a62de32c4950 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -3,6 +3,7 @@ include:
   - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.yaml_schema_cpp.yml'
   - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.clang_format.yml'
   - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.install_core.yml'
+  - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.preliminaries.yml'
 
 workflow:
   rules:
@@ -32,111 +33,9 @@ stages:
 
   ## PRINT VARIABLES
   - *print_variables_definition
-
-  ## Install ssh-agent if not already installed, it is required by Docker.
-  ## (change apt-get to yum if you use an RPM-based image)
-  - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
-
-  ## Run ssh-agent (inside the build environment)
-  - eval $(ssh-agent -s)
-
-  ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
-  ## We're using tr to fix line endings which makes ed25519 keys work
-  ## without extra base64 encoding.
-  ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
-  - mkdir -p ~/.ssh
-  - chmod 700 ~/.ssh  
-  - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
-  # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
-  - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
-
-  # update apt
-  - apt-get update
-
-  # create 'ci_deps' folder (if not exists)
-  - mkdir -pv ci_deps
-
-# .license_header_template: &license_header_definition
-#   - cd $CI_PROJECT_DIR
-
-#   # configure git
-#   - export CI_NEW_BRANCH=ci_processing$RANDOM
-#   - echo creating new temporary branch... $CI_NEW_BRANCH
-#   - git config --global user.email "${CI_EMAIL}"
-#   - git config --global user.name "${CI_USERNAME}"
-#   - git checkout -b $CI_NEW_BRANCH # temporary branch
-
-#   # download license script
-#   - if [ -f /ci_deps/license_manager.sh ]; then
-#   -   echo "File license_manager.sh already exists."
-#   - else
-#   -   echo "Downloading file license_manager.sh..."
-#   -   wget -P /ci_deps  https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/wolf_scripts/license_manager.sh
-#   - fi
-
-#   # license headers
-#   - export CURRENT_YEAR=$( date +'%Y' )
-#   - echo "current year:" ${CURRENT_YEAR}
-#   - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
-#       # add license headers to new files
-#   -   echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
-#   -   source /ci_deps/license_manager.sh --add --path=. --config-path=. --exclude=ci_deps
-#   - else
-#       # remove license headers of all files
-#   -   source /ci_deps/license_manager.sh --remove --path=. --config-path=. --exclude=ci_deps
-#       # update license header
-#   -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
-#   -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
-#   -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
-#   -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
-#       # add new license headers to all files
-#   -   source /ci_deps/license_manager.sh --add --path=. --config-path=. --exclude=ci_deps
-#   - fi
-
-#   # push changes (if any)
-#   - if git commit -a -m "[skip ci] license headers added or modified" ; then
-#   -   git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
-#   -   git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
-#   - else
-#   -   echo "No changes, nothing to commit!"
-#   - fi
-
-# .install_yamlschemacpp_template: &install_yamlschemacpp_definition
-#   - cd ${CI_PROJECT_DIR}/ci_deps
-#   - if [ -d yaml-schema-cpp ]; then
-#   -   echo "directory yaml-schema-cpp exists"
-#   -   cd yaml-schema-cpp
-#   -   git checkout main
-#   -   git pull
-#   - else
-#   -   git clone -b main ssh://git@gitlab.iri.upc.edu:2202/labrobotica/algorithms/yaml-schema-cpp.git
-#   -   cd yaml-schema-cpp
-#   - fi
-#   - mkdir -pv build
-#   - cd build
-#   - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
-#   - make -j$(nproc)
-#   - make install
-#   - ldconfig
-
-# .install_wolf_template: &install_wolf_definition
-#   - cd ${CI_PROJECT_DIR}/ci_deps
-#   - if [ -d wolf ]; then
-#   -   echo "directory wolf exists"
-#   -   cd wolf
-#   -   git checkout devel
-#   -   git pull
-#   -   git checkout $WOLF_CORE_BRANCH
-#   -   git pull
-#   - else
-#   -   git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
-#   -   cd wolf
-#   - fi
-#   - mkdir -pv build
-#   - cd build
-#   - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
-#   - make -j$(nproc)
-#   - make install
+  
+  ## preliminaries script (ssh, apt update and mkdir ci_deps)
+  - !reference [.preliminaries_script]
 
 .build_and_test_template: &build_and_test_definition
   - cd $CI_PROJECT_DIR
diff --git a/schema/processor/ProcessorImu2dTester.schema b/schema/processor/ProcessorImu2dTester.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c3126a91b243d1556d46cd23b49123aaf36afdf9
--- /dev/null
+++ b/schema/processor/ProcessorImu2dTester.schema
@@ -0,0 +1 @@
+follow: ProcessorImu2d.schema
\ No newline at end of file
diff --git a/schema/processor/ProcessorImu3dTester.schema b/schema/processor/ProcessorImu3dTester.schema
new file mode 100644
index 0000000000000000000000000000000000000000..bca5eeedfb6b150944228ee8bae4bb2f3e70c667
--- /dev/null
+++ b/schema/processor/ProcessorImu3dTester.schema
@@ -0,0 +1 @@
+follow: ProcessorImu3d.schema
diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp
index c123587ca68cf498b7cfc2857f71c1014048af06..8fdd36ffd4d0f64d727a37bda48fc04ea149fdca 100644
--- a/src/processor/processor_imu_3d.cpp
+++ b/src/processor/processor_imu_3d.cpp
@@ -134,7 +134,8 @@ void ProcessorImu3d::setCalibration(const CaptureBasePtr _capture, const VectorX
 
 void ProcessorImu3d::configure(SensorBasePtr _sensor)
 {
-    imu_drift_cov_ = _sensor->getDriftCov('I');
+    if (_sensor->isStateBlockDynamic('I'))
+        imu_drift_cov_ = _sensor->getDriftCov('I');
 }
 
 void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d55e5dec9f1c8169e57edc6283af16fe77555733..0ab211adef0f8fd3ebddf0af853b3dc851f3e920 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -31,17 +31,17 @@ wolf_add_gtest(gtest_imu_3d_static_init gtest_imu_3d_static_init.cpp)
 
 wolf_add_gtest(gtest_imu_3d_tools gtest_imu_3d_tools.cpp)
 
-# wolf_add_gtest(gtest_imu_3d gtest_imu_3d.cpp)
+wolf_add_gtest(gtest_imu_3d gtest_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_processor_imu_3d_jacobians gtest_processor_imu_3d_jacobians.cpp)
+wolf_add_gtest(gtest_processor_imu_3d_jacobians gtest_processor_imu_3d_jacobians.cpp)
 
-# wolf_add_gtest(gtest_processor_imu_3d gtest_processor_imu_3d.cpp)
+wolf_add_gtest(gtest_processor_imu_3d gtest_processor_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_processor_imu_2d_with_gravity gtest_processor_imu_2d_with_gravity.cpp)
+wolf_add_gtest(gtest_processor_imu_2d_with_gravity gtest_processor_imu_2d_with_gravity.cpp)
 
-# wolf_add_gtest(gtest_processor_imu_2d gtest_processor_imu_2d.cpp)
+wolf_add_gtest(gtest_processor_imu_2d gtest_processor_imu_2d.cpp)
 
-# wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
+wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
 
 wolf_add_gtest(gtest_schema gtest_schema.cpp)
 
diff --git a/test/gtest_factor_imu_3d.cpp b/test/gtest_factor_imu_3d.cpp
index 0ed5b1b01dd016ae68ba34efb10604e6438641c6..e78f6875819dbb4f81402e1d95c789544a35f32b 100644
--- a/test/gtest_factor_imu_3d.cpp
+++ b/test/gtest_factor_imu_3d.cpp
@@ -26,7 +26,7 @@
 #include "imu/common/imu.h"
 #include "imu/capture/capture_imu.h"
 #include "imu/processor/processor_imu_3d.h"
-#include "imu/sensor/sensor_imu.h"
+#include "imu/sensor_imu/sensor_imu.h"
 
 //Wolf
 #include <core/factor/factor_pose_3d.h>
@@ -55,10 +55,9 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
@@ -73,17 +72,12 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
-        // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
+        // IMU SENSOR & PROCESSOR
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
     
         //===================================================== END{SETTING PROBLEM}
 
@@ -110,7 +104,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
         Vector6d data_imu;
         data_imu << -gravity(), 0,0,0;
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -118,7 +112,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
 
         }
 
@@ -137,10 +131,9 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
@@ -154,17 +147,12 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor("ProcessorImu", sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -188,7 +176,7 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
         data_imu << -gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -196,7 +184,7 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         KF1 = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -214,10 +202,9 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -231,17 +218,12 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        //solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        //solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        //solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -265,7 +247,7 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
         data_imu << -gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on Imu (measure only gravity here)
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov() );//, origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -273,7 +255,7 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -291,10 +273,9 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -308,18 +289,13 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
-    
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -344,7 +320,7 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
         data_imu << -gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero()); //set data on Imu (measure only gravity here)
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero()); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -352,7 +328,7 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -370,10 +346,9 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -387,18 +362,13 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
-    
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         //===================================================== END{SETTING PROBLEM}
 
         //===================================================== INITIALIZATION
@@ -424,7 +394,7 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -432,7 +402,7 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -450,10 +420,9 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -467,18 +436,13 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
-    
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -500,7 +464,7 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -508,7 +472,7 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -526,10 +490,9 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -543,18 +506,13 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
-    
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -582,7 +540,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -594,7 +552,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -612,10 +570,9 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -629,18 +586,13 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
-    
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -674,7 +626,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(t, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -686,7 +638,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
             imu_ptr->setTimeStamp(t);
 
             // process data in capture
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
         }
 
         last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
@@ -706,10 +658,9 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
@@ -723,17 +674,12 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -757,7 +703,7 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
 
         double dt(0.001);
         TimeStamp ts(0);
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
 
         while( ts.get() < 1 )
         {
@@ -776,7 +722,7 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
             imu_ptr->setTimeStamp(ts);
             data_imu = data_imu + origin_bias;
             imu_ptr->setData(data_imu);
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
 
         }
 
@@ -788,8 +734,6 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
         origin_KF->unfix();
         last_KF->unfix();
     }
-
-    virtual void TearDown(){}
 };
 
 class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
@@ -797,11 +741,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
     public:
         TimeStamp t;
         ProblemPtr problem;
-        SolverCeresPtr ceres_manager;
-        SensorBasePtr sensor;
+        SolverManagerPtr solver;
+        SensorBasePtr sensor_imu;
         SensorImu3dPtr sensor_imu;
         SensorOdom3dPtr sensor_odo;
-        ProcessorBasePtr processor;
         ProcessorImuPtr processor_imu;
         ProcessorOdom3dPtr processor_odo;
         FrameBasePtr origin_KF;
@@ -818,34 +761,22 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        ceres_manager = make_shared<SolverCeres>(problem);
-        ceres_manager->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        sensor          = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        sensor_imu      = static_pointer_cast<SensorImu>(sensor);
-        processor       = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        processor_imu   = static_pointer_cast<ProcessorImu>(processor);
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
 
         // SENSOR + PROCESSOR ODOM 3d
-        sensor          = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
-        sensor_odo      = static_pointer_cast<SensorOdom3d>(sensor);
+        sensor_odo = static_pointer_cast<SensorOdom3d>(problem->installSensor(imu_dir + "/test/yaml/sensor_odom_3d.yaml", {imu_dir}));
+        processor_odo = static_pointer_cast<ProcessorOdom3d>(problem->installProcessor(sensor_odo, imu_dir + "/test/yaml/processor_odom_3d.yaml", {imu_dir}));
 
         sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
         sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
         WOLF_TRACE("Imu cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
         WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
 
-        ParamsProcessorOdom3dPtr prc_odom3d_params = make_shared<ParamsProcessorOdom3d>();
-        prc_odom3d_params->max_time_span    = 0.0099;
-        prc_odom3d_params->max_buff_length  = 1000; //make it very high so that this condition will not pass
-        prc_odom3d_params->dist_traveled    = 1000;
-        prc_odom3d_params->angle_turned     = 1000;
-
-        processor       = problem->installProcessor("ODOM 3d", "odom", sensor_odo, prc_odom3d_params);
-        processor_odo   = static_pointer_cast<ProcessorOdom3d>(processor);
-
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -973,11 +904,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         SensorOdom3dPtr sen_odom3d;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor_;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         ProcessorOdom3dPtr processor_odom3d;
         FrameBasePtr origin_KF;
@@ -992,30 +922,17 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor_);
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
 
         // SENSOR + PROCESSOR ODOM 3d
-        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
-        ParamsProcessorOdom3dPtr prc_odom3d_params = make_shared<ParamsProcessorOdom3d>();
-        prc_odom3d_params->max_time_span = 0.9999;
-        prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-        prc_odom3d_params->dist_traveled = 1000000000;
-        prc_odom3d_params->angle_turned = 1000000000;
-
-        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
-        sen_odom3d = static_pointer_cast<SensorOdom3d>(sen1_ptr);
-        processor_odom3d = static_pointer_cast<ProcessorOdom3d>(processor_odom);
-    
+        sensor_odo = static_pointer_cast<SensorOdom3d>(problem->installSensor(imu_dir + "/test/yaml/sensor_odom_3d.yaml", {imu_dir}));
+        processor_odo = static_pointer_cast<ProcessorOdom3d>(problem->installProcessor(sensor_odo, imu_dir + "/test/yaml/processor_odom_3d.yaml", {imu_dir}));
+
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -1043,7 +960,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
 
         double dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(0.0);
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
         CaptureOdom3dPtr mot_ptr = make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, sen_odom3d->getNoiseCov(), nullptr);
         sen_odom3d->process(mot_ptr);
         //first odometry data will be processed at this timestamp
@@ -1067,7 +984,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
             imu_ptr->setTimeStamp(ts);
             data_imu = data_imu + origin_bias;
             imu_ptr->setData(data_imu);
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
 
             D_cor = processor_imu->getLast()->getDeltaCorrected(origin_bias);
 
@@ -1107,11 +1024,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 {
     public:
         TimeStamp t;
-        SensorImu3dPtr sen_imu;
+        SensorImu3dPtr sensor_imu;
         SensorOdom3dPtr sen_odom3d;
         ProblemPtr problem;
-        SolverCeresPtr solver;
-        ProcessorBasePtr processor;
+        SolverManagerPtr solver;
         ProcessorImuPtr processor_imu;
         ProcessorOdom3dPtr processor_odom3d;
         FrameBasePtr origin_KF;
@@ -1127,30 +1043,17 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         // WOLF PROBLEM
         problem = Problem::create("POV", 3);
 
-        // CERES WRAPPER
-        solver = make_shared<SolverCeres>(problem);
-        solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        solver->getSolverOptions().max_line_search_step_contraction = 1e-3;
-        solver->getSolverOptions().max_num_iterations = 1e4;
+        // SOLVER
+        solver = FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
-        processor = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
-        sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
-        processor_imu = static_pointer_cast<ProcessorImu>(processor);
-
+        sensor_imu = static_pointer_cast<SensorImu>(problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir}));
+        processor_imu = static_pointer_cast<ProcessorImu>(problem->installProcessor(sensor_imu, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir}));
+        
         // SENSOR + PROCESSOR ODOM 3d
-        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
-        ParamsProcessorOdom3dPtr prc_odom3d_params = make_shared<ParamsProcessorOdom3d>();
-        prc_odom3d_params->max_time_span = 0.9999;
-        prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-        prc_odom3d_params->dist_traveled = 1000000000;
-        prc_odom3d_params->angle_turned = 1000000000;
-
-        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
-        sen_odom3d = static_pointer_cast<SensorOdom3d>(sen1_ptr);
-        processor_odom3d = static_pointer_cast<ProcessorOdom3d>(processor_odom);
-    
+        sensor_odo = static_pointer_cast<SensorOdom3d>(problem->installSensor(imu_dir + "/test/yaml/sensor_odom_3d.yaml", {imu_dir}));
+        processor_odo = static_pointer_cast<ProcessorOdom3d>(problem->installProcessor(sensor_odo, imu_dir + "/test/yaml/processor_odom_3d.yaml", {imu_dir}));
+
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
 
@@ -1178,7 +1081,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 
         double dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(1.0);
-        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Vector6d::Zero());
+        CaptureImuPtr imu_ptr = make_shared<CaptureImu>(ts, sensor_imu, data_imu, sensor_imu->getNoiseCov(), Vector6d::Zero());
         CaptureOdom3dPtr mot_ptr = make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, nullptr);
         sen_odom3d->process(mot_ptr);
         //first odometry data will be processed at this timestamp
@@ -1204,7 +1107,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
             imu_ptr->setTimeStamp(ts);
             data_imu = data_imu + origin_bias;
             imu_ptr->setData(data_imu);
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
 
             if(ts.get() >= t_odom.get())
             {
@@ -1243,7 +1146,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
             imu_ptr->setTimeStamp(ts);
             data_imu = data_imu + origin_bias;
             imu_ptr->setData(data_imu);
-            sen_imu->process(imu_ptr);
+            sensor_imu->process(imu_ptr);
 
             if(ts.get() >= t_odom.get())
             {
@@ -1281,7 +1184,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     KF0->getP()->fix();
     KF0->getO()->fix();
     KF0->getV()->fix();
-    KF0->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<1,2,3,1,2,3).finished());
+    KF0->getCaptureOf(sensor_imu)->setCalibration((Vector6d()<<1,2,3,1,2,3).finished());
 
     KF1->getP()->setState(expected_final_state.head(3));
     KF1->getO()->setState(expected_final_state.segment(3,4));
@@ -1290,16 +1193,16 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     KF1->getP()->fix();
     KF1->getO()->fix();
     KF1->getV()->fix();
-    KF1->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<-1,-2,-3,-1,-2,-3).finished());
+    KF1->getCaptureOf(sensor_imu)->setCalibration((Vector6d()<<-1,-2,-3,-1,-2,-3).finished());
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 }
 
 TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
@@ -1323,17 +1226,17 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1341,17 +1244,17 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1359,17 +1262,17 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1379,17 +1282,17 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     {
         err = Vector6d::Random() * epsilon_bias*10;
         perturbed_bias = origin_bias + err;
-        KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-        KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+        KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+        KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
     
-        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
     }
 }
 
@@ -1411,11 +1314,11 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 }
 
 TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
@@ -1439,17 +1342,17 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1457,17 +1360,17 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1475,17 +1378,17 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+    ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
 
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+    ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1495,17 +1398,17 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
     {
         err = Vector6d::Random() * epsilon_bias*10;
         perturbed_bias = origin_bias + err;
-        KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-        KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+        KF0->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+        KF1->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
-        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
+        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+        ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
     
-        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
-        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
+        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+        ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sensor_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
     }
 }
 
@@ -1527,8 +1430,8 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 }
 
 TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
@@ -1553,14 +1456,14 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
 
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    last_KF  ->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    last_KF  ->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1568,14 +1471,14 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    last_KF->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1583,14 +1486,14 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
 
     err = Vector6d::Random() * epsilon_bias*10;
     perturbed_bias = origin_bias + err;
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-    last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+    last_KF->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1600,14 +1503,14 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     {
         err = Vector6d::Random() * epsilon_bias*10;
         perturbed_bias = origin_bias + err;
-        origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
-        last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
+        origin_KF->getCaptureOf(sensor_imu)->setCalibration(perturbed_bias);
+        last_KF->getCaptureOf(sensor_imu)->setCalibration(origin_bias);
 
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     }
 }
 
@@ -1639,8 +1542,8 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 
     //Only biases are unfixed
 
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1654,8 +1557,8 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1669,8 +1572,8 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1686,8 +1589,8 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     }
 }
@@ -1708,8 +1611,8 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
 }
 
@@ -1739,8 +1642,8 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1754,8 +1657,8 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1769,8 +1672,8 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1786,7 +1689,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
         
     }
 }
@@ -1807,8 +1710,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
 }
 
@@ -1838,8 +1741,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1853,8 +1756,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1868,8 +1771,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1885,8 +1788,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     }
 }
@@ -1907,8 +1810,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
 }
 
@@ -1938,8 +1841,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -1953,8 +1856,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -1968,8 +1871,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -1985,8 +1888,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     }
 }
@@ -2007,8 +1910,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
 }
 
@@ -2038,8 +1941,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-4")
@@ -2053,8 +1956,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
@@ -2068,8 +1971,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-1")
@@ -2085,8 +1988,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
         report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
-        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+        ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     }
 }
@@ -2107,8 +2010,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
 }
 
@@ -2128,8 +2031,8 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
 //    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 //
 //    //Only biases are unfixed
-//    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
-//    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
+//    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-3)
+//    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-3)
 //
 //}
 
@@ -2148,15 +2051,15 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     //Only biases are unfixed
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
     MatrixXd covX(10,10);
@@ -2193,14 +2096,14 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
 
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*100)
 
@@ -2240,14 +2143,14 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     //solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
 
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
@@ -2269,13 +2172,13 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
     
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
@@ -2299,14 +2202,14 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
 }
@@ -2327,14 +2230,14 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.0001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*10000)
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*10000)
     ASSERT_QUATERNION_VECTOR_APPROX(last_KF->getO()->getState(), expected_final_state.segment(3,4), Constants::EPS*100)
@@ -2355,15 +2258,15 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     
     ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), Constants::EPS*100)
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
@@ -2412,15 +2315,15 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     
     ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), Constants::EPS*100)
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
@@ -2472,15 +2375,15 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
 
     //perturbation of origin bias
     Vector6d random_err(Vector6d::Random() * 0.00001);
-    Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
-    origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
+    Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+    origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
-    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
+    ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     
     ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), Constants::EPS*100)
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
diff --git a/test/gtest_imu_2d_static_init.cpp b/test/gtest_imu_2d_static_init.cpp
index 949efceaf2f8c5d5e5e8b2ba26502be183e3dfdb..201f81c101a080cb8dfccba58dc7bacc3f3b7835 100644
--- a/test/gtest_imu_2d_static_init.cpp
+++ b/test/gtest_imu_2d_static_init.cpp
@@ -37,6 +37,12 @@ using std::make_shared;
 std::string imu_dir         = _WOLF_IMU_CODE_DIR;
 std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
+/**
+ *  SET TO TRUE  TO PRODUCE CSV FILE
+ *  SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE false
+
 class ProcessorImu2dStaticInitTest : public testing::Test
 {
   public:  // These can be accessed in fixtures
@@ -59,7 +65,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
         WOLF_INFO("Doing setup...");
 
         // Wolf problem
-        problem_ = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_imu_2d_static_init.yaml", {imu_dir});
+        problem_ = Problem::autoSetup(imu_dir + "/test/yaml/imu_2d_static_init/problem_2d.yaml", {imu_dir});
         sensor_  = problem_->findSensor("cool sensor imu");
         processor_motion_ = std::static_pointer_cast<ProcessorMotion>(sensor_->getProcessorList().front());
 
@@ -99,12 +105,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
 
 #if WRITE_CSV_FILE
         std::fstream file_est;
-        file_est.open("./est2d-" + test_name + ".csv", std::fstream::out);
-        //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+        file_est.open(imu_dir + "/test/yaml/imu_2d_static_init/est-" + test_name + ".csv", std::fstream::out);
         std::string header_est;
         if (testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
         if (testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
-        // if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+        if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
         if (testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
         file_est << header_est;
 #endif
@@ -241,8 +246,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
 
 #if WRITE_CSV_FILE
         std::fstream file_est;
-        file_est.open("./est2dzeroodom-" + test_name + ".csv", std::fstream::out);
-        //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+        file_est.open(imu_dir + "/test/yaml/imu_2d_static_init/estzeroodom-" + test_name + ".csv", std::fstream::out);
         std::string header_est;
         if (testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
         if (testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
diff --git a/test/gtest_imu_3d.cpp b/test/gtest_imu_3d.cpp
index a2457310349eda5813873632ad1aa41c20ce328b..4212b159f07f563795abba33e469b8872269a9a2 100644
--- a/test/gtest_imu_3d.cpp
+++ b/test/gtest_imu_3d.cpp
@@ -18,1465 +18,1547 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-//Wolf
+// Wolf
 
+#include "imu/common/imu.h"
 #include "imu/processor/processor_imu_3d.h"
 #include "imu/sensor/sensor_imu.h"
 #include "imu/math/imu_3d_tools.h"
-#include "imu/common/imu.h"
 
 #include <core/utils/utils_gtest.h>
-#include <core/common/wolf.h>
 #include <core/sensor/sensor_odom.h>
 #include <core/processor/processor_odom_3d.h>
 #include <core/ceres_wrapper/solver_ceres.h>
-//#include <core/utils/logging.h>
-
 
 // make my life easier
 using namespace Eigen;
 using namespace wolf;
-using std::shared_ptr;
-using std::make_shared;
-using std::static_pointer_cast;
-using std::string;
 
-string wolf_imu_root = _WOLF_IMU_CODE_DIR;
-string wolf_root     = _WOLF_CODE_DIR;
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
 class Process_Factor_Imu : public testing::Test
 {
-    public:
-        // Wolf objects
-        ProblemPtr          problem;
-        SolverCeresPtr      solver;
-        SensorImu3dPtr        sensor_imu;
-        ProcessorImuPtr     processor_imu;
-        CaptureImuPtr       capture_imu;
-        FrameBasePtr        KF_0, KF_1;     // keyframes
-        CaptureBasePtr      C_0 , C_1;      // base captures
-        CaptureMotionPtr    CM_0, CM_1;     // motion captures
-
-        // time
-        TimeStamp           t0, t;
-        double              dt, DT;
-        int                 num_integrations;
+  public:
+    // Wolf objects
+    ProblemPtr        problem;
+    SolverManagerPtr  solver;
+    SensorImu3dPtr    sensor_imu;
+    ProcessorImu3dPtr processor_imu;
+    CaptureImuPtr     capture_imu;
+    FrameBasePtr      KF_0, KF_1;  // keyframes
+    CaptureBasePtr    C_0, C_1;    // base captures
+    CaptureMotionPtr  CM_0, CM_1;  // motion captures
+
+    // time
+    TimeStamp t0, t;
+    double    dt, DT;
+    int       num_integrations;
+
+    // initial state
+    Vector3d             p0, v0;  // initial pos and vel
+    Quaterniond          q0, q;   // initial and current orientations
+    VectorXd             x0;      // initial state
+    Matrix<double, 9, 9> P0;      // initial state covariance
+
+    // bias
+    Vector6d bias_real, bias_preint, bias_null;  // real, pre-int and zero biases.
+    Vector6d bias_0, bias_1;                     // optimized bias at KF's 0 and 1
+
+    // input
+    Matrix<double, 6, Dynamic> motion;  // Motion in Imu frame. Each column is a motion step. If just one column, then
+                                        // the number of steps is defined in num_integrations
+    Matrix<double, 3, Dynamic> a, w;  // True acc and angvel in Imu frame. Each column is a motion step. Used to create
+                                      // motion with `motion << a,w;`
+    Vector6d data;                    // Imu data. It's the motion with gravity and bias. See imu::motion2data().
+
+    // Deltas and states (exact, integrated, corrected, solved, etc)
+    VectorXd D_exact, x1_exact;                  // exact or ground truth
+    VectorXd D_preint_imu, x1_preint_imu;        // preintegrated with imu_3d_tools
+    VectorXd D_corrected_imu, x1_corrected_imu;  // corrected with imu_3d_tools
+    VectorXd D_preint, x1_preint;                // preintegrated with processor_imu
+    VectorXd D_corrected, x1_corrected;          // corrected with processor_imu
+    VectorXd D_optim, x1_optim;                  // optimized using factor_imu
+    VectorXd D_optim_imu, x1_optim_imu;          // corrected with imu_3d_tools using optimized bias
+    VectorXd x0_optim;                           // optimized using factor_imu
+
+    // Trajectory buffer of correction Jacobians
+    std::vector<MatrixXd> Buf_Jac_preint_prc;
+
+    // Trajectory matrices
+    MatrixXd Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
+    MatrixXd Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
+
+    // Delta correction Jacobian and step
+    Matrix<double, 9, 6> J_D_bias;  // Jacobian of pre-integrated delta w
+    Vector9d             step;
+
+    // Flags for fixing/unfixing state blocks
+    bool p0_fixed, q0_fixed, v0_fixed;
+    bool p1_fixed, q1_fixed, v1_fixed;
+
+    VectorXd getDeltaCorrected(CaptureMotionPtr cap, const VectorXd& bias_real)
+    {
+        const auto& motion          = cap->getBuffer().back();
+        VectorXd    step            = motion.jacobian_calib_ * (bias_real - cap->getCalibrationPreint());
+        VectorXd    delta_corrected = imu::plus(motion.delta_integr_, step);
+        return delta_corrected;
+    }
 
-        // initial state
-        Vector3d            p0, v0;                             // initial pos and vel
-        Quaterniond         q0, q;                              // initial and current orientations
-        VectorXd            x0;                                 // initial state
-        Matrix<double,9,9>  P0;                                 // initial state covariance
-        SpecComposite       problem_prior;                      // initial states
-        // VectorComposite     x0c;                                // initial state composite
-        // VectorComposite     s0c;                                // initial sigmas composite
-
-        // bias
-        Vector6d            bias_real, bias_preint, bias_null;  // real, pre-int and zero biases.
-        Vector6d            bias_0, bias_1;                     // optimized bias at KF's 0 and 1
-
-        // input
-        Matrix<double, 6, Dynamic> motion;                      // Motion in Imu frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
-        Matrix<double, 3, Dynamic> a, w;                        // True acc and angvel in Imu frame. Each column is a motion step. Used to create motion with `motion << a,w;`
-        Vector6d            data;                               // Imu data. It's the motion with gravity and bias. See imu::motion2data().
-
-        // Deltas and states (exact, integrated, corrected, solved, etc)
-        VectorXd        D_exact,         x1_exact;          // exact or ground truth
-        VectorXd        D_preint_imu,    x1_preint_imu;     // preintegrated with imu_3d_tools
-        VectorXd        D_corrected_imu, x1_corrected_imu;  // corrected with imu_3d_tools
-        VectorXd        D_preint,        x1_preint;         // preintegrated with processor_imu
-        VectorXd        D_corrected,     x1_corrected;      // corrected with processor_imu
-        VectorXd        D_optim,         x1_optim;          // optimized using factor_imu
-        VectorXd        D_optim_imu,     x1_optim_imu;      // corrected with imu_3d_tools using optimized bias
-        VectorXd                         x0_optim;          // optimized using factor_imu
-
-        // Trajectory buffer of correction Jacobians
-        std::vector<MatrixXd> Buf_Jac_preint_prc;
-
-        // Trajectory matrices
-        MatrixXd Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
-        MatrixXd Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
-
-        // Delta correction Jacobian and step
-        Matrix<double,9,6>  J_D_bias;                           // Jacobian of pre-integrated delta w
-        Vector9d            step;
-
-        // Flags for fixing/unfixing state blocks
-        bool                p0_fixed, q0_fixed, v0_fixed;
-        bool                p1_fixed, q1_fixed, v1_fixed;
-
-        VectorXd getDeltaCorrected(CaptureMotionPtr cap, const VectorXd& bias_real)
-        {
-            const auto& motion = cap->getBuffer().back();
-            VectorXd step = motion.jacobian_calib_ * (bias_real - cap->getCalibrationPreint());
-            VectorXd delta_corrected = imu::plus(motion.delta_integr_, step);
-            return delta_corrected;
-        }
+    void SetUp() override
+    {
+        //===================================== SETTING PROBLEM
+        problem = Problem::autoSetup(imu_dir + "/test/yaml/imu_3d/problem_3d.yaml", {imu_dir});
+
+        // sensor imu
+        sensor_imu = std::static_pointer_cast<SensorImu3d>(problem->findSensor("cool sensor imu"));
 
-        void SetUp( ) override
+        // processor imu
+        processor_imu = std::static_pointer_cast<ProcessorImu3d>(sensor_imu->getProcessorList().front());
+
+        // SOLVER
+        solver = FactorySolverFile::create(
+            "SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
+
+        dt = 0.01;
+
+        // Some initializations
+        bias_null.setZero();
+        x0.resize(10);
+        D_preint.resize(10);
+        D_corrected.resize(10);
+        x1_optim.resize(10);
+        x1_optim_imu.resize(10);
+    }
+
+    /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
+     * Input:
+     *   q: current orientation
+     *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
+     *   bias_real: the real bias of the Imu
+     *   bias_preint: the bias used for Delta pre-integration
+     * Input/output
+     *   Delta: the preintegrated delta
+     *   J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
+     */
+    static void integrateOneStep(const VectorXd&       motion,
+                                 const VectorXd&       bias_real,
+                                 const VectorXd&       bias_preint,
+                                 double                dt,
+                                 Quaterniond&          q_real,
+                                 VectorXd&             Delta,
+                                 Matrix<double, 9, 6>* J_D_b_ptr = nullptr)
+    {
+        VectorXd             delta(10), data(6);
+        VectorXd             Delta_plus(10);
+        Matrix<double, 9, 6> J_d_d, J_D_b, J_d_b;
+        Matrix<double, 9, 9> J_D_D, J_D_d;
+
+        data          = imu::motion2data(motion, q_real, bias_real);
+        q_real        = q_real * exp_q(motion.tail(3) * dt);
+        Vector6d body = data - bias_preint;
+        if (J_D_b_ptr == nullptr)
         {
-            //===================================== SETTING PROBLEM
-            problem = Problem::create("POV", 3);
-
-            // CERES WRAPPER
-            solver = make_shared<SolverCeres>(problem);
-
-            // SENSOR + PROCESSOR Imu
-            SensorBasePtr       sensor = problem->installSensor   ("SensorImu", wolf_imu_root + "/test/yaml/sensor_imu.yaml", {wolf_imu_root, wolf_root});
-            ProcessorBasePtr processor = problem->installProcessor("ProcessorImu", "Main Imu", wolf_imu_root + "/test/yaml/processor_imu.yaml", {wolf_imu_root, wolf_root});
-            sensor_imu    = static_pointer_cast<SensorImu3d> (sensor);
-            processor_imu = static_pointer_cast<ProcessorImu>(processor);
-
-            dt = 0.01;
-            processor_imu->setTimeTolerance(dt/2);
-
-            // Some initializations
-            bias_null   .setZero();
-            x0          .resize(10);
-            D_preint    .resize(10);
-            D_corrected .resize(10);
-            x1_optim    .resize(10);
-            x1_optim_imu.resize(10);
-
-            // x0c = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-            // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-            problem_prior.emplace('P', SpecState("StatePoint3d",    Vector3d::Zero(),                 "factor", Vector3d::Ones()));
-            problem_prior.emplace('O', SpecState("StateQuaternion", Quaterniond::Identity().coeffs(), "factor", Vector3d::Ones()));
-            problem_prior.emplace('V', SpecState("StateVector3d",   Vector3d::Zero(),                 "factor", Vector3d::Ones()));
+            delta      = imu::body2delta(body, dt);
+            Delta_plus = imu::compose(Delta, delta, dt);
         }
-
-        
-        /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
-         * Input:
-         *   q: current orientation
-         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the Imu
-         *   bias_preint: the bias used for Delta pre-integration
-         * Input/output
-         *   Delta: the preintegrated delta
-         *   J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
-         */
-        static void integrateOneStep(const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Quaterniond& q_real, VectorXd& Delta, Matrix<double, 9, 6>* J_D_b_ptr = nullptr)
+        else
         {
-            VectorXd delta(10), data(6);
-            VectorXd Delta_plus(10);
-            Matrix<double, 9, 6> J_d_d, J_D_b, J_d_b;
-            Matrix<double, 9, 9> J_D_D, J_D_d;
-
-            data                = imu::motion2data(motion, q_real, bias_real);
-            q_real              = q_real*exp_q(motion.tail(3)*dt);
-            Vector6d body       = data - bias_preint;
-            if (J_D_b_ptr == nullptr)
-            {
-                delta           = imu::body2delta(body, dt);
-                Delta_plus      = imu::compose(Delta, delta, dt);
-            }
-            else
-            {
-                imu::body2delta(body, dt, delta, J_d_d);
-                imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
-                J_D_b           = *J_D_b_ptr;
-                J_d_b           = - J_d_d;
-                J_D_b           = J_D_D*J_D_b + J_D_d*J_d_b;
-                *J_D_b_ptr      = J_D_b;
-            }
-            Delta               = Delta_plus;
+            imu::body2delta(body, dt, delta, J_d_d);
+            imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
+            J_D_b      = *J_D_b_ptr;
+            J_d_b      = -J_d_d;
+            J_D_b      = J_D_D * J_D_b + J_D_d * J_d_b;
+            *J_D_b_ptr = J_D_b;
         }
+        Delta = Delta_plus;
+    }
 
-        /* Integrate acc and angVel motion, obtain Delta_preintegrated
-         * Input:
-         *   N: number of steps
-         *   q0: initial orientation
-         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the Imu
-         *   bias_preint: the bias used for Delta pre-integration
-         * Output:
-         *   return: the preintegrated delta
-         */
-        static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
+    /* Integrate acc and angVel motion, obtain Delta_preintegrated
+     * Input:
+     *   N: number of steps
+     *   q0: initial orientation
+     *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
+     *   bias_real: the real bias of the Imu
+     *   bias_preint: the bias used for Delta pre-integration
+     * Output:
+     *   return: the preintegrated delta
+     */
+    static VectorXd integrateDelta(int                N,
+                                   const Quaterniond& q0,
+                                   const VectorXd&    motion,
+                                   const VectorXd&    bias_real,
+                                   const VectorXd&    bias_preint,
+                                   double             dt)
+    {
+        VectorXd Delta(10);
+        Delta = imu::identity();
+        Quaterniond q(q0);
+        for (int n = 0; n < N; n++)
         {
-            VectorXd    Delta(10);
-            Delta       = imu::identity();
-            Quaterniond q(q0);
-            for (int n = 0; n < N; n++)
-            {
-                integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
-            }
-            return Delta;
+            integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
         }
+        return Delta;
+    }
+
+    /* Integrate acc and angVel motion, obtain Delta_preintegrated
+     * Input:
+     *   N: number of steps
+     *   q0: initial orientation
+     *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
+     *   bias_real: the real bias of the Imu
+     *   bias_preint: the bias used for Delta pre-integration
+     * Output:
+     *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
+     *   return: the preintegrated delta
+     */
+    static VectorXd integrateDelta(int                   N,
+                                   const Quaterniond&    q0,
+                                   const VectorXd&       motion,
+                                   const VectorXd&       bias_real,
+                                   const VectorXd&       bias_preint,
+                                   double                dt,
+                                   Matrix<double, 9, 6>& J_D_b)
+    {
+        VectorXd    Delta(10);
+        Quaterniond q;
 
-        /* Integrate acc and angVel motion, obtain Delta_preintegrated
-         * Input:
-         *   N: number of steps
-         *   q0: initial orientation
-         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the Imu
-         *   bias_preint: the bias used for Delta pre-integration
-         * Output:
-         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
-         *   return: the preintegrated delta
-         */
-        static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
+        Delta = imu::identity();
+        J_D_b.setZero();
+        q = q0;
+        for (int n = 0; n < N; n++)
         {
-            VectorXd    Delta(10);
-            Quaterniond q;
-
-            Delta   = imu::identity();
-            J_D_b   .setZero();
-            q       = q0;
-            for (int n = 0; n < N; n++)
-            {
-                integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta, &J_D_b);
-            }
-            return Delta;
+            integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta, &J_D_b);
         }
+        return Delta;
+    }
 
-        /* Integrate acc and angVel motion, obtain Delta_preintegrated
-         * Input:
-         *   q0: initial orientation
-         *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
-         *   bias_real: the real bias of the Imu
-         *   bias_preint: the bias used for Delta pre-integration
-         * Output:
-         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
-         *   return: the preintegrated delta
-         */
-        static VectorXd integrateDelta(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
+    /* Integrate acc and angVel motion, obtain Delta_preintegrated
+     * Input:
+     *   q0: initial orientation
+     *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
+     *   bias_real: the real bias of the Imu
+     *   bias_preint: the bias used for Delta pre-integration
+     * Output:
+     *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
+     *   return: the preintegrated delta
+     */
+    static VectorXd integrateDelta(const Quaterniond&    q0,
+                                   const MatrixXd&       motion,
+                                   const VectorXd&       bias_real,
+                                   const VectorXd&       bias_preint,
+                                   double                dt,
+                                   Matrix<double, 9, 6>& J_D_b)
+    {
+        VectorXd    Delta(10);
+        Quaterniond q;
+
+        Delta = imu::identity();
+        J_D_b.setZero();
+        q = q0;
+        for (int n = 0; n < motion.cols(); n++)
         {
-            VectorXd    Delta(10);
-            Quaterniond q;
-
-            Delta   = imu::identity();
-            J_D_b   .setZero();
-            q       = q0;
-            for (int n = 0; n < motion.cols(); n++)
-            {
-                integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
-            }
-            return Delta;
+            integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
         }
+        return Delta;
+    }
 
-        /* Integrate acc and angVel motion, obtain Delta_preintegrated
-         * Input:
-         *   q0: initial orientation
-         *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
-         *   bias_real: the real bias of the Imu
-         *   bias_preint: the bias used for Delta pre-integration
-         * Output:
-         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
-         *   return: the preintegrated delta
-         */
-        static MotionBuffer integrateDeltaTrajectory(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
+    /* Integrate acc and angVel motion, obtain Delta_preintegrated
+     * Input:
+     *   q0: initial orientation
+     *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
+     *   bias_real: the real bias of the Imu
+     *   bias_preint: the bias used for Delta pre-integration
+     * Output:
+     *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
+     *   return: the preintegrated delta
+     */
+    static MotionBuffer integrateDeltaTrajectory(const Quaterniond&    q0,
+                                                 const MatrixXd&       motion,
+                                                 const VectorXd&       bias_real,
+                                                 const VectorXd&       bias_preint,
+                                                 double                dt,
+                                                 Matrix<double, 9, 6>& J_D_b)
+    {
+        MotionBuffer trajectory;
+        VectorXd     Delta(10);
+        MatrixXd     M9(9, 9), M6(6, 6), J9(9, 9), J96(9, 6), V10(10, 1), V6(6, 1);
+        Quaterniond  q;
+
+        Delta = imu::identity();
+        J_D_b.setZero();
+        q = q0;
+        TimeStamp t(0);
+        trajectory.emplace_back(
+            t, Vector6d::Zero(), M6, VectorXd::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXd::Zero(9, 6));
+        for (int n = 0; n < motion.cols(); n++)
         {
-            MotionBuffer trajectory;
-            VectorXd    Delta(10);
-            MatrixXd    M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
-            Quaterniond q;
-
-            Delta   = imu::identity();
-            J_D_b   .setZero();
-            q       = q0;
-            TimeStamp t(0);
-            trajectory.emplace_back(t, Vector6d::Zero(), M6, VectorXd::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXd::Zero(9,6));
-            for (int n = 0; n < motion.cols(); n++)
-            {
-                t += dt;
-                integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
-                trajectory.emplace_back(t, motion.col(n), M6, V10, M9, Delta, M9, J9, J9, J_D_b);
-            }
-            return trajectory;
+            t += dt;
+            integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
+            trajectory.emplace_back(t, motion.col(n), M6, V10, M9, Delta, M9, J9, J9, J_D_b);
         }
+        return trajectory;
+    }
 
-        MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaterniond q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, VectorXd& D_preint, VectorXd& D_corrected)
+    MotionBuffer integrateWithProcessor(int               N,
+                                        const TimeStamp&  t0,
+                                        const Quaterniond q0,
+                                        const MatrixXd&   motion,
+                                        const VectorXd&   bias_real,
+                                        const VectorXd&   bias_preint,
+                                        double            dt,
+                                        VectorXd&         D_preint,
+                                        VectorXd&         D_corrected)
+    {
+        Vector6d motion_now;
+        data        = imu::motion2data(motion.col(0), q0, bias_real);
+        capture_imu = std::make_shared<CaptureImu>(t0, sensor_imu, data, sensor_imu->computeNoiseCov(data));
+        q           = q0;
+        t           = t0;
+        for (int i = 0; i < N; i++)
         {
-            Vector6d      motion_now;
-            data        = imu::motion2data(motion.col(0), q0, bias_real);
-            capture_imu = make_shared<CaptureImu>(t0, sensor_imu, data, sensor_imu->computeNoiseCov(data));
-            q           = q0;
-            t           = t0;
-            for (int i= 0; i < N; i++)
-            {
-                t   += dt;
-                motion_now = motion.cols() == 1
-                                ? motion
-                                : motion.col(i);
-                data = imu::motion2data(motion_now, q, bias_real);
-                w    = motion_now.tail<3>();
-                q    = q * exp_q(w*dt);
-
-                capture_imu->setTimeStamp(t);
-                capture_imu->setData(data);
-
-                sensor_imu->process(capture_imu);
-
-                D_preint    = processor_imu->getLast()->getDeltaPreint();
-                D_corrected = getDeltaCorrected(processor_imu->getLast(), bias_real);
-            }
-            return processor_imu->getBuffer();
+            t += dt;
+            motion_now = motion.cols() == 1 ? motion : motion.col(i);
+            data       = imu::motion2data(motion_now, q, bias_real);
+            w          = motion_now.tail<3>();
+            q          = q * exp_q(w * dt);
+
+            capture_imu->setTimeStamp(t);
+            capture_imu->setData(data);
+
+            sensor_imu->process(capture_imu);
+
+            D_preint    = processor_imu->getLast()->getDeltaPreint();
+            D_corrected = getDeltaCorrected(processor_imu->getLast(), bias_real);
         }
+        return processor_imu->getBuffer();
+    }
+
+    // Initial configuration of variables
+    virtual bool configureAll()
+    {
+        // initial state
+        q0.normalize();
+        x0 << p0, q0.coeffs(), v0;
+        P0.setIdentity() * 0.01;
 
-        // Initial configuration of variables
-        virtual bool configureAll()
+        // motion
+        if (motion.cols() == 0)
         {
-            // initial state
-            q0      .normalize();
-            x0     << p0, q0.coeffs(), v0;
-            P0      .setIdentity() * 0.01;
-
-            // motion
-            if (motion.cols() == 0)
-            {
-                motion.resize(6,a.cols());
-                motion << a, w;
-            }
-            else
-            {
-                // if motion has any column at all, then it is already initialized in TEST_F(...) and we do nothing.
-            }
-            if (motion.cols() != 1)
-            {
-                // if motion has more than 1 col, make num_integrations consistent with nbr of cols, just for consistency
-                num_integrations = motion.cols();
-            }
-
-            // total run time
-            DT      = num_integrations * dt;
-
-            // wolf objects
-            // WOLF_INFO("x0c: ", x0c);
-            // WOLF_INFO("s0c: ", s0c);
-            // KF_0    = problem->setPriorFactor(x0c, s0c, t0);
-            WOLF_INFO("problem prior: ", problem_prior)
-            KF_0    = problem->setPrior(problem_prior, t0);
-            processor_imu->setOrigin(KF_0);
-            WOLF_INFO("prior set");
-
-            C_0     = processor_imu->getOrigin();
-
-            processor_imu->getLast()->setCalibrationPreint(bias_preint);
-
-            return true;
+            motion.resize(6, a.cols());
+            motion << a, w;
         }
-
-        // Integrate using all methods
-        virtual void integrateAll()
+        else
         {
-            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
-            if (motion.cols() == 1)
-                D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
-            else
-                D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
-            x1_exact = imu::composeOverState(x0, D_exact, DT );
-
-            // ===================================== INTEGRATE USING Imu_TOOLS
-            // pre-integrate
-            if (motion.cols() == 1)
-                D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
-            else
-                D_preint_imu = integrateDelta(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
-            // correct perturbated
-            step             = J_D_bias * (bias_real - bias_preint);
-            D_corrected_imu  = imu::plus(D_preint_imu, step);
-
-            // compose states
-            x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
-            x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
-            // ===================================== INTEGRATE USING PROCESSOR_Imu
-
-            problem->print(3,0,1,0);
-            integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
-            // compose states
-            x1_preint        = imu::composeOverState(x0, D_preint        , DT );
-            x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
+            // if motion has any column at all, then it is already initialized in TEST_F(...) and we do nothing.
         }
-
-        // Integrate Trajectories all methods
-        virtual void integrateAllTrajectories()
+        if (motion.cols() != 1)
         {
-            SizeEigen cols = motion.cols() + 1;
-            Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
-            Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
-
-            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
-            MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
-
-            // Build exact trajectories
-            int col = 0;
-            double Dt = 0;
-            for (auto m : Buf_exact)
-            {
-                Trj_D_exact.col(col) = m.delta_integr_;
-                Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
-                Dt += dt;
-                col ++;
-            }
-
-            // set
-            D_exact          = Trj_D_exact.col(cols-1);
-            x1_exact         = Trj_x_exact.col(cols-1);
-
-            // ===================================== INTEGRATE USING Imu_TOOLS
-            // pre-integrate
-            MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
-            // Build trajectories preintegrated and corrected with imu_3d_tools
-            col = 0;
-            Dt = 0;
-            for (auto m : Buf_preint_imu)
-            {
-                // preint
-                Trj_D_preint_imu.col(col) = m.delta_integr_;
-                Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
-
-                // corrected
-                VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
-                Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
-                Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
-                Dt += dt;
-                col ++;
-            }
-
-            D_preint_imu     = Trj_D_preint_imu.col(cols-1);
-
-            // correct perturbated
-            step             = J_D_bias * (bias_real - bias_preint);
-            D_corrected_imu  = imu::plus(D_preint_imu, step);
-
-            // compose states
-            x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
-            x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
-            // ===================================== INTEGRATE USING PROCESSOR_Imu
-
-            MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
-            // Build trajectories preintegrated and corrected with processorImu
-            Dt = 0;
-            col = 0;
-            Buf_Jac_preint_prc.clear();
-            for (auto m : Buf_D_preint_prc)
-            {
-                // preint
-                Trj_D_preint_prc.col(col) = m.delta_integr_;
-                Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
-
-                // corrected
-                VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
-                Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
-                Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
-
-                Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
-                Dt += dt;
-                col ++;
-            }
-
-            // compose states
-            x1_preint        = imu::composeOverState(x0, D_preint        , DT );
-            x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
+            // if motion has more than 1 col, make num_integrations consistent with nbr of cols, just for consistency
+            num_integrations = motion.cols();
         }
 
-        // Set state_blocks status
-        void setFixedBlocks()
+        // total run time
+        DT = num_integrations * dt;
+
+        // firt frame and processor origin
+        KF_0 = problem->applyPriorOptions(t0);
+        processor_imu->setOrigin(KF_0);
+
+        C_0 = processor_imu->getOrigin();
+
+        processor_imu->getLast()->setCalibrationPreint(bias_preint);
+
+        return true;
+    }
+
+    // Integrate using all methods
+    virtual void integrateAll()
+    {
+        // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
+        if (motion.cols() == 1)
+            D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
+        else
+            D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
+        x1_exact = imu::composeOverState(x0, D_exact, DT);
+
+        // ===================================== INTEGRATE USING Imu_TOOLS
+        // pre-integrate
+        if (motion.cols() == 1)
+            D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
+        else
+            D_preint_imu = integrateDelta(q0, motion, bias_real, bias_preint, dt, J_D_bias);
+
+        // correct perturbated
+        step            = J_D_bias * (bias_real - bias_preint);
+        D_corrected_imu = imu::plus(D_preint_imu, step);
+
+        // compose states
+        x1_preint_imu    = imu::composeOverState(x0, D_preint_imu, DT);
+        x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu, DT);
+
+        // ===================================== INTEGRATE USING PROCESSOR_Imu
+
+        problem->print(3, 0, 1, 0);
+        integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
+
+        // compose states
+        x1_preint    = imu::composeOverState(x0, D_preint, DT);
+        x1_corrected = imu::composeOverState(x0, D_corrected, DT);
+    }
+
+    // Integrate Trajectories all methods
+    virtual void integrateAllTrajectories()
+    {
+        SizeEigen cols = motion.cols() + 1;
+        Trj_D_exact.resize(10, cols);
+        Trj_D_preint_imu.resize(10, cols);
+        Trj_D_preint_prc.resize(10, cols);
+        Trj_D_corrected_imu.resize(10, cols);
+        Trj_D_corrected_prc.resize(10, cols);
+        Trj_x_exact.resize(10, cols);
+        Trj_x_preint_imu.resize(10, cols);
+        Trj_x_preint_prc.resize(10, cols);
+        Trj_x_corrected_imu.resize(10, cols);
+        Trj_x_corrected_prc.resize(10, cols);
+
+        // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
+        MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
+
+        // Build exact trajectories
+        int    col = 0;
+        double Dt  = 0;
+        for (auto m : Buf_exact)
         {
-            // this sets each state block status fixed / unfixed
-            KF_0->getP()->setFixed(p0_fixed);
-            KF_0->getO()->setFixed(q0_fixed);
-            KF_0->getV()->setFixed(v0_fixed);
-            KF_1->getP()->setFixed(p1_fixed);
-            KF_1->getO()->setFixed(q1_fixed);
-            KF_1->getV()->setFixed(v1_fixed);
+            Trj_D_exact.col(col) = m.delta_integr_;
+            Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt);
+            Dt += dt;
+            col++;
         }
 
-        void setKfStates()
+        // set
+        D_exact  = Trj_D_exact.col(cols - 1);
+        x1_exact = Trj_x_exact.col(cols - 1);
+
+        // ===================================== INTEGRATE USING Imu_TOOLS
+        // pre-integrate
+        MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
+
+        // Build trajectories preintegrated and corrected with imu_3d_tools
+        col = 0;
+        Dt  = 0;
+        for (auto m : Buf_preint_imu)
         {
-            // This perturbs states to estimate around the exact value, then assigns to the keyframe
-            // Perturbations are applied only if the state block is unfixed
-            KF_0->setState(x0,"POV");
-            KF_0->perturb();
-            KF_1->setState(x1_exact,"POV");
-            KF_1->perturb();
+            // preint
+            Trj_D_preint_imu.col(col) = m.delta_integr_;
+            Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt);
+
+            // corrected
+            VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
+            Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step);
+            Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt);
+            Dt += dt;
+            col++;
         }
 
-        virtual void buildProblem()
-        {
-            // ===================================== SET KF in Wolf tree
-            FrameBasePtr KF = problem->emplaceFrame(t, "POV", x1_exact);
+        D_preint_imu = Trj_D_preint_imu.col(cols - 1);
 
-            // ===================================== Imu CALLBACK
-            problem->keyFrameCallback(KF, nullptr);
+        // correct perturbated
+        step            = J_D_bias * (bias_real - bias_preint);
+        D_corrected_imu = imu::plus(D_preint_imu, step);
 
-            // Process Imu for the callback to take effect
-            data = Vector6d::Zero();
-            capture_imu = make_shared<CaptureImu>(t+dt, sensor_imu, data, sensor_imu->computeNoiseCov(data));
-            sensor_imu->process(capture_imu);
+        // compose states
+        x1_preint_imu    = imu::composeOverState(x0, D_preint_imu, DT);
+        x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu, DT);
 
-            KF_1 = problem->getLastFrame();
-            C_1  = KF_1->getCaptureList().front(); // front is Imu
-            CM_1 = static_pointer_cast<CaptureMotion>(C_1);
+        // ===================================== INTEGRATE USING PROCESSOR_Imu
 
-            // ===================================== SET BOUNDARY CONDITIONS
-            setFixedBlocks();
-            setKfStates();
-        }
+        MotionBuffer Buf_D_preint_prc = integrateWithProcessor(
+            num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
 
-        string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
+        // Build trajectories preintegrated and corrected with processorImu
+        Dt  = 0;
+        col = 0;
+        Buf_Jac_preint_prc.clear();
+        for (auto m : Buf_D_preint_prc)
         {
-            string report   = solver->solve(verbose);
+            // preint
+            Trj_D_preint_prc.col(col) = m.delta_integr_;
+            Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt);
+
+            // corrected
+            VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
+            Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step);
+            Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt);
+
+            Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
+            Dt += dt;
+            col++;
+        }
 
-            bias_0          = C_0->getSensorIntrinsic()->getState();
-            bias_1          = C_1->getSensorIntrinsic()->getState();
+        // compose states
+        x1_preint    = imu::composeOverState(x0, D_preint, DT);
+        x1_corrected = imu::composeOverState(x0, D_corrected, DT);
+    }
 
-            // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
-            // with processor
-            x0_optim        = KF_0->getStateVector("POV");
-            D_optim         = getDeltaCorrected(CM_1, bias_0);
-            x1_optim        = KF_1->getStateVector("POV");
+    // Set state_blocks status
+    void setFixedBlocks()
+    {
+        // this sets each state block status fixed / unfixed
+        KF_0->getP()->setFixed(p0_fixed);
+        KF_0->getO()->setFixed(q0_fixed);
+        KF_0->getV()->setFixed(v0_fixed);
+        KF_1->getP()->setFixed(p1_fixed);
+        KF_1->getO()->setFixed(q1_fixed);
+        KF_1->getV()->setFixed(v1_fixed);
+    }
 
-            // with imu_3d_tools
-            step            = J_D_bias * (bias_0 - bias_preint);
-            D_optim_imu     = imu::plus(D_preint, step);
-            x1_optim_imu    = imu::composeOverState(x0_optim, D_optim_imu, DT);
+    void setKfStates()
+    {
+        // This perturbs states to estimate around the exact value, then assigns to the keyframe
+        // Perturbations are applied only if the state block is unfixed
+        KF_0->setState(x0, "POV");
+        KF_0->perturb();
+        KF_1->setState(x1_exact, "POV");
+        KF_1->perturb();
+    }
 
-            return report;
-        }
+    virtual void buildProblem()
+    {
+        // ===================================== SET KF in Wolf tree
+        FrameBasePtr KF = problem->emplaceFrame(t, "POV", x1_exact);
 
-        string runAll(SolverManager::ReportVerbosity verbose)
-        {
-            configureAll();
-            integrateAll();
-            buildProblem();
-            string report = solveProblem(verbose);
-            return report;
-        }
+        // ===================================== Imu CALLBACK
+        problem->keyFrameCallback(KF, nullptr);
 
-        void printAll(std::string report = "")
-        {
-            WOLF_TRACE(report);
-
-            WOLF_TRACE("D_exact       : ", D_exact            .transpose() ); // exact delta integrated, with absolutely no bias
-            WOLF_TRACE("D_preint      : ", D_preint           .transpose() ); // pre-integrated delta using processor
-            WOLF_TRACE("D_preint_imu  : ", D_preint_imu       .transpose() ); // pre-integrated delta using imu_3d_tools
-            WOLF_TRACE("D_correct     : ", D_corrected        .transpose() ); // corrected delta using processor
-            WOLF_TRACE("D_correct_imu : ", D_corrected_imu    .transpose() ); // corrected delta using imu_3d_tools
-            WOLF_TRACE("D_optim       : ", D_optim            .transpose() ); // corrected delta using optimum bias after solving using processor
-            WOLF_TRACE("D_optim_imu   : ", D_optim_imu        .transpose() ); // corrected delta using optimum bias after solving using imu_3d_tools
-
-            WOLF_TRACE("bias real     : ", bias_real          .transpose() ); // real bias
-            WOLF_TRACE("bias preint   : ", bias_preint        .transpose() ); // bias used during pre-integration
-            WOLF_TRACE("bias optim 0  : ", bias_0             .transpose() ); // solved bias at KF_0
-            WOLF_TRACE("bias optim 1  : ", bias_1             .transpose() ); // solved bias at KF_1
-
-            // states at the end of integration, i.e., at KF_1
-            WOLF_TRACE("X1_exact      : ", x1_exact           .transpose() ); // exact state
-            WOLF_TRACE("X1_preint     : ", x1_preint          .transpose() ); // state using delta preintegrated by processor
-            WOLF_TRACE("X1_preint_imu : ", x1_preint_imu      .transpose() ); // state using delta preintegrated by imu_3d_tools
-            WOLF_TRACE("X1_correct    : ", x1_corrected       .transpose() ); // corrected state vy processor
-            WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu   .transpose() ); // corrected state by imu_3d_tools
-            WOLF_TRACE("X1_optim      : ", x1_optim           .transpose() ); // optimal state after solving using processor
-            WOLF_TRACE("X1_optim_imu  : ", x1_optim_imu       .transpose() ); // optimal state after solving using imu_3d_tools
-            WOLF_TRACE("err1_optim    : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_3d_tools w.r.t. exact state
-            WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_3d_tools w.r.t. exact state
-            WOLF_TRACE("X0_optim      : ", x0_optim           .transpose() ); // optimal state after solving using processor
-        }
+        // Process Imu for the callback to take effect
+        data        = Vector6d::Zero();
+        capture_imu = std::make_shared<CaptureImu>(t + dt, sensor_imu, data, sensor_imu->computeNoiseCov(data));
+        sensor_imu->process(capture_imu);
 
-        virtual void assertAll()
-        {
-            // check delta and state integrals
-            ASSERT_MATRIX_APPROX(D_preint           , D_preint_imu      , 1e-8 );
-            ASSERT_MATRIX_APPROX(D_corrected        , D_corrected_imu   , 1e-8 );
-            ASSERT_MATRIX_APPROX(D_corrected_imu    , D_exact           , 1e-5 );
-            ASSERT_MATRIX_APPROX(D_corrected        , D_exact           , 1e-5 );
-            ASSERT_MATRIX_APPROX(x1_corrected_imu   , x1_exact          , 1e-5 );
-            ASSERT_MATRIX_APPROX(x1_corrected       , x1_exact          , 1e-5 );
-
-            // check optimal solutions
-            ASSERT_MATRIX_APPROX(x0_optim           , x0        , 1e-5 );
-            ASSERT_NEAR(x0_optim.segment(3,4).norm(), 1.0       , 1e-8 );
-            ASSERT_MATRIX_APPROX(bias_0             , bias_real , 1e-4 );
-            ASSERT_MATRIX_APPROX(bias_1             , bias_real , 1e-4 );
-            ASSERT_MATRIX_APPROX(D_optim            , D_exact   , 1e-5 );
-            ASSERT_MATRIX_APPROX(x1_optim           , x1_exact  , 1e-5 );
-            ASSERT_MATRIX_APPROX(D_optim_imu        , D_exact   , 1e-5 );
-            ASSERT_MATRIX_APPROX(x1_optim_imu       , x1_exact  , 1e-5 );
-            ASSERT_NEAR(x1_optim.segment(3,4).norm(), 1.0       , 1e-8 );
-        }
+        KF_1 = problem->getLastFrame();
+        C_1  = KF_1->getCaptureList().front();  // front is Imu
+        CM_1 = std::static_pointer_cast<CaptureMotion>(C_1);
 
-};
+        // ===================================== SET BOUNDARY CONDITIONS
+        setFixedBlocks();
+        setKfStates();
+    }
 
-class Process_Factor_Imu_ODO : public Process_Factor_Imu
-{
-    public:
-        // Wolf objects
-        SensorOdom3dPtr     sensor_odo;
-        ProcessorOdom3dPtr  processor_odo;
-        CaptureOdom3dPtr    capture_odo;
+    std::string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
+    {
+        std::string report = solver->solve(verbose);
 
-        void SetUp( ) override
-        {
+        bias_0 = C_0->getSensorIntrinsic()->getState();
+        bias_1 = C_1->getSensorIntrinsic()->getState();
 
-            // ===================================== Imu
-            Process_Factor_Imu::SetUp();
+        // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
+        // with processor
+        x0_optim = KF_0->getStateVector("POV");
+        D_optim  = getDeltaCorrected(CM_1, bias_0);
+        x1_optim = KF_1->getStateVector("POV");
 
-            // ===================================== ODO
-            SensorBasePtr    sensor     = problem->installSensor   ("SensorOdom", wolf_imu_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_imu_root});
-            ProcessorBasePtr processor  = problem->installProcessor("ProcessorOdom3d", "Odometer", wolf_imu_root + "/test/yaml/processor_odom_3d.yaml", {wolf_imu_root});
+        // with imu_3d_tools
+        step         = J_D_bias * (bias_0 - bias_preint);
+        D_optim_imu  = imu::plus(D_preint, step);
+        x1_optim_imu = imu::composeOverState(x0_optim, D_optim_imu, DT);
 
-            sensor_odo      = static_pointer_cast<SensorOdom3d>(sensor);
+        return report;
+    }
 
-            processor_odo   = static_pointer_cast<ProcessorOdom3d>(processor);
-            processor_odo->setTimeTolerance(dt/2);
-            processor_odo->setVotingActive(false);
-        }
+    std::string runAll(SolverManager::ReportVerbosity verbose)
+    {
+        configureAll();
+        integrateAll();
+        buildProblem();
+        std::string report = solveProblem(verbose);
+        return report;
+    }
 
-        // Initial configuration of variables
-        bool configureAll() override
-        {
-            // ===================================== Imu
-            Process_Factor_Imu::configureAll();
+    void printAll(std::string report = "")
+    {
+        WOLF_TRACE(report);
+
+        WOLF_TRACE("D_exact       : ", D_exact.transpose());       // exact delta integrated, with absolutely no bias
+        WOLF_TRACE("D_preint      : ", D_preint.transpose());      // pre-integrated delta using processor
+        WOLF_TRACE("D_preint_imu  : ", D_preint_imu.transpose());  // pre-integrated delta using imu_3d_tools
+        WOLF_TRACE("D_correct     : ", D_corrected.transpose());   // corrected delta using processor
+        WOLF_TRACE("D_correct_imu : ", D_corrected_imu.transpose());  // corrected delta using imu_3d_tools
+        WOLF_TRACE("D_optim       : ",
+                   D_optim.transpose());      // corrected delta using optimum bias after solving using processor
+        WOLF_TRACE("D_optim_imu   : ",
+                   D_optim_imu.transpose());  // corrected delta using optimum bias after solving using imu_3d_tools
+
+        WOLF_TRACE("bias real     : ", bias_real.transpose());    // real bias
+        WOLF_TRACE("bias preint   : ", bias_preint.transpose());  // bias used during pre-integration
+        WOLF_TRACE("bias optim 0  : ", bias_0.transpose());       // solved bias at KF_0
+        WOLF_TRACE("bias optim 1  : ", bias_1.transpose());       // solved bias at KF_1
+
+        // states at the end of integration, i.e., at KF_1
+        WOLF_TRACE("X1_exact      : ", x1_exact.transpose());       // exact state
+        WOLF_TRACE("X1_preint     : ", x1_preint.transpose());      // state using delta preintegrated by processor
+        WOLF_TRACE("X1_preint_imu : ", x1_preint_imu.transpose());  // state using delta preintegrated by imu_3d_tools
+        WOLF_TRACE("X1_correct    : ", x1_corrected.transpose());   // corrected state vy processor
+        WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu.transpose());  // corrected state by imu_3d_tools
+        WOLF_TRACE("X1_optim      : ", x1_optim.transpose());          // optimal state after solving using processor
+        WOLF_TRACE("X1_optim_imu  : ", x1_optim_imu.transpose());  // optimal state after solving using imu_3d_tools
+        WOLF_TRACE(
+            "err1_optim    : ",
+            (x1_optim - x1_exact).transpose());  // error of optimal state corrected by imu_3d_tools w.r.t. exact state
+        WOLF_TRACE("err1_optim_imu: ",
+                   (x1_optim_imu - x1_exact)
+                       .transpose());  // error of optimal state corrected by imu_3d_tools w.r.t. exact state
+        WOLF_TRACE("X0_optim      : ", x0_optim.transpose());  // optimal state after solving using processor
+    }
+
+    virtual void assertAll()
+    {
+        // check delta and state integrals
+        ASSERT_MATRIX_APPROX(D_preint, D_preint_imu, 1e-8);
+        ASSERT_MATRIX_APPROX(D_corrected, D_corrected_imu, 1e-8);
+        ASSERT_MATRIX_APPROX(D_corrected_imu, D_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(D_corrected, D_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(x1_corrected_imu, x1_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(x1_corrected, x1_exact, 1e-5);
+
+        // check optimal solutions
+        ASSERT_MATRIX_APPROX(x0_optim, x0, 1e-5);
+        ASSERT_NEAR(x0_optim.segment(3, 4).norm(), 1.0, 1e-8);
+        ASSERT_MATRIX_APPROX(bias_0, bias_real, 1e-4);
+        ASSERT_MATRIX_APPROX(bias_1, bias_real, 1e-4);
+        ASSERT_MATRIX_APPROX(D_optim, D_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(x1_optim, x1_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(D_optim_imu, D_exact, 1e-5);
+        ASSERT_MATRIX_APPROX(x1_optim_imu, x1_exact, 1e-5);
+        ASSERT_NEAR(x1_optim.segment(3, 4).norm(), 1.0, 1e-8);
+    }
+};
 
-            // ===================================== ODO
-            processor_odo->setOrigin(KF_0);
+class Process_Factor_Imu_ODO : public Process_Factor_Imu
+{
+  public:
+    // Wolf objects
+    SensorOdom3dPtr    sensor_odo;
+    ProcessorOdom3dPtr processor_odo;
+    CaptureOdom3dPtr   capture_odo;
 
-            return true;
-        }
+    void SetUp() override
+    {
+        // ===================================== Imu
+        Process_Factor_Imu::SetUp();
+
+        // ===================================== ODO
+        sensor_odo = std::static_pointer_cast<SensorOdom3d>(
+            problem->installSensor(imu_dir + "/test/yaml/imu_3d/sensor_odom_3d.yaml", {}));
+        processor_odo = std::static_pointer_cast<ProcessorOdom3d>(
+            problem->installProcessor(sensor_odo, imu_dir + "/test/yaml/imu_3d/processor_odom_3d.yaml", {}));
+    }
 
-        void integrateAll() override
-        {
-            // ===================================== Imu
-            Process_Factor_Imu::integrateAll();
+    // Initial configuration of variables
+    bool configureAll() override
+    {
+        // ===================================== Imu
+        Process_Factor_Imu::configureAll();
 
-            // ===================================== ODO
-            Vector6d    data;
-            Vector3d    p1  = x1_exact.head(3);
-            Quaterniond q1   (x1_exact.data() + 3);
-            Vector3d    dp  =        q0.conjugate() * (p1 - p0);
-            Vector3d    dth = log_q( q0.conjugate() *  q1     );
-            data           << dp, dth;
+        // ===================================== ODO
+        processor_odo->setOrigin(KF_0);
 
-            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->computeNoiseCov(data));
+        return true;
+    }
 
-            sensor_odo->process(capture_odo);
-       }
+    void integrateAll() override
+    {
+        // ===================================== Imu
+        Process_Factor_Imu::integrateAll();
 
-        void integrateAllTrajectories() override
-        {
-            // ===================================== Imu
-            Process_Factor_Imu::integrateAllTrajectories();
+        // ===================================== ODO
+        Vector6d    data;
+        Vector3d    p1 = x1_exact.head(3);
+        Quaterniond q1(x1_exact.data() + 3);
+        Vector3d    dp  = q0.conjugate() * (p1 - p0);
+        Vector3d    dth = log_q(q0.conjugate() * q1);
+        data << dp, dth;
 
-            // ===================================== ODO
-            Vector6d    data;
-            Vector3d    p1  = x1_exact.head(3);
-            Quaterniond q1   (x1_exact.data() + 3);
-            Vector3d    dp  =        q0.conjugate() * (p1 - p0);
-            Vector3d    dth = log_q( q0.conjugate() *  q1     );
-            data           << dp, dth;
+        CaptureOdom3dPtr capture_odo =
+            std::make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->computeNoiseCov(data));
 
-            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->computeNoiseCov(data));
+        sensor_odo->process(capture_odo);
+    }
 
-            sensor_odo->process(capture_odo);
-       }
+    void integrateAllTrajectories() override
+    {
+        // ===================================== Imu
+        Process_Factor_Imu::integrateAllTrajectories();
 
-        void buildProblem() override
-        {
-            // ===================================== Imu
-            Process_Factor_Imu::buildProblem();
+        // ===================================== ODO
+        Vector6d    data;
+        Vector3d    p1 = x1_exact.head(3);
+        Quaterniond q1(x1_exact.data() + 3);
+        Vector3d    dp  = q0.conjugate() * (p1 - p0);
+        Vector3d    dth = log_q(q0.conjugate() * q1);
+        data << dp, dth;
 
-            // ===================================== ODO
-            // Process ODO for the callback to take effect
-            data = Vector6d::Zero();
-            capture_odo = make_shared<CaptureOdom3d>(t+dt, sensor_odo, data, sensor_odo->computeNoiseCov(data));
-            sensor_odo->process(capture_odo);
+        CaptureOdom3dPtr capture_odo =
+            std::make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->computeNoiseCov(data));
 
-        }
+        sensor_odo->process(capture_odo);
+    }
 
+    void buildProblem() override
+    {
+        // ===================================== Imu
+        Process_Factor_Imu::buildProblem();
+
+        // ===================================== ODO
+        // Process ODO for the callback to take effect
+        data        = Vector6d::Zero();
+        capture_odo = std::make_shared<CaptureOdom3d>(t + dt, sensor_odo, data, sensor_odo->computeNoiseCov(data));
+        sensor_odo->process(capture_odo);
+    }
 };
 
-TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, test_capture)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    Eigen::Vector6d initial_bias((Eigen::Vector6d()<< .002, .0007, -.001,   .003, -.002, .001).finished());
+    Eigen::Vector6d initial_bias((Eigen::Vector6d() << .002, .0007, -.001, .003, -.002, .001).finished());
     sensor_imu->getIntrinsic()->setState(initial_bias);
     configureAll();
     integrateAll();
     buildProblem();
-    //Since we did not solve, hence bias estimates did not change yet, both capture should have the same calibration
-    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), initial_bias, 1e-8 );
-    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), KF_1->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), 1e-8 );
+    // Since we did not solve, hence bias estimates did not change yet, both capture should have the same calibration
+    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), initial_bias, 1e-8);
+    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(),
+                         KF_1->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(),
+                         1e-8);
 }
 
-TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = false;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = false;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = false;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Matrix<double, 3, 50>::Random();
-    w                  = Matrix<double, 3, 50>::Random();
+    a = Matrix<double, 3, 50>::Random();
+    w = Matrix<double, 3, 50>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Matrix<double, 3, 50>::Random();
-    w                  = Matrix<double, 3, 50>::Random();
+    a = Matrix<double, 3, 50>::Random();
+    w = Matrix<double, 3, 50>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = false;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Matrix<double, 3, 50>::Random();
-    w                  = Matrix<double, 3, 50>::Random();
+    a = Matrix<double, 3, 50>::Random();
+    w = Matrix<double, 3, 50>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     // printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 3,2,1;
-    q0.coeffs()        << 0.5,0.5,0.5,.5;
-    v0                 << 1,2,3;
+    p0 << 3, 2, 1;
+    q0.coeffs() << 0.5, 0.5, 0.5, .5;
+    v0 << 1, 2, 3;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 0,0,0 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(0, 0, 0);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 0,0,0 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(0, 0, 0);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = true;
-    v1_fixed       = false;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = true;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 0,0,0 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(0, 0, 0);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = true;
-    q1_fixed       = false;
-    v1_fixed       = false;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = true;
+    q1_fixed = false;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     // printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 0,0,0 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(0, 0, 0);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = true;
-    v1_fixed       = false;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = true;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 0,0,0 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(0, 0, 0);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = false;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = false;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = true;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = false;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Vector3d( 1,2,3 );
-    w                  = Vector3d( 1,2,3 );
+    a = Vector3d(1, 2, 3);
+    w = Vector3d(1, 2, 3);
 
     // ---------- fix boundaries
-    p0_fixed       = false;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = false;
+    p0_fixed = false;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Matrix<double, 3, 50>::Random();
-    w                  = Matrix<double, 3, 50>::Random();
+    a = Matrix<double, 3, 50>::Random();
+    w = Matrix<double, 3, 50>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = true;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = false;
+    p0_fixed = true;
+    q0_fixed = true;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = false;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
     // printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b)  // F_ixed___e_stimated
 {
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 50;
+    t0               = 0;
+    num_integrations = 50;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a                  = Matrix<double, 3, 50>::Random();
-    w                  = Matrix<double, 3, 50>::Random();
+    a = Matrix<double, 3, 50>::Random();
+    w = Matrix<double, 3, 50>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
-    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
-//    printAll(report);
+    //    printAll(report);
 
     assertAll();
-
 }
 
-TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)  // F_ixed___e_stimated
 {
-
-    // ================================================================================================================ //
-    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
-    // ================================================================================================================ //
+    // ================================================================================================================
+    // //
+    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
+    // //
+    // ================================================================================================================
+    // //
     //
     // ---------- time
-    t0                  = 0;
-    num_integrations    = 25;
+    t0               = 0;
+    num_integrations = 25;
 
     // ---------- initial pose
-    p0                 << 0,0,0;
-    q0.coeffs()        << 0,0,0,1;
-    v0                 << 0,0,0;
+    p0 << 0, 0, 0;
+    q0.coeffs() << 0, 0, 0, 1;
+    v0 << 0, 0, 0;
 
     // ---------- bias
-    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
-    bias_preint         = -bias_real;
+    bias_real << .001, .002, .003, -.001, -.002, -.003;
+    bias_preint = -bias_real;
 
     // ---------- motion params
-    a  = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
-    w  = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
+    a = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
+    w = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
 
     // ---------- fix boundaries
-    p0_fixed       = true;
-    q0_fixed       = false;
-    v0_fixed       = true;
-    p1_fixed       = false;
-    q1_fixed       = false;
-    v1_fixed       = true;
+    p0_fixed = true;
+    q0_fixed = false;
+    v0_fixed = true;
+    p1_fixed = false;
+    q1_fixed = false;
+    v1_fixed = true;
     //
-    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
-    // ================================================================================================================ //
+    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
+    // //
+    // ================================================================================================================
+    // //
 
     // ===================================== RUN ALL
     configureAll();
@@ -1485,30 +1567,30 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
     WOLF_INFO("TEST integrated");
     buildProblem();
     WOLF_INFO("TEST built");
-    string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
     WOLF_INFO("TEST solved");
 
     assertAll();
     WOLF_INFO("TEST asserted");
 
     // Build optimal trajectory
-    MatrixXd Trj_x_optim(10,Trj_D_preint_prc.cols());
-    double Dt = 0;
-    SizeEigen i = 0;
+    MatrixXd  Trj_x_optim(10, Trj_D_preint_prc.cols());
+    double    Dt = 0;
+    SizeEigen i  = 0;
     for (auto J_D_b : Buf_Jac_preint_prc)
     {
-        VectorXd step       = J_D_b * (bias_0 - bias_preint);
-        VectorXd D_opt      = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
-        Trj_x_optim.col(i)  = imu::composeOverState(x0_optim, D_opt, Dt);
+        VectorXd step      = J_D_b * (bias_0 - bias_preint);
+        VectorXd D_opt     = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
+        Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
         Dt += dt;
-        i  ++;
+        i++;
     }
     WOLF_INFO("optimal trajectory built");
 
     // Get optimal trajectory via getState()
     i = 0;
     t = 0;
-    MatrixXd Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
+    MatrixXd Trj_x_optim_prc(10, Trj_D_preint_prc.cols());
     for (int i = 0; i < Trj_x_optim_prc.cols(); i++)
     {
         Trj_x_optim_prc.col(i) = problem->getState(t).vector("POV");
@@ -1518,21 +1600,20 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
 
     // printAll(report);
 
-//    WOLF_INFO("------------------------------------");
-//    WOLF_INFO("Exact x0 \n", x0         .transpose());
-//    WOLF_INFO("Optim x0 \n", x0_optim   .transpose());
-//    WOLF_INFO("Optim x  \n", Trj_x_optim_prc.transpose());
-//    WOLF_INFO("Optim x1 \n", x1_optim   .transpose());
-//    WOLF_INFO("Exact x1 \n", x1_exact   .transpose());
-//    WOLF_INFO("------------------------------------");
+    //    WOLF_INFO("------------------------------------");
+    //    WOLF_INFO("Exact x0 \n", x0         .transpose());
+    //    WOLF_INFO("Optim x0 \n", x0_optim   .transpose());
+    //    WOLF_INFO("Optim x  \n", Trj_x_optim_prc.transpose());
+    //    WOLF_INFO("Optim x1 \n", x1_optim   .transpose());
+    //    WOLF_INFO("Exact x1 \n", x1_exact   .transpose());
+    //    WOLF_INFO("------------------------------------");
 
     // The Mother of Asserts !!!
     ASSERT_MATRIX_APPROX(Trj_x_optim, Trj_x_optim_prc, 1e-6);
 
     // First and last trj states match known extrema
-    ASSERT_MATRIX_APPROX(Trj_x_optim_prc.leftCols (1), x0,       1e-6);
+    ASSERT_MATRIX_APPROX(Trj_x_optim_prc.leftCols(1), x0, 1e-6);
     ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);
-
 }
 
 TEST_F(Process_Factor_Imu, bootstrap)
@@ -1541,14 +1622,16 @@ TEST_F(Process_Factor_Imu, bootstrap)
     processor_imu->setMaxTimeSpan(0.04);
     processor_imu->bootstrapEnable(true);
 
-    auto KF0 = problem->emplaceFrame(0.0);
-    problem->keyFrameCallback(KF0,nullptr);
+    // FIXME: error maybe due to enable bootstrap before problem->applyPriorOptions()
+
+    // auto KF0 = problem->emplaceFrame(0.0);
+    // problem->keyFrameCallback(KF0, nullptr);
     problem->print(4, 0, 1, 0);
 
     // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z
     // Vector6d data(0.0, 9.806, 0.0,  0.0, 0.0, 0.0); // gravity on Y
     Vector6d data;
-    data <<  0.0, 9.806, 0.0,  0.0, 0.0, 0.0  ; // gravity on Y
+    data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0;  // gravity on Y
 
     capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());
 
@@ -1569,12 +1652,12 @@ TEST_F(Process_Factor_Imu, bootstrap)
     }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
+    testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
+    // testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
+    // testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
 
     return RUN_ALL_TESTS();
 }
@@ -1583,17 +1666,20 @@ int main(int argc, char **argv)
  *
  * - Process_Factor_Imu_ODO.MotionConstant_PQv_b__PQv_b :
  *      this test will not work + jacobian is rank deficient because of estimating both initial
- *      and final velocities. 
- *      Imu data integration is done with correct biases (so this is the case of a calibrated Imu). Before solving the problem, we perturbate the initial bias.
- *      We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
- *      bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
- *      difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
- *      solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
- * 
+ *      and final velocities.
+ *      Imu data integration is done with correct biases (so this is the case of a calibrated Imu). Before solving the
+ * problem, we perturbate the initial bias. We solve the problem by fixing all states excepted V1 and V2. while
+ * creating the factors, both velocities are corrected using the difference between the actual bias and the bias used
+ * during preintegration. One way to solve this in the solver side would be to make the actual bias match the
+ * preintegraion bias so that the difference is 0 and does not affect the states of the KF. Another possibility is to
+ * have both velocities modified without changing the biases. it is likely that this solution is chosen in this case
+ * (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
+ *
  *  - Bias evaluation with a precision of 1e-4 :
- *      The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated Imu
- *      Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
- *      A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- *      In addition, for Process_Factor_Imu tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
- *      Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
+ *      The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating
+ * the case of a non calibrated Imu Because of cross relations between acc and gyro biases (respectively a_b and w_b)
+ * it is difficult to expect a better estimation. A small change in a_b can be cancelled by a small variation in w_b.
+ * in other words : there are local minima. In addition, for Process_Factor_Imu tests, P and V are tested against 1e-5
+ * precision while 1e-8 is used for Q. Errors tend to be distributed in different estimated variable when we get into a
+ * local minima (to minimize residuals in a least square sense).
  */
diff --git a/test/gtest_imu_3d_mocap_fusion.cpp b/test/gtest_imu_3d_mocap_fusion.cpp
index 4dbc45f25d998fdf74b3d4c151ab53fefb4b60a0..59d8167cafdb87381edb3b4e835c81766f9518ca 100644
--- a/test/gtest_imu_3d_mocap_fusion.cpp
+++ b/test/gtest_imu_3d_mocap_fusion.cpp
@@ -46,6 +46,10 @@ const double dt = 0.001;
 std::string imu_dir         = _WOLF_IMU_CODE_DIR;
 std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
+/**
+ *  SET TO TRUE  TO PRODUCE CSV FILE
+ *  SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
 #define WRITE_CSV_FILE false
 
 class ImuMocapFusion_Test : public testing::Test
@@ -98,7 +102,7 @@ class ImuMocapFusion_Test : public testing::Test
         Vector3d    w_v_wb = Vector3d::Zero();  // * cos(0)
 
         // Problem
-        problem_          = Problem::autoSetup(imu_dir + "/test/yaml/imu_mocap_fusion/problem_3d.yaml", {imu_dir});
+        problem_          = Problem::autoSetup(imu_dir + "/test/yaml/imu_3d_mocap_fusion/problem_3d.yaml", {imu_dir});
         sensor_pose_      = problem_->findSensor("cool sensor pose");
         sensor_imu_       = problem_->findSensor("cool sensor imu");
         Matrix6d cov_pose = sensor_pose_->computeNoiseCov(VectorXd(0));
diff --git a/test/gtest_imu_3d_static_init.cpp b/test/gtest_imu_3d_static_init.cpp
index eeb3a6f88531f2605a237de1a83169f6ee74d6b6..40f3cedd899673e36db1bcbb29c93e1fbf608931 100644
--- a/test/gtest_imu_3d_static_init.cpp
+++ b/test/gtest_imu_3d_static_init.cpp
@@ -41,7 +41,7 @@ std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
  *  SET TO TRUE  TO PRODUCE CSV FILE
  *  SET TO FALSE TO STOP PRODUCING CSV FILE
  */
-#define WRITE_CSV_FILE true
+#define WRITE_CSV_FILE false
 
 class ProcessorImuStaticInitTest : public testing::Test
 {
@@ -63,14 +63,9 @@ class ProcessorImuStaticInitTest : public testing::Test
     void SetUp() override
     {
         WOLF_INFO("Doing setup...");
-        using namespace Eigen;
-        using std::make_shared;
-        using std::shared_ptr;
-        using std::static_pointer_cast;
-        using namespace Constants;
 
         // Wolf problem
-        problem_ = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_imu_3d_static_init.yaml", {imu_dir});
+        problem_ = Problem::autoSetup(imu_dir + "/test/yaml/imu_3d_static_init/problem_3d.yaml", {imu_dir});
         sensor_  = problem_->findSensor("cool sensor imu");
         processor_motion_ = std::static_pointer_cast<ProcessorMotion>(sensor_->getProcessorList().front());
 
@@ -109,7 +104,7 @@ class ProcessorImuStaticInitTest : public testing::Test
 
 #if WRITE_CSV_FILE
         std::fstream file_est;
-        file_est.open("./est-" + test_name + ".csv", std::fstream::out);
+        file_est.open(imu_dir + "/test/yaml/imu_3d_static_init/est-" + test_name + ".csv", std::fstream::out);
         //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
         std::string header_est;
         if (testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
@@ -252,7 +247,7 @@ class ProcessorImuStaticInitTest : public testing::Test
 
 #if WRITE_CSV_FILE
         std::fstream file_est;
-        file_est.open("./estzeroodom-" + test_name + ".csv", std::fstream::out);
+        file_est.open(imu_dir + "/test/yaml/imu_3d_static_init/estzeroodom-" + test_name + ".csv", std::fstream::out);
         //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
         std::string header_est;
         if (testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
diff --git a/test/gtest_processor_imu_2d.cpp b/test/gtest_processor_imu_2d.cpp
index 04d00d85acb423465e4d873f292ca5daf7858b56..306d80509b5d6c6dcf459a169fa31c9cc7a31b61 100644
--- a/test/gtest_processor_imu_2d.cpp
+++ b/test/gtest_processor_imu_2d.cpp
@@ -1,4 +1,4 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
+// WOLF - Copyright (capture_imu) 2020,2021,2022,2023
 // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
 // Joan Vallvé Navarro (jvallve@iri.upc.edu)
@@ -32,236 +32,176 @@
 
 using namespace Eigen;
 using namespace wolf;
-using std::shared_ptr;
-using std::make_shared;
-using std::static_pointer_cast;
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
+
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
 class ProcessorImu2dTest : public testing::Test
 {
-    public: //These can be accessed in fixtures
-        ProblemPtr problem;
-        SensorBasePtr sensor_ptr;
-        ProcessorMotionPtr processor_motion;
-        TimeStamp t;
-        TimeStamp t0;
-        double mti_clock, tmp;
-        double dt;
-        Vector6d data;
-        Matrix6d data_cov;
-        VectorComposite     x0c;                                // initial state composite
-        VectorComposite     s0c;                                // initial sigmas composite
-        CaptureImuPtr C;
-
-    //a new of this will be created for each new test
+  public:
+    ProblemPtr         problem;
+    SensorBasePtr      sensor;
+    ProcessorMotionPtr processor;
+    double             dt;
+    Vector6d           data;
+    Matrix6d           data_cov;
+    Vector2d           p_0, v_0;
+    Vector1d           o_0;
+    Vector2d           g;
+    CaptureImuPtr      capture_imu;
+
+    // a new of this will be created for each new test
     void SetUp() override
     {
         // Wolf problem
-        problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
-        processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+        problem = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_processor_imu_2d.yaml", {imu_dir});
+
+        // sensor
+        sensor    = problem->findSensor("cool sensor imu");
+        processor = std::static_pointer_cast<ProcessorMotion>(sensor->getProcessorList().front());
 
         // Time and data variables
-        dt = 0.01;
-        t0.set(0);
-        t = t0;
-        data = Vector6d::Zero();
+        dt       = 0.001;
+        data     = Vector6d::Zero();
         data_cov = Matrix6d::Identity();
 
+        // Set the origin
+        problem->applyPriorOptions(0);
+        p_0 = problem->getLastFrame()->getState().vector("P");
+        o_0 = problem->getLastFrame()->getState().vector("O");
+        v_0 = problem->getLastFrame()->getState().vector("V");
+        processor->setOrigin(problem->getLastFrame());
+
         // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
-        C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+        capture_imu = std::make_shared<CaptureImu>(0, sensor, data, data_cov, Vector3d::Zero());
     }
-
 };
 
-TEST(ProcessorImu2d_constructors, ALL)
+TEST(ProcessorImu2d_factory, ALL)
 {
-    //constructor with ProcessorImu2dParamsPtr argument only
-    ParamsProcessorImu2dPtr param_ptr = std::make_shared<ParamsProcessorImu2d>();
-    param_ptr->max_time_span =   2.0;
-    param_ptr->max_buff_length = 20000;
-    param_ptr->dist_traveled =   2.0;
-    param_ptr->angle_turned =    2.0;
-
-    ProcessorImu2dPtr prc1 = std::make_shared<ProcessorImu2d>(param_ptr);
-    ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
-    ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
-    ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
-    ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
-    //Factory constructor with pointers
-    std::string wolf_root = _WOLF_IMU_CODE_DIR;
-    ProblemPtr problem = Problem::create("POV", 2);
-    std::cout << "creating sensor_ptr" << std::endl;
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d.yaml", {wolf_root});
-    std::cout << "created sensor_ptr" << std::endl;
-    ParamsProcessorImu2dPtr params_default = std::make_shared<ParamsProcessorImu2d>();
-    // ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default);
-    ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu2d>(sensor_ptr, params_default);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
+    // Factory constructor
+    auto processor_ptr =
+        ProcessorImu2d::create(imu_dir + "/test/yaml/processor_imu_2d.yaml", {imu_dir, wolf_schema_dir});
 
-    //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(),   2.0);
+    // values copied from yaml file
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), 2.0);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(),  2.0);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(),   0.2);
-}
-
-TEST_F(ProcessorImu2dTest, Prior)
-{
-    // Set the origin
-    // x0c['P'] = Vector2d(1,2);
-    // x0c['O'] = Vector1d(0);
-    // x0c['V'] = Vector2d(4,5);
-    // auto KF0 = problem->setPriorFix(x0c, t0);
-    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-    auto KF0 = problem->setPrior(problem_prior, t0);
-    processor_motion->setOrigin(KF0);
-    //WOLF_DEBUG("x0 =", x0c);
-    //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t0)->getState("POV"));
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxDistTraveled(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxAngleTurned(), 0.2);
+
+    // install
+    auto problem  = Problem::create("POV", 2);
+    auto sensor   = problem->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
+    processor_ptr = problem->installProcessor(sensor, imu_dir + "/test/yaml/processor_imu_2d.yaml", {imu_dir});
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxDistTraveled(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxAngleTurned(), 0.2);
 }
 
 TEST_F(ProcessorImu2dTest, MRUA)
 {
-    data << 1, 0, 0,   0, 0, 0; 
+    data << 1, 0, 0, 0, 0, 0;
     data_cov *= 1e-3;
-    // Set the origin
-    // x0c['P'] = Vector2d(1,2);
-    // x0c['O'] = Vector1d(0);
-    // x0c['V'] = Vector2d(4,5);
-    // auto KF0 = problem->setPriorFix(x0c, t0);
-    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-    auto KF0 = problem->setPrior(problem_prior, t0);
-    processor_motion->setOrigin(KF0);
- 
-    problem->print(4,1,1,1);
+
+    // problem->print(4,1,1,1);
     WOLF_INFO("Current State: ", problem->getState());
-    for(t = t0; t <= t0 + 1.0 + dt/2; t+=dt){
-        C->setTimeStamp(t);
-        C->setData(data);
-        C->setDataCovariance(data_cov);
-        C->process();
+    for (double t = 0; t <= 1.0 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
         WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
-        WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
-        ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+        WOLF_INFO((p_0 + v_0 * t + 0.5 * data.head(2) * std::pow(t, 2)).transpose());
+        ASSERT_MATRIX_APPROX(p_0 + v_0 * t + 0.5 * data.head(2) * std::pow(t, 2), problem->getState()['P'], 1e-9);
     }
 }
 
 TEST_F(ProcessorImu2dTest, parabola)
 {
-    double v0 = 10;
-    double a = 1;
+    double v0x = 10;
+    double a_y  = 1;
     data_cov *= 1e-3;
- 
+
+    // set initial velocity
+    problem->getLastFrame()->getStateBlock('V')->setState(Vector2d(v0x,0));
+
     Vector2d pos;
-    // Set the origin
-    // x0c['P'] = Vector2d(0, 0);
-    // x0c['O'] = Vector1d(0);
-    // x0c['V'] = Vector2d(v0, 0);
-    // auto KF0 = problem->setPriorFix(x0c, t0);
-    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-    auto KF0 = problem->setPrior(problem_prior, t0);
-    processor_motion->setOrigin(KF0);
- 
-    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-        C->setTimeStamp(t);
-        data <<  0, a, 0,    0,  0, 0;
-        C->setData(data);
-        C->setDataCovariance(data_cov);
-        C->process();
-        //WOLF_DEBUG("Current State: ", problem->getState());
-        pos << v0*(t-t0),
-               0.5*a*std::pow(t-t0, 2);
-        //WOLF_DEBUG("Calculated State: ", pos.transpose());
-        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
-        EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9);
+
+    for (double t = dt; t <= 0.5 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        data << 0, a_y, 0, 0, 0, 0;
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
+        // WOLF_DEBUG("Current State: ", problem->getState());
+        pos << v0x * t, 0.5 * a_y * std::pow(t, 2);
+        // WOLF_DEBUG("Calculated State: ", pos.transpose());
+        EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(Vector2d(v0x, a_y * t), problem->getState()['V'], 1e-9);
     }
 }
 
-TEST_F(ProcessorImu2dTest, parabola_deluxe)
+TEST_F(ProcessorImu2dTest, parabola_xy)
 {
     Vector2d v0(13, 56);
     Vector2d a(1, 2);
     data_cov *= 1e-3;
- 
+
+    // set initial velocity
+    problem->getLastFrame()->getStateBlock('V')->setState(v0);
+
     Vector2d pos;
-    // Set the origin
-    // x0c['P'] = Vector2d(0, 0);
-    // x0c['O'] = Vector1d(0);
-    // x0c['V'] = v0;
-    // auto KF0 = problem->setPriorFix(x0c, t0);
-    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-    auto KF0 = problem->setPrior(problem_prior, t0);
-    processor_motion->setOrigin(KF0);
- 
-    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-        C->setTimeStamp(t);
-        data <<  a(0), a(1), 0,    0,  0, 0;
-        C->setData(data);
-        C->setDataCovariance(data_cov);
-        C->process();
-        //WOLF_DEBUG("Current State: ", problem->getState());
-        pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2);
-        //WOLF_DEBUG("Calculated State: ", pos.transpose());
-        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
-        EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9);
+
+    for (double t = dt; t <= 0.5 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        data << a, 0, 0, 0, 0;
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
+        // WOLF_DEBUG("Current State: ", problem->getState());
+        pos = v0 * t + 0.5 * a * std::pow(t, 2);
+        // WOLF_DEBUG("Calculated State: ", pos.transpose());
+        EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(v0 + a * t, problem->getState()['V'], 1e-9);
     }
 }
 
 TEST_F(ProcessorImu2dTest, Circular_move)
 {
-    double pi = 3.14159265358993;
-    double r = 1;
-    double w = 1;
-    double alpha = pi/4.;
-    Vector2d pos(r, 0);
+    double   pi    = 3.14159265358993;
+    double   r     = 1;
+    double   w     = 1;
+    double   alpha = pi / 4.;
+    Vector2d pos;
     data_cov *= 1e-3;
-    //dt = 0.0001;
- 
-    // Set the origin
-    // x0c['P'] = pos;
-    // x0c['O'] = Vector1d(alpha);
-    // x0c['V'] = Vector2d(0, w*r);
-    //auto KF0 = problem->setPriorFix(x0c, t0);
-    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  pos)},
-                                {'O', SpecState("StateAngle",    Vector1d(alpha))},
-                                {'V', SpecState("StateVector2d", Vector2d(0, w*r))}};
-    auto KF0 = problem->setPrior(problem_prior, t0);
- 
-    processor_motion->setOrigin(KF0);
- 
+
+    // Set initial frame POV
+    problem->getLastFrame()->getStateBlock('P')->setState(Vector2d(r, 0));
+    problem->getLastFrame()->getStateBlock('O')->setState(Vector1d(alpha));
+    problem->getLastFrame()->getStateBlock('V')->setState(Vector2d(0, w*r));
+
     WOLF_DEBUG("Current State: ", problem->getState());
-    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-        C->setTimeStamp(t);
-        data <<  -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0,    0,  0, w;
-        C->setData(data);
-        C->setDataCovariance(data_cov);
-        C->process();
+    for (double t = dt; t <= 0.5 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        data << -std::cos(alpha) * w * w * r, std::sin(alpha) * w * w * r, 0, 0, 0, w;
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
         WOLF_DEBUG("Current State: ", problem->getState());
-        pos << r*std::cos( w*(t-t0) ),
-               r*std::sin( w*(t-t0) );
+        pos << r * std::cos(w * t), r * std::sin(w * t);
         WOLF_DEBUG("Calculated State: ", pos.transpose());
-        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9);
     }
 }
 
 int main(int argc, char **argv)
 {
-  testing::InitGoogleTest(&argc, argv);
-  //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
-  return RUN_ALL_TESTS();
+    testing::InitGoogleTest(&argc, argv);
+    //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
+    return RUN_ALL_TESTS();
 }
-
diff --git a/test/gtest_processor_imu_2d_with_gravity.cpp b/test/gtest_processor_imu_2d_with_gravity.cpp
index 9d30d56fb789a3ca143628a33cfaeef09e516dc5..d39cd53c56e8ceb62b83015e48a86ba952c7f35f 100644
--- a/test/gtest_processor_imu_2d_with_gravity.cpp
+++ b/test/gtest_processor_imu_2d_with_gravity.cpp
@@ -1,4 +1,4 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
+// WOLF - Copyright (capture_imu) 2020,2021,2022,2023
 // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
 // Joan Vallvé Navarro (jvallve@iri.upc.edu)
@@ -35,155 +35,123 @@
 using namespace Eigen;
 using namespace wolf;
 
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
 class ProcessorImu2dTest : public testing::Test
 {
+  public:
+    ProblemPtr         problem;
+    SensorBasePtr      sensor;
+    ProcessorMotionPtr processor;
+    double             dt;
+    Vector6d           data;
+    Matrix6d           data_cov;
+    Vector2d           p_0, v_0;
+    Vector1d           o_0;
+    Vector2d           g;
+    CaptureImuPtr      capture_imu;
 
-    public: //These can be accessed in fixtures
-        wolf::ProblemPtr problem;
-        wolf::SensorBasePtr sensor_ptr;
-        wolf::ProcessorMotionPtr processor_motion;
-        wolf::TimeStamp t;
-        wolf::TimeStamp t0;
-        double mti_clock, tmp;
-        double dt;
-        Vector6d data;
-        Matrix6d data_cov;
-        Vector2d g;
-        VectorComposite     x0c;                                // initial state composite
-        VectorComposite     s0c;                                // initial sigmas composite
-        std::shared_ptr<wolf::CaptureImu> C;
-
-    //a new of this will be created for each new test
     void SetUp() override
     {
-        using namespace Eigen;
-        using std::shared_ptr;
-        using std::make_shared;
-        using std::static_pointer_cast;
-        using namespace wolf::Constants;
-
-        std::string wolf_root = _WOLF_IMU_CODE_DIR;
-
         // Wolf problem
-        problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d_with_gravity.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
-        processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+        problem =
+            Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_processor_imu_2d_with_gravity.yaml", {imu_dir});
+
+        // sensor
+        sensor    = problem->findSensor("cool sensor imu");
+        processor = std::static_pointer_cast<ProcessorMotion>(sensor->getProcessorList().front());
 
         // Time and data variables
-        dt = 0.01;
-        t0.set(0);
-        t = t0;
-        data = Vector6d::Zero();
+        dt       = 0.001;
+        data     = Vector6d::Zero();
         data_cov = Matrix6d::Identity();
 
+        // Set the origin
+        problem->applyPriorOptions(0);
+        p_0 = problem->getLastFrame()->getState().vector("P");
+        o_0 = problem->getLastFrame()->getState().vector("O");
+        v_0 = problem->getLastFrame()->getState().vector("V");
+        processor->setOrigin(problem->getLastFrame());
+
         // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
-        C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+        capture_imu = std::make_shared<CaptureImu>(0, sensor, data, data_cov, Vector3d::Zero());
     }
-
 };
 
-
 TEST_F(ProcessorImu2dTest, MUA_Only_G)
 {
-   data << 0, 0, 0,   0, 0, 0; 
-   data_cov *= 1e-3;
-   // Set the origin
-//    x0c['P'] = Vector2d(1,2);
-//    x0c['O'] = Vector1d(0);
-//    x0c['V'] = Vector2d(4,5);
-//    auto KF0 = problem->setPriorFix(x0c, t0);
-   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-   auto KF0 = problem->setPrior(problem_prior, t0);
-   processor_motion->setOrigin(KF0);
-
-   // Set the gravity
-   g << 6, 7;
-   sensor_ptr->getStateBlock('G')->setState(g);
-
-   //WOLF_DEBUG("Current State: ", problem->getState());
-   for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
-       //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
-       ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*g*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
-       EXPECT_MATRIX_APPROX(x0c['V'] + g*(t-t0), problem->getState()['V'], 1e-9);
-   }
+    data << 0, 0, 0, 0, 0, 0;
+    data_cov *= 1e-3;
+
+    // Set the gravity
+    g << 6, 7;
+    sensor->getStateBlock('G')->setState(g);
+
+    // WOLF_DEBUG("Current State: ", problem->getState());
+    for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
+        // WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+        // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, 2)).transpose());
+        ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState()['V'], 1e-9);
+    }
 }
 
 TEST_F(ProcessorImu2dTest, MUA)
 {
-   data << 1, 2, 0,   0, 0, 0; 
-   data_cov *= 1e-3;
-   // Set the origin
-//    x0c['P'] = Vector2d(1,2);
-//    x0c['O'] = Vector1d(0);
-//    x0c['V'] = Vector2d(4,5);
-//    auto KF0 = problem->setPriorFix(x0c, t0);
-   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-   auto KF0 = problem->setPrior(problem_prior, t0);
-   processor_motion->setOrigin(KF0);
-
-   // Set the gravity
-   g << 6, 7;
-   sensor_ptr->getStateBlock('G')->setState(g);
-
-   //WOLF_DEBUG("Current State: ", problem->getState());
-   for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
-       //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
-       ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*(data.head(2)+g)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
-       EXPECT_MATRIX_APPROX(x0c['V'] + (data.head(2)+g)*(t-t0), problem->getState()['V'], 1e-9);
-   }
+    data << 1, 2, 0, 0, 0, 0;
+    data_cov *= 1e-3;
+
+    // Set the gravity
+    g << 6, 7;
+    sensor->getStateBlock('G')->setState(g);
+
+    // WOLF_DEBUG("Current State: ", problem->getState());
+    for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
+        // WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+        // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, 2)).transpose());
+        ASSERT_MATRIX_APPROX(
+            p_0 + v_0 * (t - 0) + 0.5 * (data.head(2) + g) * std::pow(t - 0, 2), problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(v_0 + (data.head(2) + g) * (t - 0), problem->getState()['V'], 1e-9);
+    }
 }
 
 TEST_F(ProcessorImu2dTest, Spinning)
 {
-   double w = 0.5;
-   data << 0, 0, 0,   0, 0, w; 
-   data_cov *= 1e-3;
-   // Set the origin
-//    x0c['P'] = Vector2d(1,2);
-//    x0c['O'] = Vector1d(0);
-//    x0c['V'] = Vector2d(4,5);
-//    auto KF0 = problem->setPriorFix(x0c, t0);
-   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
-                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
-                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
-   auto KF0 = problem->setPrior(problem_prior, t0);
-   processor_motion->setOrigin(KF0);
-
-   // Set the gravity
-   g << 6, 7;
-   sensor_ptr->getStateBlock('G')->setState(g);
-
-   //WOLF_DEBUG("Current State: ", problem->getState());
-   for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
-       //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
-       ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*g*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
-       EXPECT_MATRIX_APPROX(x0c['V'] + g*(t-t0), problem->getState()['V'], 1e-9);
-       EXPECT_NEAR(x0c['O'](0) + w*(t-t0), problem->getState()['O'](0), 1e-9);
-   }
-}
+    double w = 0.5;
+    data << 0, 0, 0, 0, 0, w;
+    data_cov *= 1e-3;
 
+    // Set the gravity
+    g << 6, 7;
+    sensor->getStateBlock('G')->setState(g);
 
-//TEST_F(ProcessorImu2dTest, Circular_move)
+    // WOLF_DEBUG("Current State: ", problem->getState());
+    for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        capture_imu->setData(data);
+        capture_imu->setDataCovariance(data_cov);
+        capture_imu->process();
+        // WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+        // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, 2)).transpose());
+        ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState()['V'], 1e-9);
+        EXPECT_NEAR(o_0(0) + w * (t - 0), problem->getState()['O'](0), 1e-9);
+    }
+}
+
+// TEST_F(ProcessorImu2dTest, Circular_move)
 //{
 ////   double pi = 3.14159265358993;
 //   double r = 1;
@@ -191,41 +159,38 @@ TEST_F(ProcessorImu2dTest, Spinning)
 //   double alpha = 0;
 //   Vector2d pos;
 //   // Set the origin
-//   x0c['P'] = Vector2d(r, 0);
-//   pos = x0c['P'];
-//   x0c['O'] = Vector1d(alpha);
-//   x0c['V'] = Vector2d(0, w*r);
+//   p_0 = Vector2d(r, 0);
+//   pos = p_0;
+//   o_0 = Vector1d(alpha);
+//   v_0 = Vector2d(0, w*r);
 //
 //   data_cov *= 1e-3;
 //   //dt = 0.0001;
-//   auto KF0 = problem->setPriorFix(x0c, t0, dt/2);
-//   processor_motion->setOrigin(KF0);
 //
 //   // Set the gravity
 //   g << 0.1, 0;
-//   sensor_ptr->getStateBlock('G')->setState(g);
+//   sensor->getStateBlock('G')->setState(g);
 //
 //   WOLF_DEBUG("Current State: ", problem->getState());
 //   dt = 0.001;
-//   for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-//       C->setTimeStamp(t);
+//   for(t = 0+dt; t <= 0 + 0.5 + dt/2; t+=dt){
+//       capture_imu->setTimeStamp(t);
 //       data <<  -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0,    0,  0, w;
-//       data.head(2) -= Eigen::Rotation2Dd(alpha + w*(t-t0))*g;
-//       C->setData(data);
-//       C->setDataCovariance(data_cov);
-//       C->process();
+//       data.head(2) -= Eigen::Rotation2Dd(alpha + w*(t-0))*g;
+//       capture_imu->setData(data);
+//       capture_imu->setDataCovariance(data_cov);
+//       capture_imu->process();
 //       WOLF_INFO("Current State: ", problem->getState());
-//       pos << r*std::cos( w*(t-t0) ),
-//              r*std::sin( w*(t-t0) );
+//       pos << r*std::cos( w*(t-0) ),
+//              r*std::sin( w*(t-0) );
 //       WOLF_INFO("Calculated State: ", pos.transpose());
-//       EXPECT_MATRIX_APPROX(Eigen::Rotation2Dd(w*(t-t0))*x0c['V'] , problem->getState()['V'], 1e-3);
+//       EXPECT_MATRIX_APPROX(Eigen::Rotation2Dd(w*(t-0))*v_0 , problem->getState()['V'], 1e-3);
 //   }
 //}
 
 int main(int argc, char **argv)
 {
-  testing::InitGoogleTest(&argc, argv);
-  //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
-  return RUN_ALL_TESTS();
+    testing::InitGoogleTest(&argc, argv);
+    //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
+    return RUN_ALL_TESTS();
 }
-
diff --git a/test/gtest_processor_imu_3d.cpp b/test/gtest_processor_imu_3d.cpp
index 281a96dc065a9a4cd1a40f3358293f15e3a6a176..ae3c0f08f8594a3fcd077902a673c330547ec41f 100644
--- a/test/gtest_processor_imu_3d.cpp
+++ b/test/gtest_processor_imu_3d.cpp
@@ -32,159 +32,97 @@
 
 using namespace Eigen;
 using namespace wolf;
-using std::shared_ptr;
-using std::make_shared;
-using std::static_pointer_cast;
-
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
-
-SpecComposite problem_prior_{{'P', SpecState("StatePoint3d",
-                                             Vector3d::Zero(),
-                                             "factor",
-                                             Vector3d::Ones())},
-                             {'O', SpecState("StateQuaternion",
-                                             Quaterniond::Identity().coeffs(),
-                                             "factor",
-                                             Vector3d::Ones())},
-                             {'V', SpecState("StateVector3d",
-                                             Vector3d::Zero(),
-                                             "factor",
-                                             Vector3d::Ones())}};
+
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
 class ProcessorImu3dTest : public testing::Test
 {
+  public:
+    ProblemPtr         problem;
+    SensorBasePtr      sensor;
+    ProcessorMotionPtr processor;
+    Vector6d           data;
+    Matrix6d           data_cov;
+    CaptureImuPtr      capture_imu;
 
-    public: //These can be accessed in fixtures
-        ProblemPtr problem;
-        SensorBasePtr sensor_ptr;
-        ProcessorMotionPtr processor_motion;
-        TimeStamp t;
-        double mti_clock, tmp;
-        double dt;
-        Vector6d data;
-        Matrix6d data_cov;
-        VectorXd x0;
-        VectorComposite     x0c;                                // initial state composite
-        VectorComposite     s0c;                                // initial sigmas composite
-        CaptureImuPtr cap_imu_ptr;
-
-    //a new of this will be created for each new test
     void SetUp() override
     {
         // Wolf problem
-        problem = Problem::create("POV", 3);
-        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu3d", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
-        processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+        problem = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_processor_imu_3d.yaml", {imu_dir});
+
+        // sensor
+        sensor    = problem->findSensor("cool sensor imu");
+        processor = std::static_pointer_cast<ProcessorMotion>(sensor->getProcessorList().front());
 
         // Time and data variables
-        t.set(0);
-        data = Vector6d::Zero();
+        data     = Vector6d::Zero();
         data_cov = Matrix6d::Identity();
 
         // Set the origin
-        x0.resize(10);
+        problem->applyPriorOptions(0);
 
         // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
-        cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval());
-    }
-
-    void TearDown() override
-    {
-        // code here will be called just after the test completes
-        // ok to through exceptions from here if need be
-        /*
-            You can do deallocation of resources in TearDown or the destructor routine. 
-                However, if you want exception handling you must do it only in the TearDown code because throwing an exception 
-                from the destructor results in undefined behavior.
-            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. 
-                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
-        */
+        capture_imu = std::make_shared<CaptureImu>(0, sensor, data, data_cov, Vector6d::Zero().eval());
     }
 };
 
-TEST(ProcessorImu3d_constructors, ALL)
+TEST(ProcessorImu3d_factory, ALL)
 {
-    //constructor with ProcessorImuParamsPtr argument only
-    ParamsProcessorImuPtr param_ptr = std::make_shared<ParamsProcessorImu>();
-    param_ptr->max_time_span =   2.0;
-    param_ptr->max_buff_length = 20000;
-    param_ptr->dist_traveled =   2.0;
-    param_ptr->angle_turned =    2.0;
-
-    ProcessorImuPtr prc1 = std::make_shared<ProcessorImu>(param_ptr);
-    ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
-    ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
-    ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
-    ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
-    //Factory constructor with pointers
-    ProblemPtr problem = Problem::create("POV", 3);
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
-    ParamsProcessorImuPtr params_default = std::make_shared<ParamsProcessorImu>();
-    ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, params_default);
-    //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
-
-    //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   2.0);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), 20000);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  2.0);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   0.2);
+    // Factory constructor
+    auto processor_ptr =
+        ProcessorImu3d::create(imu_dir + "/test/yaml/processor_imu_3d.yaml", {imu_dir, wolf_schema_dir});
+
+    // values copied from yaml file
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxTimeSpan(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxBuffLength(), 20000);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxDistTraveled(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxAngleTurned(), 0.2);
+
+    // install
+    auto problem  = Problem::create("POV", 3);
+    auto sensor   = problem->installSensor(imu_dir + "/test/yaml/sensor_imu_3d.yaml", {imu_dir});
+    processor_ptr = problem->installProcessor(sensor, imu_dir + "/test/yaml/processor_imu_3d.yaml", {imu_dir});
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxTimeSpan(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxBuffLength(), 20000);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxDistTraveled(), 2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu3d>(processor_ptr)->getMaxAngleTurned(), 0.2);
 }
 
-TEST(ProcessorImu3d, voteForKeyFrame)
+TEST_F(ProcessorImu3dTest, voteForKeyFrame)
 {
-    // Wolf problem
-    ProblemPtr problem = Problem::create("POV", 3);
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
-    ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>();
-    prc_imu_params->max_time_span = 1;
-    prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-    prc_imu_params->dist_traveled = 1000000000;
-    prc_imu_params->angle_turned = 1000000000;
-    prc_imu_params->voting_active = true;
-    auto processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params);
-    //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
-    
-    //setting origin
-    TimeStamp t(0);
-    problem->setPrior(problem_prior_, t);
-
-    //data variable and covariance matrix
-    // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
+    // data variable and covariance matrix
+    //  since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it
+    //  won't pass following assertions
     Vector6d data;
-    data << 1,0,0, 0,0,0;
+    data << 1, 0, 0, 0, 0, 0;
     Matrix6d data_cov(Matrix6d::Identity());
-    data_cov(0,0) = 0.5;
+    data_cov(0, 0) = 0.5;
 
     // Create the captureImu to store the Imu data arriving from (sensor / callback / file / etc.)
-    std::shared_ptr<CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval());
+    TimeStamp t(0);
+    auto capture_imu = std::make_shared<CaptureImu>(t, sensor, data, data_cov, Vector6d::Zero().eval());
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->process();
+    capture_imu->process();
 
-    //  Time  
+    //  Time
     // we want more than one data to integrate otherwise covariance will be 0
-    double dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() - 0.1;
+    double dt = processor->getMaxTimeSpan() - 0.1;
     t.set(dt);
-    cap_imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setTimeStamp(t);
+    sensor->process(capture_imu);
 
-    dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() + 0.1;
+    dt = processor->getMaxTimeSpan() + 0.1;
     t.set(dt);
-    cap_imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setTimeStamp(t);
+    sensor->process(capture_imu);
 
-    /*There should be 3 frames :
+    /*There should be 2 frames :
         - 1 KeyFrame at origin
         - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
     */
-    ASSERT_EQ(problem->getTrajectory()->size(),(unsigned int) 2);
+    ASSERT_EQ(problem->getTrajectory()->size(), (unsigned int)2);
 
     /* if max_time_span == 2,  Wolf tree should be
 
@@ -198,49 +136,36 @@ TEST(ProcessorImu3d, voteForKeyFrame)
             Estim, ts=0,	 x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
             C1 -> S1
         KF2
-            Estim, ts=2.1,	 x = ( 2.2     0       -22     0       0       0       1       2.1     0       -21     0       0       0       0       0       0      )
-            C2 -> S1
-                f1
-                    m = ( 2.21 0   0   0   0   0   1   2.1 0   0  )
-                    c1 --> F1
-        F3
-            Estim, ts=2.1,	 x = ( . . .)
-            C4 -> S1
+            Estim, ts=2.1,	 x = ( 2.2     0       -22     0       0       0       1       2.1     0       -21     0 0
+    0       0       0       0      ) C2 -> S1 f1 m = ( 2.21 0   0   0   0   0   1   2.1 0   0  ) c1 --> F1 F3 Estim,
+    ts=2.1,	 x = ( . . .) C4 -> S1
     */
-    //TODO : ASSERT TESTS to make sure the factors are correctly set + check the tree above
-
+    // TODO : ASSERT TESTS to make sure the factors are correctly set + check the tree above
 }
 
-
 TEST_F(ProcessorImu3dTest, acc_x)
 {
-    t.set(0); // clock in 0,1 ms ticks
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    data << -gravity(), 0, 0, 0; data[0] += 2; // only acc_x, but measure gravity!
+    data << -gravity(), 0, 0, 0;
+    data[0] += 2;  // only acc_x, but measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.1);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.1);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     VectorXd x(10);
-    x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
-    Vector6d b; b << 0,0,0, 0,0,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
+    x << 0.01, 0, 0, 0, 0, 0, 1, 0.2, 0,
+        0;  // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
+    Vector6d b;
+    b << 0, 0, 0, 0, 0, 0;
+
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL);
 }
 
 TEST_F(ProcessorImu3dTest, acc_y)
@@ -248,366 +173,321 @@ TEST_F(ProcessorImu3dTest, acc_y)
     // last part of this test fails with precision Constants::EPS_SMALL beacause error is in 1e-12
     // difference hier is that we integrate over 1ms periods
 
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
-
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    data << -gravity(), 0, 0, 0; data[1] += 20; // only acc_y, but measure gravity!
+    data << -gravity(), 0, 0, 0;
+    data[1] += 20;  // only acc_y, but measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     VectorXd x(10);
-    x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
-    Vector6d b; b<< 0,0,0, 0,0,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
-
-    //do so for 5s
-    const unsigned int iter = 1000; //how many ms
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    x << 0, 0.00001, 0, 0, 0, 0, 1, 0, 0.02,
+        0;  // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
+    Vector6d b;
+    b << 0, 0, 0, 0, 0, 0;
+
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL);
+
+    // do so for 5s
+    const unsigned int iter = 1000;                    // how many ms
+    for (unsigned int i = 1; i < iter; i++)            // already did one integration above
     {
-        cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * 0.001 + 0.001);  // first one will be 0.002 and last will be 5.000
+        sensor->process(capture_imu);
     }
 
     // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
-    x << 0,10,0, 0,0,0,1, 0,20,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
+    x << 0, 10, 0, 0, 0, 0, 1, 0, 20, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, acc_z)
 {
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
-
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    data << -gravity(), 0, 0, 0; data[2] += 2.0; // only acc_z, but measure gravity!
+    data << -gravity(), 0, 0, 0;
+    data[2] += 2.0;  // only acc_z, but measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.1);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.1);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     VectorXd x(10);
-    x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
-    Vector6d b; b<< 0,0,0, 0,0,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV").head(10) , x, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
-
-    //do so for 5s
-    const unsigned int iter = 50; //how 0.1s 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    x << 0, 0, 0.01, 0, 0, 0, 1, 0, 0,
+        0.2;  // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
+    Vector6d b;
+    b << 0, 0, 0, 0, 0, 0;
+
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV").head(10), x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL);
+
+    // do so for 5s
+    const unsigned int iter = 50;                  // how 0.1s
+    for (unsigned int i = 1; i < iter; i++)        // already did one integration above
     {
-        cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * 0.1 + 0.1);  // first one will be 0.2 and last will be 5.0
+        sensor->process(capture_imu);
     }
 
     // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
-    x << 0,0,25, 0,0,0,1, 0,0,10;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS);
-    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
+    x << 0, 0, 25, 0, 0, 0, 1, 0, 0, 10;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, check_Covariance)
 {
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
-
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    data << -gravity(), 0, 0, 0; data[0] += 2; // only acc_x, but measure gravity!
+    data << -gravity(), 0, 0, 0;
+    data[0] += 2;  // only acc_x, but measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.1);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.1);
+    sensor->process(capture_imu);
 
-    VectorXd delta_preint(processor_motion->getMotion().delta_integr_);
-//    Matrix<double,9,9> delta_preint_cov = processor_motion->getCurrentDeltaPreintCov();
+    VectorXd delta_preint(processor->getMotion().delta_integr_);
+    //    Matrix<double,9,9> delta_preint_cov = processor->getCurrentDeltaPreintCov();
 
     ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, Constants::EPS_SMALL));
-//    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
+    //    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), Constants::EPS_SMALL); // << "delta_preint_cov
+    //    :\n" << delta_preint_cov; //covariances computed only at keyframe creation
 }
 
 TEST_F(ProcessorImu3dTest, gyro_x)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), rate_of_turn, 0, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), rate_of_turn, 0, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    capture_imu->process();
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0,
+        0;  // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 5s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 5s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity());
+        data.head(3) = rot.conjugate() * (-gravity());
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx)
 {
-    // time
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
 
     // bias
-    double abx = 0.002;
-    Vector6d bias; bias << abx,0,0,  0,0,0;
+    double   abx = 0.002;
+    Vector6d bias;
+    bias << abx, 0, 0, 0, 0, 0;
     Vector3d acc_bias = bias.head(3);
-    // state
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-
-    // init things
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    processor_motion->getOrigin()->getSensorIntrinsic()->setState(bias);
-    processor_motion->getLast()->setCalibrationPreint(bias);
-//    WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
+    processor->getOrigin()->getSensorIntrinsic()->setState(bias);
+    processor->getLast()->setCalibrationPreint(bias);
+    //    WOLF_DEBUG("calib: ", capture_imu->getCalibration().transpose());
 
     // data
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << acc_bias - gravity(), rate_of_turn, 0, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << acc_bias - gravity(), rate_of_turn, 0, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x_true(10);
-    x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+    x_true << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0,
+        0;  // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
     VectorXd x_est(10);
     x_est = problem->getState().vector("POV");
 
-    ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), Constants::EPS);
+    ASSERT_MATRIX_APPROX(x_est.transpose(), x_true.transpose(), Constants::EPS);
 
-    //do so for 5s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 5s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()) + acc_bias;
+        data.head(3) = rot.conjugate() * (-gravity()) + acc_bias;
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x_true, Constants::EPS);
-
+    x_true << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x_true, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
     double abx(0.002), aby(0.01);
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    Vector6d bias; bias << abx,aby,0,  0,0,0;
+    Vector6d bias;
+    bias << abx, aby, 0, 0, 0, 0;
     Vector3d acc_bias = bias.head(3);
 
-    processor_motion->getOrigin()->getSensorIntrinsic()->setState(bias);
-    processor_motion->getLast()->setCalibrationPreint(bias);
+    processor->getOrigin()->getSensorIntrinsic()->setState(bias);
+    processor->getLast()->setCalibrationPreint(bias);
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << acc_bias - gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << acc_bias - gravity(), rate_of_turn * 1.5, 0, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);// << "expected state : " << problem->getState().vector(problem->getFrameStructure()).transpose()
-//    << "\n estimated state : " << x.transpose();
-
-    //do so for 5s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;  //, abx,aby,0, 0,0,0;
+
+    ASSERT_MATRIX_APPROX(
+        problem->getState().vector("POV"),
+        x,
+        Constants::EPS_SMALL);  // << "expected state : " <<
+                                // problem->getState().vector(problem->getFrameStructure()).transpose()
+                                //    << "\n estimated state : " << x.transpose();
+
+    // do so for 5s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()) + acc_bias;
+        data.head(3) = rot.conjugate() * (-gravity()) + acc_bias;
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);// << "estimated state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
-//    "\n expected state is : \n" << x.transpose() << std::endl;
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"),
+                         x,
+                         Constants::EPS);  // << "estimated state is : \n" <<
+                                           // problem->getState().vector(problem->getFrameStructure()).transpose() <<
+    //    "\n expected state is : \n" << x.transpose() << std::endl;
 }
 
 TEST_F(ProcessorImu3dTest, gyro_z)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), 0, 0, rate_of_turn; // measure gravity!
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), 0, 0, rate_of_turn;  // measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0,
+        0;  // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 5s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 5s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        sensor->process(capture_imu);
     }
 
-    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_xyz)
 {
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
-
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    Vector3d tmp_vec; //will be used to store rate of turn data
-    Quaterniond quat_comp(Quaterniond::Identity());
-    Matrix3d R0(Matrix3d::Identity());
-    double time = 0;
+    Vector3d           tmp_vec;  // will be used to store rate of turn data
+    Quaterniond        quat_comp(Quaterniond::Identity());
+    Matrix3d           R0(Matrix3d::Identity());
+    double             time      = 0;
     const unsigned int x_rot_vel = 5;
     const unsigned int y_rot_vel = 6;
     const unsigned int z_rot_vel = 13;
 
     double tmpx, tmpy, tmpz;
-    
+
     /*
         ox oy oz evolution in degrees (for understanding) --> converted in rad
         with * pi/180
@@ -622,284 +502,281 @@ TEST_F(ProcessorImu3dTest, gyro_xyz)
         wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
      */
 
-     const double dt = 0.001;
+    const double dt = 0.001;
 
-    for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
-    {   
+    for (unsigned int data_iter = 0; data_iter <= 1000; data_iter++)
+    {
         time += dt;
 
-        tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
-        tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
-        tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
+        tmpx = M_PI * x_rot_vel * cos((M_PI / 180) * x_rot_vel * time) * M_PI / 180;
+        tmpy = M_PI * y_rot_vel * cos((M_PI / 180) * y_rot_vel * time) * M_PI / 180;
+        tmpz = M_PI * z_rot_vel * cos((M_PI / 180) * z_rot_vel * time) * M_PI / 180;
         tmp_vec << tmpx, tmpy, tmpz;
 
         // quaternion composition
-        quat_comp = quat_comp * v2q(tmp_vec*dt);
-        R0 = R0 * v2R(tmp_vec*dt);
+        quat_comp = quat_comp * v2q(tmp_vec * dt);
+        R0        = R0 * v2R(tmp_vec * dt);
         // use processorImu
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()); //gravity measured
+        data.head(3) = rot.conjugate() * (-gravity());  // gravity measured
         data.tail(3) = tmp_vec;
 
-        cap_imu_ptr->setData(data);
-        cap_imu_ptr->setTimeStamp(time);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setData(data);
+        capture_imu->setTimeStamp(time);
+        sensor->process(capture_imu);
     }
 
     /* We focus on orientation here. position is supposed not to have moved
-     * we integrated using 2 ways : 
+     * we integrated using 2 ways :
         - a direct quaternion composition q = q (x) q(w*dt)
         - using methods implemented in processorImu
 
         We check one against the other
      */
 
-     // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
+    // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
     Quaterniond R2quat(v2q(R2v(R0)));
-    Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
-    Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+    Vector4d    quat_comp_vec((Vector4d() << quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished());
+    Vector4d    R2quat_vec((Vector4d() << R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished());
 
-    ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+    ASSERT_MATRIX_APPROX(
+        quat_comp_vec, R2quat_vec, Constants::EPS);  // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n
+                                                     // R2quat_vec : " << R2quat_vec.transpose() << std::endl;
 
     VectorXd x(10);
-    x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
+    x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0;
 
     Quaterniond result_quat(problem->getState().vector("O").data());
-    //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
+    // std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
 
-    //check position part
+    // check position part
     ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
 
-    //check orientation part
-    ASSERT_MATRIX_APPROX(problem->getState().vector("O"), x.segment(3,4) , Constants::EPS);
+    // check orientation part
+    ASSERT_MATRIX_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
 
-    //check velocity and bias parts
-    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7,3), Constants::EPS);
+    // check velocity and bias parts
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_z_ConstVelocity)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
+
+    // Add velocity to first frame
+    Vector3d v = Vector3d::Random();
+    problem->getLastFrame()->getStateBlock('V')->setState(v);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), 0, 0, rate_of_turn; // measure gravity!
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), 0, 0, rate_of_turn;  // measure gravity!
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
+    x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 1s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 5.000
+        sensor->process(capture_imu);
     }
 
-    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    // check states
+    x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
+    ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
+
+    // Add velocity to first frame
+    Vector3d v = Vector3d::Random();
+    problem->getLastFrame()->getStateBlock('V')->setState(v);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), rate_of_turn, 0, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), rate_of_turn, 0, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(dt);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
-    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+    x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 1s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity());
+        data.head(3) = rot.conjugate() * (-gravity());
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    // check states
+    x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
+    ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
+
+    // Add velocity to first frame
+    Vector3d v = Vector3d::Random();
+    problem->getLastFrame()->getStateBlock('V')->setState(v);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), 0, rate_of_turn, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), 0, rate_of_turn, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
-    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+    x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 1s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity());
+        data.head(3) = rot.conjugate() * (-gravity());
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    // check states
+    x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
+    ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
+
+    // Add velocity to first frame
+    Vector3d v = Vector3d::Random();
+    problem->getLastFrame()->getStateBlock('V')->setState(v);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << -gravity(), 0, rate_of_turn, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << -gravity(), 0, rate_of_turn, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
-    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
-    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+    x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL);
 
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    // do so for 1s
+    const unsigned int iter = 1000;          // how many ms
+    for (unsigned int i = 1; i < iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity());
+        data.head(3) = rot.conjugate() * (-gravity());
 
-        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt + dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
-    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    // check states
+    x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
+    ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity)
 {
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
+    double dt(0.001);
+
+    // Add velocity to first frame
+    Vector3d v = Vector3d::Random();
+    problem->getLastFrame()->getStateBlock('V')->setState(v);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    Vector3d tmp_vec; //will be used to store rate of turn data
-    Quaterniond quat_comp(Quaterniond::Identity());
-    Matrix3d R0(Matrix3d::Identity());
-    double time = 0;
+    Vector3d           tmp_vec;  // will be used to store rate of turn data
+    Quaterniond        quat_comp(Quaterniond::Identity());
+    Matrix3d           R0(Matrix3d::Identity());
+    double             time      = 0;
     const unsigned int x_rot_vel = 5;
     const unsigned int y_rot_vel = 6;
     const unsigned int z_rot_vel = 13;
 
     double tmpx, tmpy, tmpz;
-    
+
     /*
         ox oy oz evolution in degrees (for understanding) --> converted in rad
         with * pi/180
@@ -913,236 +790,215 @@ TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity)
         wy = pi*beta*cos(beta*t*pi/180)*pi/180;
         wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
      */
-
-     const double dt = 0.001;
-
-    for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
-    {   
+    auto iter = 1000;
+    for (unsigned int data_iter = 0; data_iter < iter; data_iter++)
+    {
         time += dt;
 
-        tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
-        tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
-        tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
+        tmpx = M_PI * x_rot_vel * cos((M_PI / 180) * x_rot_vel * time) * M_PI / 180;
+        tmpy = M_PI * y_rot_vel * cos((M_PI / 180) * y_rot_vel * time) * M_PI / 180;
+        tmpz = M_PI * z_rot_vel * cos((M_PI / 180) * z_rot_vel * time) * M_PI / 180;
         tmp_vec << tmpx, tmpy, tmpz;
 
         // quaternion composition
-        quat_comp = quat_comp * v2q(tmp_vec*dt);
-        R0 = R0 * v2R(tmp_vec*dt);
+        quat_comp = quat_comp * v2q(tmp_vec * dt);
+        R0        = R0 * v2R(tmp_vec * dt);
         // use processorImu
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()); //gravity measured
+        data.head(3) = rot.conjugate() * (-gravity());  // gravity measured
         data.tail(3) = tmp_vec;
 
-        cap_imu_ptr->setData(data);
-        cap_imu_ptr->setTimeStamp(time);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setData(data);
+        capture_imu->setTimeStamp(time);
+        sensor->process(capture_imu);
     }
 
     /* We focus on orientation here. position is supposed not to have moved
-     * we integrated using 2 ways : 
+     * we integrated using 2 ways :
         - a direct quaternion composition q = q (x) q(w*dt)
         - using methods implemented in processorImu
 
         We check one against the other
      */
 
-     // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
+    // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
     Quaterniond R2quat(v2q(R2v(R0)));
-    Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
-    Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+    Vector4d    quat_comp_vec((Vector4d() << quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished());
+    Vector4d    R2quat_vec((Vector4d() << R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished());
 
-    ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+    ASSERT_MATRIX_APPROX(
+        quat_comp_vec, R2quat_vec, Constants::EPS);  // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n
+                                                     // R2quat_vec : " << R2quat_vec.transpose() << std::endl;
 
     VectorXd x(10);
-    //rotation + translation due to initial velocity
-    x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
+    // rotation + translation due to initial velocity
+    x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v;
 
     Quaterniond result_quat(problem->getState().vector("O").data());
-    //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
-
-    //check position part
-    ASSERT_MATRIX_APPROX(problem->getState().vector("P") , x.head(3), Constants::EPS);
-
-    //check orientation part
-    ASSERT_MATRIX_APPROX(problem->getState().vector("O") , x.segment(3,4) , Constants::EPS);
-
-    //check velocity
-    ASSERT_MATRIX_APPROX(problem->getState().vector("V") , x.segment(7,3), Constants::EPS);
+    // std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
 
+    // check states
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
+    ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_x_acc_x)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << 1, 0, -gravity()(2), rate_of_turn, 0, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << 1, 0, -gravity()(2), rate_of_turn, 0, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
-    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
-    // v = a*dt = 0.001
-    x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
-//    "\n current x is : \n" << x.transpose() << std::endl;
-
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt =
+    // 0.5*1*0.001^2 = 0.0000005 on x axis v = a*dt = 0.001
+    x << 0.0000005, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0.001, 0, 0;
+
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"),
+                         x,
+                         Constants::EPS);  // << "1. current state is : \n" <<
+                                           // problem->getState().vector(problem->getFrameStructure()).transpose() <<
+                                           //    "\n current x is : \n" << x.transpose() << std::endl;
+
+    // do so for 1s
+    const unsigned int iter = 1000;           // how many ms
+    for (unsigned int i = 2; i <= iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<1,0,0).finished();
+        data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 1, 0, 0).finished();
 
-        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
     // v = a*dt = 1
-    x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    x << 0.5, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 1, 0, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_y_acc_y)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << 0, 1, -gravity()(2), 0, rate_of_turn, 0; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << 0, 1, -gravity()(2), 0, rate_of_turn, 0;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
-    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
-    // v = a*dt = 0.001
-    x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
-//    "\n current x is : \n" << x.transpose() << std::endl;
-
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt =
+    // 0.5*1*0.001^2 = 0.0000005 on y axis v = a*dt = 0.001
+    x << 0, 0.0000005, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0.001, 0;
+
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"),
+                         x,
+                         Constants::EPS);  // << "1. current state is : \n" <<
+                                           // problem->getState().vector(problem->getFrameStructure()).transpose() <<
+                                           //    "\n current x is : \n" << x.transpose() << std::endl;
+
+    // do so for 1s
+    const unsigned int iter = 1000;           // how many ms
+    for (unsigned int i = 2; i <= iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<0,1,0).finished();
+        data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 0, 1, 0).finished();
 
-        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
     // v = a*dt = 1
-    x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    x << 0, 0.5, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 1, 0;
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 }
 
 TEST_F(ProcessorImu3dTest, gyro_z_acc_z)
 {
     double dt(0.001);
-    t.set(0); // clock in 0,1 ms ticks
-    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    // MatrixXd P0(9,9); P0.setIdentity();
-    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    // problem->setPriorFactor(x0c, s0c, t);
-    problem->setPrior(problem_prior_, t);
 
     // process this capture for joining prior KF (t=0) and set it as origin KF
-    cap_imu_ptr->setTimeStamp(t);
-    cap_imu_ptr->process();
+    capture_imu->setTimeStamp(0);
+    capture_imu->process();
 
-    double rate_of_turn = 5 * M_PI/180.0;
-    data << 0, 0, -gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
+    double rate_of_turn = 5 * M_PI / 180.0;
+    data << 0, 0, -gravity()(2) + 1, 0, 0, rate_of_turn;  // measure gravity
 
-    cap_imu_ptr->setData(data);
-    cap_imu_ptr->setTimeStamp(0.001);
-    sensor_ptr->process(cap_imu_ptr);
+    capture_imu->setData(data);
+    capture_imu->setTimeStamp(0.001);
+    sensor->process(capture_imu);
 
     // Expected state after one integration
     Quaterniond quat_comp(Quaterniond::Identity());
-    quat_comp = quat_comp * v2q(data.tail(3)*dt);
+    quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
     VectorXd x(10);
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
-    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
-    // v = a*dt = 0.001
-    x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt =
+    // 0.5*1*0.001^2 = 0.0000005 on z axis v = a*dt = 0.001
+    x << 0, 0, 0.0000005, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0.001;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 
-    //do so for 1s
-    const unsigned int iter = 1000; //how many ms 
-    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    // do so for 1s
+    const unsigned int iter = 1000;           // how many ms
+    for (unsigned int i = 2; i <= iter; i++)  // already did one integration above
     {
         // quaternion composition
-        quat_comp = quat_comp * v2q(data.tail(3)*dt);
+        quat_comp = quat_comp * v2q(data.tail(3) * dt);
 
         Quaterniond rot(problem->getState().vector("O").data());
-        data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<0,0,1).finished();
+        data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 0, 0, 1).finished();
 
-        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
-        cap_imu_ptr->setData(data);
-        sensor_ptr->process(cap_imu_ptr);
+        capture_imu->setTimeStamp(i * dt);  // first one will be 0.002 and last will be 1.000
+        capture_imu->setData(data);
+        sensor->process(capture_imu);
     }
 
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
     // v = a*dt = 1
-    x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
+    x << 0, 0, 0.5, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 1;
     WOLF_DEBUG(problem->getState().vector("POV").transpose());
-    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS);
 }
 
 int main(int argc, char **argv)
 {
-  testing::InitGoogleTest(&argc, argv);
-  //::testing::GTEST_FLAG(filter) = "ProcessorImu3dTest.check_Covariance";
-  return RUN_ALL_TESTS();
+    testing::InitGoogleTest(&argc, argv);
+    // testing::GTEST_FLAG(filter) = "ProcessorImu3dTest.gyro_x_ConstVelocity";
+    return RUN_ALL_TESTS();
 }
-
diff --git a/test/gtest_processor_imu_3d_jacobians.cpp b/test/gtest_processor_imu_3d_jacobians.cpp
index ef429cf072840049e14d53d86e846ccc3d0ae6e0..e4575c131cde13c7ab111c5aad6e401e059fcdbd 100644
--- a/test/gtest_processor_imu_3d_jacobians.cpp
+++ b/test/gtest_processor_imu_3d_jacobians.cpp
@@ -18,7 +18,7 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
- //Wolf
+// Wolf
 #include "imu/capture/capture_imu.h"
 #include "imu/sensor/sensor_imu.h"
 #include "processor_imu_3d_tester.h"
@@ -32,171 +32,154 @@
 #include <ctime>
 #include <cmath>
 
-//google test
+// google test
 #include <core/utils/utils_gtest.h>
 
-//#define DEBUG_RESULTS
-//#define WRITE_RESULTS
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
 using namespace wolf;
 
 // A new one of these is created for each test
 class ProcessorImu3dJacobians : public testing::Test
 {
-    public:
-        TimeStamp t;
-        Eigen::Vector6d data_;
-        Eigen::Matrix<double,10,1> Delta0;
-        double ddelta_bias;
-        double dt;
-        Eigen::Matrix<double,9,1> Delta_noise;
-        Eigen::Matrix<double,9,1> delta_noise;
-        struct Imu_jac_bias bias_jac;
-        struct Imu_jac_deltas deltas_jac;
-
-        void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
-
-            new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
-            new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
-        }
-
-        void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
-    
-            assert(place < _jac_delta.Delta_noisy_vect_.size());
-            new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
-            new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
-        }
+  public:
+    TimeStamp                    t;
+    Eigen::Vector6d              data_;
+    Eigen::Matrix<double, 10, 1> Delta0;
+    double                       ddelta_bias;
+    double                       dt;
+    Eigen::Matrix<double, 9, 1>  Delta_noise;
+    Eigen::Matrix<double, 9, 1>  delta_noise;
+    struct Imu_jac_bias          bias_jac;
+    struct Imu_jac_deltas        deltas_jac;
+
+    void remapJacDeltas_quat0(Imu_jac_deltas&                 _jac_delta,
+                              Eigen::Map<Eigen::Quaterniond>& _Dq0,
+                              Eigen::Map<Eigen::Quaterniond>& _dq0)
+    {
+        new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
+        new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
+    }
+
+    void remapJacDeltas_quat(Imu_jac_deltas&                 _jac_delta,
+                             Eigen::Map<Eigen::Quaterniond>& _Dq,
+                             Eigen::Map<Eigen::Quaterniond>& _dq,
+                             const int&                      place)
+    {
+        assert(place < _jac_delta.Delta_noisy_vect_.size());
+        new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+        new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+    }
 
     void SetUp() override
     {
-        //SetUp for jacobians 
-        double deg_to_rad = M_PI/180.0;
-        data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
+        // SetUp for jacobians
+        double deg_to_rad = M_PI / 180.0;
+        data_ << 10, 0.5, 3, 100 * deg_to_rad, 110 * deg_to_rad, 30 * deg_to_rad;
 
         // Wolf problem
-        ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
-        Eigen::VectorXd Imu_extrinsics(7);
-        Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
+        ProblemPtr problem = Problem::create("POV", 3);
 
-        ProcessorImu3dTester processor_imu;
+        auto processor_imu = std::static_pointer_cast<ProcessorImu3dTester>(
+            ProcessorImu3dTester::create(imu_dir + "/test/yaml/processor_imu_3d.yaml", {imu_dir, wolf_schema_dir}));
         ddelta_bias = 0.00000001;
-        dt = 0.001;
+        dt          = 0.001;
 
-        //defining a random Delta to begin with (not to use Origin point)
-        Eigen::Matrix<double,10,1> Delta0;
-        Delta0 = Eigen::Matrix<double,10,1>::Random();
-        Delta0.head<3>() = Delta0.head<3>()*100;
-        Delta0.tail<3>() = Delta0.tail<3>()*10;
+        // defining a random Delta to begin with (not to use Origin point)
+        Eigen::Matrix<double, 10, 1> Delta0;
+        Delta0           = Eigen::Matrix<double, 10, 1>::Random();
+        Delta0.head<3>() = Delta0.head<3>() * 100;
+        Delta0.tail<3>() = Delta0.tail<3>() * 10;
         Eigen::Vector3d ang0, ang;
-        ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad; 
+        ang0 << 120.08 * deg_to_rad, 12.36 * deg_to_rad, 54.32 * deg_to_rad;
 
-        Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data()+3);
+        Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data() + 3);
         Delta0_quat = v2q(ang0);
         Delta0_quat.normalize();
         ang = q2v(Delta0_quat);
 
-        //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
-        //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
+        // std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
+        // std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
 
-        //get data to compute jacobians
-        struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
+        // get data to compute jacobians
+        struct Imu_jac_bias bias_jac_c = processor_imu->finite_diff_ab(dt, data_, ddelta_bias, Delta0);
         bias_jac.copyfrom(bias_jac_c);
 
-        Delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
-        delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
+        Delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
+        delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
 
-        struct Imu_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
+        struct Imu_jac_deltas deltas_jac_c =
+            processor_imu->finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
         deltas_jac = deltas_jac_c;
     }
-
-    void TearDown() override
-    {
-        // code here will be called just after the test completes
-        // ok to through exceptions from here if need be
-        /*
-            You can do deallocation of resources in TearDown or the destructor routine. 
-                However, if you want exception handling you must do it only in the TearDown code because throwing an exception 
-                from the destructor results in undefined behavior.
-            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. 
-                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
-        */
-    }
 };
 
 class ProcessorImu3dJacobians_Dq : public testing::Test
 {
-    public:
-        TimeStamp t;
-        Eigen::Vector6d data_;
-        Eigen::Matrix<double,10,1> Delta0;
-        double ddelta_bias2;
-        double dt;
-        struct Imu_jac_bias bias_jac2;
-
-        void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
-
-            new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
-            new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
-        }
-
-        void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
-    
-            assert(place < _jac_delta.Delta_noisy_vect_.size());
-            new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
-            new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
-        }
+  public:
+    TimeStamp                    t;
+    Eigen::Vector6d              data_;
+    Eigen::Matrix<double, 10, 1> Delta0;
+    double                       ddelta_bias2;
+    double                       dt;
+    struct Imu_jac_bias          bias_jac2;
+
+    void remapJacDeltas_quat0(Imu_jac_deltas&                 _jac_delta,
+                              Eigen::Map<Eigen::Quaterniond>& _Dq0,
+                              Eigen::Map<Eigen::Quaterniond>& _dq0)
+    {
+        new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
+        new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
+    }
+
+    void remapJacDeltas_quat(Imu_jac_deltas&                 _jac_delta,
+                             Eigen::Map<Eigen::Quaterniond>& _Dq,
+                             Eigen::Map<Eigen::Quaterniond>& _dq,
+                             const int&                      place)
+    {
+        assert(place < _jac_delta.Delta_noisy_vect_.size());
+        new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+        new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+    }
 
     void SetUp() override
     {
-        //SetUp for jacobians 
-        double deg_to_rad = M_PI/180.0;
-        data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
+        // SetUp for jacobians
+        double deg_to_rad = M_PI / 180.0;
+        data_ << 10, 0.5, 3, 100 * deg_to_rad, 110 * deg_to_rad, 30 * deg_to_rad;
 
         // Wolf problem
-        ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
-        Eigen::VectorXd Imu_extrinsics(7);
-        Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
+        ProblemPtr problem = Problem::create("POV", 3);
 
-        ProcessorImu3dTester processor_imu;
+        auto processor_imu = std::static_pointer_cast<ProcessorImu3dTester>(
+            ProcessorImu3dTester::create(imu_dir + "/test/yaml/processor_imu_3d.yaml", {imu_dir, wolf_schema_dir}));
         ddelta_bias2 = 0.0001;
-        dt = 0.001;
+        dt           = 0.001;
 
-        //defining a random Delta to begin with (not to use Origin point)
-        Eigen::Matrix<double,10,1> Delta0;
-        Delta0 = Eigen::Matrix<double,10,1>::Random();
-        Delta0.head<3>() = Delta0.head<3>()*100;
-        Delta0.tail<3>() = Delta0.tail<3>()*10;
+        // defining a random Delta to begin with (not to use Origin point)
+        Eigen::Matrix<double, 10, 1> Delta0;
+        Delta0           = Eigen::Matrix<double, 10, 1>::Random();
+        Delta0.head<3>() = Delta0.head<3>() * 100;
+        Delta0.tail<3>() = Delta0.tail<3>() * 10;
         Eigen::Vector3d ang0, ang;
-        ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad; 
+        ang0 << 120.08 * deg_to_rad, 12.36 * deg_to_rad, 54.32 * deg_to_rad;
 
-        Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data()+3);
+        Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data() + 3);
         Delta0_quat = v2q(ang0);
         Delta0_quat.normalize();
         ang = q2v(Delta0_quat);
 
-        //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
-        //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
+        // std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
+        // std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
 
-        //get data to compute jacobians
-        struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
+        // get data to compute jacobians
+        struct Imu_jac_bias bias_jac_c = processor_imu->finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
         bias_jac2.copyfrom(bias_jac_c);
     }
-
-    void TearDown() override
-    {
-        // code here will be called just after the test completes
-        // ok to through exceptions from here if need be
-        /*
-            You can do deallocation of resources in TearDown or the destructor routine. 
-                However, if you want exception handling you must do it only in the TearDown code because throwing an exception 
-                from the destructor results in undefined behavior.
-            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. 
-                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
-        */
-    }
 };
 
-                            ///BIAS TESTS
+/// BIAS TESTS
 
 /*                              Imu_jac_deltas struct form :
     contains vectors of size 7 :
@@ -222,10 +205,10 @@ class ProcessorImu3dJacobians_Dq : public testing::Test
         dDq_dwb_y = log( dR(wb).transpose() * dR(wb - dwb_y))/dwb_y
         dDq_dwb_z = log( dR(wb).transpose() * dR(wb + dwb_z))/dwb_z
 
-        Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical 
+        Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical
         one will have no sense if we aredoing this from a random Delta. The Delta here should be the origin.
         dDq_dwb_ = dR.tr()*dDq_dwb - Jr(wdt)*dt
-        Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt 
+        Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
 */
 
 TEST_F(ProcessorImu3dJacobians, dDp_dab)
@@ -233,11 +216,14 @@ TEST_F(ProcessorImu3dJacobians, dDp_dab)
     using namespace wolf;
     Eigen::Matrix3d dDp_dab;
 
-    for(int i=0;i<3;i++)
-         dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
+    for (int i = 0; i < 3; i++)
+        dDp_dab.block<3, 1>(0, i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3)) / ddelta_bias;
 
-    ASSERT_TRUE((dDp_dab - bias_jac.dDp_dab_).isMuchSmallerThan(1,0.000001)) << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ <<
-     "\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
+    ASSERT_TRUE((dDp_dab - bias_jac.dDp_dab_).isMuchSmallerThan(1, 0.000001))
+        << "dDp_dab : \n"
+        << dDp_dab << "\n bias_jac.dDp_dab_ :\n"
+        << bias_jac.dDp_dab_ << "\ndDp_dab_a - dDp_dab_ : \n"
+        << bias_jac.dDp_dab_ - dDp_dab << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDv_dab)
@@ -245,11 +231,14 @@ TEST_F(ProcessorImu3dJacobians, dDv_dab)
     using namespace wolf;
     Eigen::Matrix3d dDv_dab;
 
-    for(int i=0;i<3;i++)
-         dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
+    for (int i = 0; i < 3; i++)
+        dDv_dab.block<3, 1>(0, i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3)) / ddelta_bias;
 
-    ASSERT_TRUE((dDv_dab - bias_jac.dDv_dab_).isMuchSmallerThan(1,0.000001)) << "dDv_dab : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ <<
-     "\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
+    ASSERT_TRUE((dDv_dab - bias_jac.dDv_dab_).isMuchSmallerThan(1, 0.000001))
+        << "dDv_dab : \n"
+        << dDv_dab << "\n bias_jac.dDv_dab_ :\n"
+        << bias_jac.dDv_dab_ << "\ndDv_dab_a - dDv_dab_ : \n"
+        << bias_jac.dDv_dab_ - dDv_dab << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDp_dwb)
@@ -257,11 +246,15 @@ TEST_F(ProcessorImu3dJacobians, dDp_dwb)
     using namespace wolf;
     Eigen::Matrix3d dDp_dwb;
 
-    for(int i=0;i<3;i++)
-         dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
+    for (int i = 0; i < 3; i++)
+        dDp_dwb.block<3, 1>(0, i) =
+            (bias_jac.Deltas_noisy_vect_(i + 3).head(3) - bias_jac.Delta0_.head(3)) / ddelta_bias;
 
-    ASSERT_TRUE((dDp_dwb - bias_jac.dDp_dwb_).isMuchSmallerThan(1,0.000001)) << "dDp_dwb : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ <<
-     "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
+    ASSERT_TRUE((dDp_dwb - bias_jac.dDp_dwb_).isMuchSmallerThan(1, 0.000001))
+        << "dDp_dwb : \n"
+        << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n"
+        << bias_jac.dDp_dwb_ << "\ndDp_dwb_a - dDv_dab_ : \n"
+        << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDv_dwb_)
@@ -269,23 +262,28 @@ TEST_F(ProcessorImu3dJacobians, dDv_dwb_)
     using namespace wolf;
     Eigen::Matrix3d dDv_dwb;
 
-    for(int i=0;i<3;i++)
-         dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
+    for (int i = 0; i < 3; i++)
+        dDv_dwb.block<3, 1>(0, i) =
+            (bias_jac.Deltas_noisy_vect_(i + 3).tail(3) - bias_jac.Delta0_.tail(3)) / ddelta_bias;
 
-    ASSERT_TRUE((dDv_dwb - bias_jac.dDv_dwb_).isMuchSmallerThan(1,0.000001)) << "dDv_dwb : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ <<
-     "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
+    ASSERT_TRUE((dDv_dwb - bias_jac.dDv_dwb_).isMuchSmallerThan(1, 0.000001))
+        << "dDv_dwb : \n"
+        << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n"
+        << bias_jac.dDv_dwb_ << "\ndDv_dwb_a - dDv_dwb_ : \n"
+        << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDq_dab)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
-    Eigen::Matrix3d dDq_dab;
+    Eigen::Matrix3d                dDq_dab;
 
     new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac.Delta0_.data() + 3);
-    for(int i=0;i<3;i++){
+    for (int i = 0; i < 3; i++)
+    {
         new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
-        dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
+        dDq_dab.block<3, 1>(0, i) = R2v(q_in_1.matrix().transpose() * q_in_2.matrix()) / ddelta_bias;
     }
 
     ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
@@ -295,48 +293,52 @@ TEST_F(ProcessorImu3dJacobians, dDq_dwb_noise_1Em8_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
-    Eigen::Matrix3d dDq_dwb;
+    Eigen::Matrix3d                dDq_dwb;
 
     new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac.Delta0_.data() + 3);
 
-    for(int i=0;i<3;i++){
-        new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
+    for (int i = 0; i < 3; i++)
+    {
+        new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i + 3).data() + 3);
 
-        dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
+        dDq_dwb.block<3, 1>(0, i) = R2v(q_in_1.matrix().transpose() * q_in_2.matrix()) / ddelta_bias;
     }
 
-    ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001))
-        << "dDq_dwb : \n" << dDq_dwb
-        << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_
-        << "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb
-        << "\n R1^T * R2 : \n"  << q_in_1.matrix().transpose() * q_in_2.matrix()
-        << std::endl;
+    ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1, 0.000001))
+        << "dDq_dwb : \n"
+        << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n"
+        << bias_jac.dDq_dwb_ << "\ndDq_dwb_a - dDq_dwb_ : \n"
+        << bias_jac.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n"
+        << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
-    Eigen::Matrix3d dDq_dwb;
+    Eigen::Matrix3d                dDq_dwb;
 
     new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac2.Delta0_.data() + 3);
-    for(int i=0;i<3;i++){
-        new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac2.Deltas_noisy_vect_(i+3).data() + 3);
-        dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias2;
+    for (int i = 0; i < 3; i++)
+    {
+        new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac2.Deltas_noisy_vect_(i + 3).data() + 3);
+        dDq_dwb.block<3, 1>(0, i) = R2v(q_in_1.matrix().transpose() * q_in_2.matrix()) / ddelta_bias2;
     }
 
-    ASSERT_TRUE((dDq_dwb - bias_jac2.dDq_dwb_).isMuchSmallerThan(1,0.001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac2.dDq_dwb_ :\n" << bias_jac2.dDq_dwb_ <<
-     "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac2.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n"  << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
+    ASSERT_TRUE((dDq_dwb - bias_jac2.dDq_dwb_).isMuchSmallerThan(1, 0.001))
+        << "dDq_dwb : \n"
+        << dDq_dwb << "\n bias_jac2.dDq_dwb_ :\n"
+        << bias_jac2.dDq_dwb_ << "\ndDq_dwb_a - dDq_dwb_ : \n"
+        << bias_jac2.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n"
+        << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
 }
 
-                                ///Perturbation TESTS
+/// Perturbation TESTS
 
-/* reminder : 
-                            jacobian_delta_preint_vect_                                                            jacobian_delta_vect_
-                            0: + 0,                                                                                 0: + 0
-                            1: +dPx, 2: +dPy, 3: +dPz                                                               1: + dpx, 2: +dpy, 3: +dpz
-                            4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox, 5: +doy, 6: +doz
-                            7: +dVx, 8: +dVy, 9: +dVz                                                               7: + dvx, 8: +dvy, 9: +dvz
+/* reminder :
+                            jacobian_delta_preint_vect_ jacobian_delta_vect_ 0: + 0, 0: + 0 1: +dPx, 2: +dPy, 3: +dPz
+   1: + dpx, 2: +dpy, 3: +dpz 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx,
+   8: +dvy, 9: +dvz
 */
 
 /*              Numerical method to check jacobians wrt noise
@@ -352,7 +354,7 @@ TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDp_dVz = ((V + dVz)*dt - V*dt)/dVz = Id*dt
 
                                                             dDp_dO = [dDp_dOx, dDp_dOy, dDp_dOz]
-    dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax 
+    dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax
             = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
     dDp_dOy = (( dR(Theta) * exp(0,dThetay,0)*dp ) - ( dR(Theta)*dp ))/dThetay
     dDp_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dp ) - ( dR(Theta)*dp ))/dThetaz
@@ -363,9 +365,9 @@ TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDv_dVx = ((V + dVx) - V)/dVx = Id
     dDv_dVy = ((V + dVy) - V)/dVy = Id
     dDv_dVz = ((V + dVz) - V)/dVz = Id
-    
+
                                                             dDv_dO = [dDv_dOx, dDv_dOy, dDv_dOz]
-    dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax 
+    dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax
             = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
     dDv_dOx = (( dR(Theta) * exp(0,dThetay,0)*dv ) - ( dR(Theta)*dv ))/dThetay
     dDv_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dv ) - ( dR(Theta)*dv ))/dThetaz
@@ -376,11 +378,11 @@ TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDp_dpz = ( dR*(p + dpz) - dR*(p))/dpy
 
                                                             dDp_dv = [dDp_dvx, dDp_dvy, dDp_dvz] = [0, 0, 0]
-    
+
                                                             dDp_do = [dDp_dox, dDp_doy, dDp_doz] = [0, 0, 0]
 
                                                             dDv_dp = [dDv_dpx, dDv_dpy, dDv_dpz] = [0, 0, 0]
-                                                            
+
                                                             dDv_dv = [dDv_dvx, dDv_dvy, dDv_dvz]
     dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
     dDv_dvy = ( dR*(v + dvy) - dR*(v))/dvy
@@ -391,7 +393,7 @@ TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
                                                             dDo_dp = dDo_dv = dDo_dP = dDo_dV = [0, 0, 0]
 
                                                             dDo_dO = [dDo_dOx, dDo_dOy, dDo_dOz]
-                                                            
+
     dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta+dThetax) * dr(theta) )/dThetax
             = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
             = log( (_Delta * _delta).transpose() * (_Delta_noisy * _delta))
@@ -406,18 +408,22 @@ TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
     dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
      */
-     
+
 TEST_F(ProcessorImu3dJacobians, dDp_dP)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dP;
 
-    //dDp_dPx = ((P + dPx) - P)/dPx
-    for(int i=0;i<3;i++)
-        dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
+    // dDp_dPx = ((P + dPx) - P)/dPx
+    for (int i = 0; i < 3; i++)
+        dDp_dP.block<3, 1>(0, i) =
+            (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3)) / Delta_noise(i);
 
-    ASSERT_TRUE((dDp_dP - deltas_jac.jacobian_delta_preint_.block(0,0,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) <<
-     "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
+    ASSERT_TRUE((dDp_dP - deltas_jac.jacobian_delta_preint_.block(0, 0, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDp_dP : \n"
+        << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 0, 3, 3) << "\ndDp_dP_a - dDp_dP_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 0, 3, 3) - dDp_dP << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDp_dV)
@@ -425,29 +431,39 @@ TEST_F(ProcessorImu3dJacobians, dDp_dV)
     using namespace wolf;
     Eigen::Matrix3d dDp_dV;
 
-    //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
-    for(int i=0;i<3;i++)
-        dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
+    // Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
+    for (int i = 0; i < 3; i++)
+        dDp_dV.block<3, 1>(0, i) =
+            (deltas_jac.Delta_noisy_vect_(i + 6).tail(3) * dt - deltas_jac.Delta0_.tail(3) * dt) / Delta_noise(i + 6);
 
-    ASSERT_TRUE((dDp_dV - deltas_jac.jacobian_delta_preint_.block(0,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) <<
-     "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
+    ASSERT_TRUE((dDp_dV - deltas_jac.jacobian_delta_preint_.block(0, 6, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDp_dV : \n"
+        << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 6, 3, 3) << "\ndDp_dV_a - dDp_dV_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 6, 3, 3) - dDp_dV << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDp_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
-    Eigen::Matrix3d dDp_dO;
+    Eigen::Matrix3d                dDp_dO;
 
-    //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
+    // dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    for(int i=0;i<3;i++){
-        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
-        dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
+    for (int i = 0; i < 3; i++)
+    {
+        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i + 3);
+        dDp_dO.block<3, 1>(0, i) =
+            ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3))) /
+            Delta_noise(i + 3);
     }
 
-    ASSERT_TRUE((dDp_dO - deltas_jac.jacobian_delta_preint_.block(0,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) <<
-     "\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
+    ASSERT_TRUE((dDp_dO - deltas_jac.jacobian_delta_preint_.block(0, 3, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDp_dO : \n"
+        << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 3, 3, 3) << "\ndDp_dO_a - dDp_dO_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(0, 3, 3, 3) - dDp_dO << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDv_dV)
@@ -455,109 +471,136 @@ TEST_F(ProcessorImu3dJacobians, dDv_dV)
     using namespace wolf;
     Eigen::Matrix3d dDv_dV;
 
-    //dDv_dVx = ((V + dVx) - V)/dVx
-    for(int i=0;i<3;i++)
-        dDv_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3) - deltas_jac.Delta0_.tail(3))/Delta_noise(i+6);
+    // dDv_dVx = ((V + dVx) - V)/dVx
+    for (int i = 0; i < 3; i++)
+        dDv_dV.block<3, 1>(0, i) =
+            (deltas_jac.Delta_noisy_vect_(i + 6).tail(3) - deltas_jac.Delta0_.tail(3)) / Delta_noise(i + 6);
 
-    ASSERT_TRUE((dDv_dV - deltas_jac.jacobian_delta_preint_.block(6,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dV : \n" << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) <<
-     "\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
+    ASSERT_TRUE((dDv_dV - deltas_jac.jacobian_delta_preint_.block(6, 6, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDv_dV : \n"
+        << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(6, 6, 3, 3) << "\ndDv_dV_a - dDv_dV_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(6, 6, 3, 3) - dDv_dV << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDv_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
-    Eigen::Matrix3d dDv_dO;
+    Eigen::Matrix3d                dDv_dO;
 
-    //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
+    // dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    for(int i=0;i<3;i++){
-        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
-        dDv_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix()* deltas_jac.delta0_.tail(3)))/Delta_noise(i+3);
+    for (int i = 0; i < 3; i++)
+    {
+        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i + 3);
+        dDv_dO.block<3, 1>(0, i) =
+            ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix() * deltas_jac.delta0_.tail(3))) /
+            Delta_noise(i + 3);
     }
 
-    ASSERT_TRUE((dDv_dO - deltas_jac.jacobian_delta_preint_.block(6,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dO : \n" << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) <<
-     "\ndDv_dO_a - dDv_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << std::endl;
+    ASSERT_TRUE((dDv_dO - deltas_jac.jacobian_delta_preint_.block(6, 3, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDv_dO : \n"
+        << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(6, 3, 3, 3) << "\ndDv_dO_a - dDv_dO_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(6, 3, 3, 3) - dDv_dO << std::endl;
 }
 
-//dDo_dP = dDo_dV = [0, 0, 0]
+// dDo_dP = dDo_dV = [0, 0, 0]
 
 TEST_F(ProcessorImu3dJacobians, dDo_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
-    Eigen::Matrix3d dDo_dO;
+    Eigen::Matrix3d                dDo_dO;
 
-    //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
+    // dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    for(int i=0;i<3;i++){
-        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
-        dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
+    for (int i = 0; i < 3; i++)
+    {
+        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i + 3);
+        dDo_dO.block<3, 1>(0, i) =
+            R2v((Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix())) / Delta_noise(i + 3);
     }
 
-    ASSERT_TRUE((dDo_dO - deltas_jac.jacobian_delta_preint_.block(3,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) <<
-     "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
+    ASSERT_TRUE((dDo_dO - deltas_jac.jacobian_delta_preint_.block(3, 3, 3, 3)).isMuchSmallerThan(1, 0.000001))
+        << "dDo_dO : \n"
+        << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n"
+        << deltas_jac.jacobian_delta_preint_.block(3, 3, 3, 3) << "\ndDo_dO_a - dDo_dO_ : \n"
+        << deltas_jac.jacobian_delta_preint_.block(3, 3, 3, 3) - dDo_dO << std::endl;
 }
 
 TEST_F(ProcessorImu3dJacobians, dDp_dp)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
-    Eigen::Matrix3d dDp_dp, dDp_dp_a;
+    Eigen::Matrix3d                dDp_dp, dDp_dp_a;
 
-    dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
+    dDp_dp_a = deltas_jac.jacobian_delta_.block(0, 0, 3, 3);
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
-    for(int i=0;i<3;i++)
-        dDp_dp.block<3,1>(0,i) = ( (Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3)) )/delta_noise(i);
-
-    ASSERT_TRUE((dDp_dp - dDp_dp_a).isMuchSmallerThan(1,0.000001)) << "dDp_dp : \n" << dDp_dp << "\n dDp_dp_a :\n" << dDp_dp_a <<
-     "\ndDp_dp_a - dDp_dp_ : \n" << dDp_dp_a - dDp_dp << std::endl;
+    // dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
+    for (int i = 0; i < 3; i++)
+        dDp_dp.block<3, 1>(0, i) =
+            ((Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3))) /
+            delta_noise(i);
+
+    ASSERT_TRUE((dDp_dp - dDp_dp_a).isMuchSmallerThan(1, 0.000001)) << "dDp_dp : \n"
+                                                                    << dDp_dp << "\n dDp_dp_a :\n"
+                                                                    << dDp_dp_a << "\ndDp_dp_a - dDp_dp_ : \n"
+                                                                    << dDp_dp_a - dDp_dp << std::endl;
 }
 
-//dDv_dp = [0, 0, 0]
+// dDv_dp = [0, 0, 0]
 
 TEST_F(ProcessorImu3dJacobians, dDv_dv)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
-    Eigen::Matrix3d dDv_dv, dDv_dv_a;
+    Eigen::Matrix3d                dDv_dv, dDv_dv_a;
 
-    dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
+    dDv_dv_a = deltas_jac.jacobian_delta_.block(6, 6, 3, 3);
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
-    for(int i=0;i<3;i++)
-        dDv_dv.block<3,1>(0,i) = ( (Dq0 * deltas_jac.delta_noisy_vect_(i+6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3)) )/delta_noise(i+6);
-
-    ASSERT_TRUE((dDv_dv - dDv_dv_a).isMuchSmallerThan(1,0.000001)) << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a <<
-     "\ndDv_dv_a - dDv_dv_ : \n" << dDv_dv_a - dDv_dv << std::endl;
+    // dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
+    for (int i = 0; i < 3; i++)
+        dDv_dv.block<3, 1>(0, i) =
+            ((Dq0 * deltas_jac.delta_noisy_vect_(i + 6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3))) /
+            delta_noise(i + 6);
+
+    ASSERT_TRUE((dDv_dv - dDv_dv_a).isMuchSmallerThan(1, 0.000001)) << "dDv_dv : \n"
+                                                                    << dDv_dv << "\n dDv_dv_a :\n"
+                                                                    << dDv_dv_a << "\ndDv_dv_a - dDv_dv_ : \n"
+                                                                    << dDv_dv_a - dDv_dv << std::endl;
 }
 
-//dDo_dp = dDo_dv = [0, 0, 0]
+// dDo_dp = dDo_dv = [0, 0, 0]
 
 TEST_F(ProcessorImu3dJacobians, dDo_do)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
-    Eigen::Matrix3d dDo_do, dDo_do_a;
+    Eigen::Matrix3d                dDo_do, dDo_do_a;
 
-    //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
+    // dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
     remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
+    dDo_do_a = deltas_jac.jacobian_delta_.block(3, 3, 3, 3);
 
-    for(int i=0;i<3;i++){
-        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
-        dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
+    for (int i = 0; i < 3; i++)
+    {
+        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i + 3);
+        dDo_do.block<3, 1>(0, i) =
+            R2v((Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix())) / Delta_noise(i + 3);
     }
 
-    ASSERT_TRUE((dDo_do - dDo_do_a).isMuchSmallerThan(1,0.000001)) << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a <<
-     "\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl;
+    ASSERT_TRUE((dDo_do - dDo_do_a).isMuchSmallerThan(1, 0.000001)) << "dDo_do : \n"
+                                                                    << dDo_do << "\n dDo_do_a :\n"
+                                                                    << dDo_do_a << "\ndDo_do_a - dDo_do_ : \n"
+                                                                    << dDo_do_a - dDo_do << std::endl;
 }
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     using namespace wolf;
 
-     testing::InitGoogleTest(&argc, argv);
-     return RUN_ALL_TESTS();
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index b6e170671c5c76a8ce358f05b5fa168410a460c0..0cafdb10a91acd4ef00256da6281ee758c227b59 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -34,194 +34,188 @@
 using namespace wolf;
 using namespace Eigen;
 
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
+/**
+ *  SET TO TRUE  TO PRODUCE CSV FILE
+ *  SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE false
+
 /**
 Simplified test rolling along x axis only:
 Delta_x = 2*pi*R*(ticks/ticks_per_wheel_revolution)
 
 If we take ticks_per_wheel_revolution = 2*pi, even simpler formulat:
 Delta_x = R*ticks
-
 **/
 
-Vector10d integrateX(double ax, double dt){
-    Vector10d x; x << 0.5*ax*pow(dt,2),0,0, 0,0,0,1, ax*dt,0,0;
-    return x; 
+Vector10d integrateX(double ax, double dt)
+{
+    Vector10d x;
+    x << 0.5 * ax * pow(dt, 2), 0, 0, 0, 0, 0, 1, ax * dt, 0, 0;
+    return x;
 }
 
 class ProcessorImuTest : public testing::Test
 {
-    public:
-        ParamsSensorImuPtr    intr_;
-        SensorImu3dPtr          sensor_;
-        ParamsProcessorImuPtr params_;
-        ProcessorImuPtr       processor_;
-        ProblemPtr            problem_;
-        SolverCeresPtr        solver_;
-        FrameBasePtr          KF0_;
-        double                radius;
-
-        void SetUp() override
-        {
-            problem_ = Problem::create("POV", 3);
-            solver_ = std::make_shared<SolverCeres>(problem_);
-
-            // ground truth of wheels radius
-            radius = 1;
-
-            // make a sensor first
-            intr_ = std::make_shared<ParamsSensorImu>();
-            intr_->a_noise = 0.001;
-            intr_->w_noise = 0.001;
-            
-            Vector6d bias; bias << 0.42,0,0, 0,0,0;
-            // Vector6d bias; bias << 0.0,0,0, 0.42,0,0;
-
-            SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())},
-                                       {'O',SpecStateSensor("StateQuaternion",(Vector4d() << 0,0,0,1).finished())},
-                                       {'I',SpecStateSensor("StateParams6",bias,"factor",Vector6d::Constant(0.001),true,Vector6d::Constant(0.001))}};
-            sensor_ = SensorBase::emplace<SensorImu3d>(problem_->getHardware(), intr_, priors);
-
-            // params and processor
-            params_ = std::make_shared<ParamsProcessorImu>();
-            params_->voting_active = true;
-            params_->max_time_span = 0.9999;  // create one KF every second
-            params_->max_buff_length = 1000000000;
-            params_->dist_traveled = 100000000000;
-            params_->angle_turned = 10000000000;
-            
-            params_->unmeasured_perturbation_std = 1e-4;
-            params_->time_tolerance = 0.0025;
-            processor_ = ProcessorBase::emplace<ProcessorImu>(sensor_, params_);
-            
-            // Vector4d q_init; q_init << 0,0,0,1;
-            // VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
-            // VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
-            // KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
-            SpecComposite problem_prior{{'P', SpecState("StatePoint3d",    Vector3d::Zero(),                 "factor", Vector3d::Constant(0.001))},
-                                        {'O', SpecState("StateQuaternion", Quaterniond::Identity().coeffs(), "factor", Vector3d::Constant(0.001))},
-                                        {'V', SpecState("StateVector3d",   Vector3d::Zero(),                 "factor", Vector3d::Constant(0.001))}};
-            KF0_ = problem_->setPrior(problem_prior, 0);
-
-            processor_->setOrigin(KF0_);
-        }
-
-        void TearDown() override{}
-
+  public:
+    SensorImu3dPtr    sensor_;
+    ProcessorImu3dPtr processor_;
+    ProblemPtr        problem_;
+    SolverManagerPtr  solver_;
+    FrameBasePtr      KF0_;
+
+    void SetUp() override
+    {
+        // problem and solver
+        problem_ =
+            Problem::autoSetup(imu_dir + "/test/yaml/processor_motion_intrinsics_update/problem_3d.yaml", {imu_dir});
+        solver_ = FactorySolverFile::create(
+            "SolverCeres", problem_, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
+
+        // sensor and processor
+        sensor_    = std::static_pointer_cast<SensorImu3d>(problem_->findSensor("cool sensor imu"));
+        processor_ = std::static_pointer_cast<ProcessorImu3d>(sensor_->getProcessorList().front());
+
+        // bias
+        Vector6d bias;
+        bias << 0.42, 0, 0, 0, 0, 0;
+        // Vector6d bias; bias << 0.0,0,0, 0.42,0,0;            
+        sensor_->getIntrinsic(0)->setState(bias);
+
+        // set origin
+        KF0_ = problem_->applyPriorOptions(0);
+        processor_->setOrigin(KF0_);
+    }
 };
 
 TEST_F(ProcessorImuTest, test1)
 {
-    Matrix6d data_cov; data_cov. setIdentity();
+    Matrix6d data_cov;
+    data_cov.setIdentity();
     // Advances at constant speed on both wheels
     double AX = 0;
 
-    problem_->print(4,true,true,true);
+    problem_->print(4, true, true, true);
     CaptureMotionPtr CM;
 
     CM = std::static_pointer_cast<CaptureMotion>(KF0_->getCaptureOf(sensor_));
-    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     solver_->solve();
-    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
-
-
-    Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
-    Vector10d state_pred; state_pred << 0,0,0, 0,0,0,1, 0,0,0;
-    auto C1 = std::make_shared<CaptureImu>(1, sensor_, data, data_cov, KF0_->getCaptureList().front()); C1->process();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+
+    Vector6d data;
+    data << -wolf::gravity(), 0, 0, 0;
+    data(0) = AX;
+    Vector10d state_pred;
+    state_pred << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+    auto C1 = std::make_shared<CaptureImu>(1, sensor_, data, data_cov, KF0_->getCaptureList().front());
+    C1->process();
     FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
-    KF1->setState(integrateX(AX,1),"POV"); KF1->fix();
+    KF1->setState(integrateX(AX, 1), "POV");
+    KF1->fix();
     CM = std::static_pointer_cast<CaptureMotion>(KF1->getCaptureOf(sensor_));
-    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     solver_->solve();
-    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     // problem_->print(4,true,true,true);
 
-    auto C2 = std::make_shared<CaptureImu>(2, sensor_, data, data_cov, KF0_->getCaptureList().front()); C2->process();
+    auto C2 = std::make_shared<CaptureImu>(2, sensor_, data, data_cov, KF0_->getCaptureList().front());
+    C2->process();
     FrameBasePtr KF2 = problem_->getTrajectory()->getLastFrame();
-    KF2->setState(integrateX(AX,2),"POV"); KF2->fix();
+    KF2->setState(integrateX(AX, 2), "POV");
+    KF2->fix();
     CM = std::static_pointer_cast<CaptureMotion>(KF2->getCaptureOf(sensor_));
-    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     solver_->solve();
-    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     // problem_->print(4,true,true,true);
 
-    auto C3 = std::make_shared<CaptureImu>(3, sensor_, data, data_cov, KF0_->getCaptureList().front()); C3->process();
+    auto C3 = std::make_shared<CaptureImu>(3, sensor_, data, data_cov, KF0_->getCaptureList().front());
+    C3->process();
     FrameBasePtr KF3 = problem_->getTrajectory()->getLastFrame();
-    KF3->setState(integrateX(AX,3),"POV"); KF3->fix();
+    KF3->setState(integrateX(AX, 3), "POV");
+    KF3->fix();
     CM = std::static_pointer_cast<CaptureMotion>(KF3->getCaptureOf(sensor_));
-    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     solver_->solve();
-    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     // problem_->print(4,true,true,true);
 
-    auto C4 = std::make_shared<CaptureImu>(4, sensor_, data, data_cov, KF0_->getCaptureList().front()); C4->process();
+    auto C4 = std::make_shared<CaptureImu>(4, sensor_, data, data_cov, KF0_->getCaptureList().front());
+    C4->process();
     FrameBasePtr KF4 = problem_->getTrajectory()->getLastFrame();
-    KF4->setState(integrateX(AX,4),"POV"); KF4->fix();
+    KF4->setState(integrateX(AX, 4), "POV");
+    KF4->fix();
     CM = std::static_pointer_cast<CaptureMotion>(KF4->getCaptureOf(sensor_));
-    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     solver_->solve();
-    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose()
+              << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
     // problem_->print(4,true,true,true);
 
-    for(auto kf: problem_->getTrajectory()->getFrameMap()){
+    for (auto kf : problem_->getTrajectory()->getFrameMap())
+    {
         CaptureMotionPtr cap = std::static_pointer_cast<CaptureMotion>(kf.second->getCaptureOf(sensor_));
     }
-
-
 }
 
-
-/**
- *  SET TO TRUE  TO PRODUCE CSV FILE
- *  SET TO FALSE TO STOP PRODUCING CSV FILE
- */
-#define WRITE_CSV_FILE  true
-
 TEST_F(ProcessorImuTest, getState)
 {
-    Matrix6d data_cov; data_cov. setIdentity();
+    Matrix6d data_cov;
+    data_cov.setIdentity();
     // Advances at constant acceleration
     double AX = 0;
 
-    problem_->print(4,true,true,true);
-    Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
+    problem_->print(4, true, true, true);
+    Vector6d data;
+    data << -wolf::gravity(), 0, 0, 0;
+    data(0) = AX;
     CaptureImuPtr C;
 
     TimeStamp t(0);
-    double dt(0.1);
-    int nb_kf = 1;
+    double    dt(0.1);
+    int       nb_kf = 1;
 
 #if WRITE_CSV_FILE
-    std::fstream file_est; 
-    file_est.open("./est.csv", std::fstream::out);
-//    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
-    std::string header_est = "t;px;vx;bax_est;bax_preint\n";
-    file_est << header_est;
+    std::fstream file_est;
+    file_est.open(imu_dir + "/test/yaml/processor_motion_intrinsics_update/est.csv", std::fstream::out);
+    //    file_est << "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+    file_est << "t;px;vx;bax_est;bax_preint\n";
 #endif
 
-    while (t < 4){
-        auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); 
+    while (t < 4)
+    {
+        auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
         C->process();
 
-
-        std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
-
+        std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts "
+                  << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
 
 #if WRITE_CSV_FILE
         // pre-solve print to CSV
-        VectorComposite state = problem_->getState(t);
-        VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
-        VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
-        file_est << std::fixed << t << ";"
-            << state['P'](0) << ";"
-            << state['V'](0) << ";"
-            << calib_estim(0) << ";"
-            << calib_preint(0) << "\n";
+        VectorComposite state        = problem_->getState(t);
+        VectorXd        calib_estim  = sensor_->getIntrinsic(t)->getState();
+        VectorXd        calib_preint = processor_->getLast()->getCalibrationPreint();
+        file_est << std::fixed << t << ";" << state['P'](0) << ";" << state['V'](0) << ";" << calib_estim(0) << ";"
+                 << calib_preint(0) << "\n";
 #endif
 
-
-        if (problem_->getTrajectory()->size() > nb_kf){
+        if (problem_->getTrajectory()->size() > nb_kf)
+        {
             FrameBasePtr KF = problem_->getTrajectory()->getLastFrame();
-            KF->setState(integrateX(AX, KF->getTimeStamp().get()),"POV"); 
+            KF->setState(integrateX(AX, KF->getTimeStamp().get()), "POV");
             KF->fix();
 
             solver_->solve();
@@ -231,14 +225,11 @@ TEST_F(ProcessorImuTest, getState)
 
 #if WRITE_CSV_FILE
             // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
-            state = problem_->getState(t);
-            calib_estim = sensor_->getIntrinsic(t)->getState();
+            state        = problem_->getState(t);
+            calib_estim  = sensor_->getIntrinsic(t)->getState();
             calib_preint = processor_->getLast()->getCalibrationPreint();
-            file_est << std::fixed << t+dt/2 << ";"
-                << state['P'](0) << ";"
-                << state['V'](0) << ";"
-                << calib_estim(0) << ";"
-                << calib_preint(0) << "\n";
+            file_est << std::fixed << t + dt / 2 << ";" << state['P'](0) << ";" << state['V'](0) << ";"
+                     << calib_estim(0) << ";" << calib_preint(0) << "\n";
 #endif
         }
 
@@ -260,19 +251,19 @@ TEST_F(ProcessorImuTest, getState)
      * though we only test at KFs so t = { 0, 1, 2, 3 }
      */
 
-    for (auto F_pair : problem_->getTrajectory()->getFrameMap() )
+    for (auto F_pair : problem_->getTrajectory()->getFrameMap())
     {
         auto ts = F_pair.second->getTimeStamp();
 
         double bias = (t == 0 ? 0.42 : 0.0);
         ASSERT_NEAR(F_pair.second->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
-        ASSERT_NEAR(F_pair.second->getP()->getState()(0),0.0,1e-6);
-        ASSERT_NEAR(F_pair.second->getV()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F_pair.second->getP()->getState()(0), 0.0, 1e-6);
+        ASSERT_NEAR(F_pair.second->getV()->getState()(0), 0.0, 1e-6);
     }
 }
 
 int main(int argc, char **argv)
 {
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
 }
diff --git a/test/processor_imu_2d_tester.cpp b/test/processor_imu_2d_tester.cpp
index 6b3185d857a29e6037ced6d9f0786933e708e630..75082c13322372bd3b61624aa6b21ad7604b92fb 100644
--- a/test/processor_imu_2d_tester.cpp
+++ b/test/processor_imu_2d_tester.cpp
@@ -29,5 +29,8 @@ ProcessorImu2dTester::ProcessorImu2dTester(const YAML::Node& _params) :
 
 ProcessorImu2dTester::~ProcessorImu2dTester(){}
 
+
+// Register in the FactoryProcessor
+WOLF_REGISTER_PROCESSOR(ProcessorImu2dTester)
 } // namespace wolf
 
diff --git a/test/processor_imu_2d_tester.h b/test/processor_imu_2d_tester.h
index 647956b4e7bfceffb226c9a2610feb4a54c60dde..dce8a5154e76761e5a926ee7b35d985edd8be14b 100644
--- a/test/processor_imu_2d_tester.h
+++ b/test/processor_imu_2d_tester.h
@@ -24,190 +24,195 @@
 #include "imu/processor/processor_imu_2d.h"
 #include "core/processor/processor_motion.h"
 
-namespace wolf {
-    struct Imu_jac_bias{ //struct used for checking jacobians by finite difference
-
-        Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
-                     Eigen::VectorXd _Delta0 ,
-                     Eigen::Matrix3d _dDp_dab,
-                     Eigen::Matrix3d _dDv_dab,
-                     Eigen::Matrix3d _dDp_dwb,
-                     Eigen::Matrix3d _dDv_dwb,
-                     Eigen::Matrix3d _dDq_dwb) :
-                         Deltas_noisy_vect_(_Deltas_noisy_vect),
-                         Delta0_(_Delta0) ,
-                         dDp_dab_(_dDp_dab),
-                         dDv_dab_(_dDv_dab),
-                         dDp_dwb_(_dDp_dwb),
-                         dDv_dwb_(_dDv_dwb),
-                         dDq_dwb_(_dDq_dwb)
+namespace wolf
+{
+struct Imu_jac_bias
+{  // struct used for checking jacobians by finite difference
+
+    Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd, 6, 1> _Deltas_noisy_vect,
+                 Eigen::VectorXd                      _Delta0,
+                 Eigen::Matrix3d                      _dDp_dab,
+                 Eigen::Matrix3d                      _dDv_dab,
+                 Eigen::Matrix3d                      _dDp_dwb,
+                 Eigen::Matrix3d                      _dDv_dwb,
+                 Eigen::Matrix3d                      _dDq_dwb)
+        : Deltas_noisy_vect_(_Deltas_noisy_vect),
+          Delta0_(_Delta0),
+          dDp_dab_(_dDp_dab),
+          dDv_dab_(_dDv_dab),
+          dDp_dwb_(_dDp_dwb),
+          dDv_dwb_(_dDv_dwb),
+          dDq_dwb_(_dDq_dwb)
+    {
+        //
+    }
+
+    Imu_jac_bias()
+    {
+        for (int i = 0; i < 6; i++)
         {
-            //
-        }
-                
-        Imu_jac_bias(){
-
-            for (int i=0; i<6; i++){
-                Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-            }
-
-            Delta0_ = Eigen::VectorXd::Zero(1,1);
-            dDp_dab_ = Eigen::Matrix3d::Zero();
-            dDv_dab_ = Eigen::Matrix3d::Zero();
-            dDp_dwb_ = Eigen::Matrix3d::Zero();
-            dDv_dwb_ = Eigen::Matrix3d::Zero();
-            dDq_dwb_ = Eigen::Matrix3d::Zero();
+            Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
         }
 
-        Imu_jac_bias(Imu_jac_bias const & toCopy){
+        Delta0_  = Eigen::VectorXd::Zero(1, 1);
+        dDp_dab_ = Eigen::Matrix3d::Zero();
+        dDv_dab_ = Eigen::Matrix3d::Zero();
+        dDp_dwb_ = Eigen::Matrix3d::Zero();
+        dDv_dwb_ = Eigen::Matrix3d::Zero();
+        dDq_dwb_ = Eigen::Matrix3d::Zero();
+    }
 
-            Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
-            Delta0_  = toCopy.Delta0_;
-            dDp_dab_ = toCopy.dDp_dab_;
-            dDv_dab_ = toCopy.dDv_dab_;
-            dDp_dwb_ = toCopy.dDp_dwb_;
-            dDv_dwb_ = toCopy.dDv_dwb_;
-            dDq_dwb_ = toCopy.dDq_dwb_;
-        }
+    Imu_jac_bias(Imu_jac_bias const& toCopy)
+    {
+        Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
+        Delta0_            = toCopy.Delta0_;
+        dDp_dab_           = toCopy.dDp_dab_;
+        dDv_dab_           = toCopy.dDv_dab_;
+        dDp_dwb_           = toCopy.dDp_dwb_;
+        dDv_dwb_           = toCopy.dDv_dwb_;
+        dDq_dwb_           = toCopy.dDq_dwb_;
+    }
 
-        public:
-            /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
-              place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
-              place 4 : added dw_bx in data         place 5 : added dw_by in data       place 6 : added dw_bz in data
-             */
-            Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect_;
-            Eigen::VectorXd Delta0_;
-            Eigen::Matrix3d dDp_dab_;
-            Eigen::Matrix3d dDv_dab_;
-            Eigen::Matrix3d dDp_dwb_;
-            Eigen::Matrix3d dDv_dwb_;
-            Eigen::Matrix3d dDq_dwb_;
-
-        public:
-            void copyfrom(Imu_jac_bias const& right){
-
-                Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
-                Delta0_  = right.Delta0_;
-                dDp_dab_ = right.dDp_dab_;
-                dDv_dab_ = right.dDv_dab_;
-                dDp_dwb_ = right.dDp_dwb_;
-                dDv_dwb_ = right.dDv_dwb_;
-                dDq_dwb_ = right.dDq_dwb_;
-            }
-    };
-
-    struct Imu_jac_deltas{
-
-        Imu_jac_deltas(Eigen::VectorXd _Delta0,
-                       Eigen::VectorXd _delta0,
-                       Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
-                       Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
-                       Eigen::MatrixXd _jacobian_delta_preint,
-                       Eigen::MatrixXd _jacobian_delta ) :
-                           Delta0_(_Delta0),
-                           delta0_(_delta0),
-                           Delta_noisy_vect_(_Delta_noisy_vect),
-                           delta_noisy_vect_(_delta_noisy_vect),
-                           jacobian_delta_preint_(_jacobian_delta_preint),
-                           jacobian_delta_(_jacobian_delta)
+  public:
+    /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+      place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
+      place 4 : added dw_bx in data         place 5 : added dw_by in data       place 6 : added dw_bz in data
+     */
+    Eigen::Matrix<Eigen::VectorXd, 6, 1> Deltas_noisy_vect_;
+    Eigen::VectorXd                      Delta0_;
+    Eigen::Matrix3d                      dDp_dab_;
+    Eigen::Matrix3d                      dDv_dab_;
+    Eigen::Matrix3d                      dDp_dwb_;
+    Eigen::Matrix3d                      dDv_dwb_;
+    Eigen::Matrix3d                      dDq_dwb_;
+
+  public:
+    void copyfrom(Imu_jac_bias const& right)
+    {
+        Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
+        Delta0_            = right.Delta0_;
+        dDp_dab_           = right.dDp_dab_;
+        dDv_dab_           = right.dDv_dab_;
+        dDp_dwb_           = right.dDp_dwb_;
+        dDv_dwb_           = right.dDv_dwb_;
+        dDq_dwb_           = right.dDq_dwb_;
+    }
+};
+
+struct Imu_jac_deltas
+{
+    Imu_jac_deltas(Eigen::VectorXd                      _Delta0,
+                   Eigen::VectorXd                      _delta0,
+                   Eigen::Matrix<Eigen::VectorXd, 9, 1> _Delta_noisy_vect,
+                   Eigen::Matrix<Eigen::VectorXd, 9, 1> _delta_noisy_vect,
+                   Eigen::MatrixXd                      _jacobian_delta_preint,
+                   Eigen::MatrixXd                      _jacobian_delta)
+        : Delta0_(_Delta0),
+          delta0_(_delta0),
+          Delta_noisy_vect_(_Delta_noisy_vect),
+          delta_noisy_vect_(_delta_noisy_vect),
+          jacobian_delta_preint_(_jacobian_delta_preint),
+          jacobian_delta_(_jacobian_delta)
+    {
+        //
+    }
+
+    Imu_jac_deltas()
+    {
+        for (int i = 0; i < 9; i++)
         {
-            //
+            Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
+            delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
         }
 
-        Imu_jac_deltas(){
-            for (int i=0; i<9; i++){
-                Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-                delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-            }
+        Delta0_                = Eigen::VectorXd::Zero(1, 1);
+        delta0_                = Eigen::VectorXd::Zero(1, 1);
+        jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9, 9);
+        jacobian_delta_        = Eigen::MatrixXd::Zero(9, 9);
+    }
 
-            Delta0_ = Eigen::VectorXd::Zero(1,1);
-            delta0_ = Eigen::VectorXd::Zero(1,1);
-            jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9,9);
-            jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
-        }
+    Imu_jac_deltas(Imu_jac_deltas const& toCopy)
+    {
+        Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
+        delta_noisy_vect_ = toCopy.delta_noisy_vect_;
 
-        Imu_jac_deltas(Imu_jac_deltas const & toCopy){
+        Delta0_                = toCopy.Delta0_;
+        delta0_                = toCopy.delta0_;
+        jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
+        jacobian_delta_        = toCopy.jacobian_delta_;
+    }
 
-            Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
-            delta_noisy_vect_ = toCopy.delta_noisy_vect_;
+  public:
+    /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+      Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx,
+      dVx, dvx,..., dOz,doz). Delta_noisy_vect_ delta_noisy_vect_ 0: + 0, 0: + 0 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2:
+      +dpy, 3: +dpz 4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox,
+      5: +doy, 6: +doz 7: +dVx, 8: +dVy, 9: +dVz                                                               7: +
+      dvx, 9: +dvy, +: +dvz
+     */
+    Eigen::VectorXd                      Delta0_;            // this will contain the Delta not affected by noise
+    Eigen::VectorXd                      delta0_;            // this will contain the delta not affected by noise
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> Delta_noisy_vect_;  // this will contain the Deltas affected by noises
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> delta_noisy_vect_;  // this will contain the deltas affected by noises
+    Eigen::MatrixXd                      jacobian_delta_preint_;
+    Eigen::MatrixXd                      jacobian_delta_;
+
+  public:
+    void copyfrom(Imu_jac_deltas const& right)
+    {
+        Delta_noisy_vect_      = right.Delta_noisy_vect_;
+        delta_noisy_vect_      = right.delta_noisy_vect_;
+        Delta0_                = right.Delta0_;
+        delta0_                = right.delta0_;
+        jacobian_delta_preint_ = right.jacobian_delta_preint_;
+        jacobian_delta_        = right.jacobian_delta_;
+    }
+};
 
-            Delta0_ = toCopy.Delta0_;
-            delta0_ = toCopy.delta0_;
-            jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
-            jacobian_delta_ = toCopy.jacobian_delta_;
-        }
-        
-        public:
-            /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
-              Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz).
-                            Delta_noisy_vect_                                                                       delta_noisy_vect_
-                            0: + 0,                                                                                 0: + 0
-                            1: +dPx, 2: +dPy, 3: +dPz                                                               1: + dpx, 2: +dpy, 3: +dpz
-                            4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox, 5: +doy, 6: +doz
-                            7: +dVx, 8: +dVy, 9: +dVz                                                               7: + dvx, 9: +dvy, +: +dvz
-             */
-            Eigen::VectorXd Delta0_; //this will contain the Delta not affected by noise
-            Eigen::VectorXd delta0_; //this will contain the delta not affected by noise
-            Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
-            Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
-            Eigen::MatrixXd jacobian_delta_preint_;
-            Eigen::MatrixXd jacobian_delta_;
-
-        public:
-            void copyfrom(Imu_jac_deltas const& right){
-
-                Delta_noisy_vect_       = right.Delta_noisy_vect_;
-                delta_noisy_vect_       = right.delta_noisy_vect_;
-                Delta0_                 = right.Delta0_;
-                delta0_                 = right.delta0_;
-                jacobian_delta_preint_  = right.jacobian_delta_preint_;
-                jacobian_delta_         = right.jacobian_delta_;
-            }
-    };
-
-    class ProcessorImu2dTester : public ProcessorImu2d{
-
-        public:
-
-        ProcessorImu2dTester(const YAML::Node& _params);
-        ~ProcessorImu2dTester() override;
-
-        //Functions to test jacobians with finite difference method
-
-        /* params :
-            _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 Imu measurements
-            da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one. 
-         */
-        Imu_jac_bias finite_diff_ab(const double _dt,
-                                    Eigen::Vector6d& _data,
-                                    const double& da_b,
-                                    const Eigen::Matrix<double,10,1>& _delta_preint0);
-
-        /* params :
-            _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 Imu measurements
-            _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
-            _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
-         */
-        Imu_jac_deltas finite_diff_noise(const double& _dt,
-                                         Eigen::Vector6d& _data,
-                                         const Eigen::Matrix<double,9,1>& _Delta_noise,
-                                         const Eigen::Matrix<double,9,1>& _delta_noise,
-                                         const Eigen::Matrix<double,10,1>& _Delta0);
-
-        // public:
-        // static ProcessorBasePtr create(const std::string& _unique_name,
-        //                                const ParamsProcessorBasePtr _params,
-        //                                const SensorBasePtr = nullptr);
-
-        public:
-        // Maps quat, to be used as temporary
-        Eigen::Map<Eigen::Quaterniond> Dq_out_;
-
-    };
+class ProcessorImu2dTester : public ProcessorImu2d
+{
+  public:
+    ProcessorImu2dTester(const YAML::Node& _params);
+    WOLF_PROCESSOR_CREATE(ProcessorImu2dTester);
+    ~ProcessorImu2dTester() override;
+
+    // Functions to test jacobians with finite difference method
+
+    /* params :
+        _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+        _dt : time interval between 2 Imu measurements
+        da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx,
+       wby, wbz) one by one.
+     */
+    Imu_jac_bias finite_diff_ab(const double                        _dt,
+                                Eigen::Vector6d&                    _data,
+                                const double&                       da_b,
+                                const Eigen::Matrix<double, 10, 1>& _delta_preint0);
+
+    /* params :
+        _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+        _dt : time interval between 2 Imu measurements
+        _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector
+       (R2v(q.matrix())) _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because
+       rotation expressed as a vector (R2v(q.matrix()))
+     */
+    Imu_jac_deltas finite_diff_noise(const double&                       _dt,
+                                     Eigen::Vector6d&                    _data,
+                                     const Eigen::Matrix<double, 9, 1>&  _Delta_noise,
+                                     const Eigen::Matrix<double, 9, 1>&  _delta_noise,
+                                     const Eigen::Matrix<double, 10, 1>& _Delta0);
 
-}
+    // public:
+    // static ProcessorBasePtr create(const std::string& _unique_name,
+    //                                const ParamsProcessorBasePtr _params,
+    //                                const SensorBasePtr = nullptr);
+
+  public:
+    // Maps quat, to be used as temporary
+    Eigen::Map<Eigen::Quaterniond> Dq_out_;
+};
+
+}  // namespace wolf
 
 /////////////////////////////////////////////////////////
 // IMPLEMENTATION. Put your implementation includes here
@@ -217,35 +222,36 @@ namespace wolf {
 #include "core/state_block/state_block.h"
 #include "core/math/rotations.h"
 
-namespace wolf{
+namespace wolf
+{
 
-    //Functions to test jacobians with finite difference method
-inline Imu_jac_bias ProcessorImu2dTester::finite_diff_ab(const double _dt,
-                                                            Eigen::Vector6d& _data,
-                                                            const double& da_b,
-                                                            const Eigen::Matrix<double,10,1>& _delta_preint0)
+// Functions to test jacobians with finite difference method
+inline Imu_jac_bias ProcessorImu2dTester::finite_diff_ab(const double                        _dt,
+                                                         Eigen::Vector6d&                    _data,
+                                                         const double&                       da_b,
+                                                         const Eigen::Matrix<double, 10, 1>& _delta_preint0)
 {
-    //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
-    ///Define all the needed variables
-    Eigen::VectorXd Delta0;
-    Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect;
-    Eigen::Vector6d data0;
+    // TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
+    /// Define all the needed variables
+    Eigen::VectorXd                      Delta0;
+    Eigen::Matrix<Eigen::VectorXd, 6, 1> Deltas_noisy_vect;
+    Eigen::Vector6d                      data0;
     data0 = _data;
 
     Eigen::MatrixXd data_cov;
     Eigen::MatrixXd jacobian_delta_preint;
     Eigen::MatrixXd jacobian_delta;
     Eigen::VectorXd delta_preint_plus_delta0;
-    data_cov.resize(6,6);
-    jacobian_delta_preint.resize(9,9);
-    jacobian_delta.resize(9,9);
+    data_cov.resize(6, 6);
+    jacobian_delta_preint.resize(9, 9);
+    jacobian_delta.resize(9, 9);
     delta_preint_plus_delta0.resize(10);
 
-    //set all variables to 0
-    data_cov = Eigen::MatrixXd::Zero(6,6);
-    jacobian_delta_preint = Eigen::MatrixXd::Zero(9,9);
-    jacobian_delta = Eigen::MatrixXd::Zero(9,9);
-    delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
+    // set all variables to 0
+    data_cov              = Eigen::MatrixXd::Zero(6, 6);
+    jacobian_delta_preint = Eigen::MatrixXd::Zero(9, 9);
+    jacobian_delta        = Eigen::MatrixXd::Zero(9, 9);
+    delta_preint_plus_delta0 << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;  // PQV
 
     Vector6d bias = Vector6d::Zero();
 
@@ -256,140 +262,147 @@ inline Imu_jac_bias ProcessorImu2dTester::finite_diff_ab(const double _dt,
 
     Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
 
-    //Deltas without use of da_b
-    computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
+    // Deltas without use of da_b
+    computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
     deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
     MatrixXd jacobian_bias = jacobian_delta * jacobian_delta_calib_;
-    Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
-    dDp_dab = jacobian_bias.block(0,0,3,3);
-    dDp_dwb = jacobian_bias.block(0,3,3,3);
-    dDq_dwb = jacobian_bias.block(3,3,3,3);
-    dDv_dab = jacobian_bias.block(6,0,3,3);
-    dDv_dwb = jacobian_bias.block(6,3,3,3);
-    
+    Delta0 = delta_preint_plus_delta0;  // this is the first preintegrated delta, not affected by any added bias noise
+    dDp_dab = jacobian_bias.block(0, 0, 3, 3);
+    dDp_dwb = jacobian_bias.block(0, 3, 3, 3);
+    dDq_dwb = jacobian_bias.block(3, 3, 3, 3);
+    dDv_dab = jacobian_bias.block(6, 0, 3, 3);
+    dDv_dwb = jacobian_bias.block(6, 3, 3, 3);
 
     // propagate bias noise
-    for(int i=0; i<6; i++){
-        //need to reset stuff here
-        delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0;  //PQV
-        data_cov = Eigen::MatrixXd::Zero(6,6);
+    for (int i = 0; i < 6; i++)
+    {
+        // need to reset stuff here
+        delta_preint_plus_delta0 << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;  // PQV
+        data_cov = Eigen::MatrixXd::Zero(6, 6);
 
         // add da_b
-        _data = data0;
-        _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
-        //data2delta
+        _data    = data0;
+        _data(i) = _data(i) - da_b;  //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
+        // data2delta
         computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
         deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
-        Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
+        Deltas_noisy_vect(i) = delta_preint_plus_delta0;  // preintegrated deltas affected by added bias noise
     }
 
     Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
     return bias_jacobians;
 }
 
-inline Imu_jac_deltas ProcessorImu2dTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu2dTester::finite_diff_noise(const double&                       _dt,
+                                                              Eigen::Vector6d&                    _data,
+                                                              const Eigen::Matrix<double, 9, 1>&  _Delta_noise,
+                                                              const Eigen::Matrix<double, 9, 1>&  _delta_noise,
+                                                              const Eigen::Matrix<double, 10, 1>& _Delta0)
 {
-    //we do not propagate any noise from data vector
-    Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
-    Eigen::VectorXd delta0; //will contain the delta not affected by added noise
+    // we do not propagate any noise from data vector
+    Eigen::VectorXd Delta_;  // will contain the preintegrated Delta affected by added noise
+    Eigen::VectorXd delta0;  // will contain the delta not affected by added noise
     // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
     Eigen::VectorXd delta_preint_plus_delta;
     delta0.resize(10);
     delta_preint_plus_delta.resize(10);
-    delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
-
-    Eigen::MatrixXd jacobian_delta_preint;  //will be used as input for deltaPlusDelta
-    Eigen::MatrixXd jacobian_delta;         //will be used as input for deltaPlusDelta
-    jacobian_delta_preint.resize(9,9);
-    jacobian_delta.resize(9,9);
-    jacobian_delta_preint.setIdentity(9,9);
-    jacobian_delta.setIdentity(9,9);
-    Eigen::MatrixXd jacobian_delta_preint0; //will contain the jacobian we want to check
-    Eigen::MatrixXd jacobian_delta0;        //will contain the jacobian we want to check
-    jacobian_delta_preint0.resize(9,9);
-    jacobian_delta0.resize(9,9);
-    jacobian_delta_preint0.setIdentity(9,9);
-    jacobian_delta0.setIdentity(9,9);
-
-    Eigen::MatrixXd data_cov;   //will be used filled with Zeros as input for data2delta
-    data_cov.resize(6,6);
-    data_cov = Eigen::MatrixXd::Zero(6,6);
-
-    Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
-    Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
+    delta_preint_plus_delta << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+
+    Eigen::MatrixXd jacobian_delta_preint;  // will be used as input for deltaPlusDelta
+    Eigen::MatrixXd jacobian_delta;         // will be used as input for deltaPlusDelta
+    jacobian_delta_preint.resize(9, 9);
+    jacobian_delta.resize(9, 9);
+    jacobian_delta_preint.setIdentity(9, 9);
+    jacobian_delta.setIdentity(9, 9);
+    Eigen::MatrixXd jacobian_delta_preint0;  // will contain the jacobian we want to check
+    Eigen::MatrixXd jacobian_delta0;         // will contain the jacobian we want to check
+    jacobian_delta_preint0.resize(9, 9);
+    jacobian_delta0.resize(9, 9);
+    jacobian_delta_preint0.setIdentity(9, 9);
+    jacobian_delta0.setIdentity(9, 9);
+
+    Eigen::MatrixXd data_cov;  // will be used filled with Zeros as input for data2delta
+    data_cov.resize(6, 6);
+    data_cov = Eigen::MatrixXd::Zero(6, 6);
+
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> Delta_noisy_vect;  // this will contain the Deltas affected by noises
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> delta_noisy_vect;  // this will contain the deltas affected by noises
 
     Vector6d bias = Vector6d::Zero();
 
-    computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
-    delta0 = delta_;        //save the delta that is not affected by noise
-    deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta); 
+    computeCurrentDelta(
+        _data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);  // Affects dp_out, dv_out and dq_out
+    delta0 = delta_;  // save the delta that is not affected by noise
+    deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta);
     jacobian_delta_preint0 = jacobian_delta_preint;
-    jacobian_delta0 = jacobian_delta;
+    jacobian_delta0        = jacobian_delta;
 
-    //We compute all the jacobians wrt deltas and noisy deltas
-    for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
-    {   
-        //PQV formulation
-            //Add perturbation in position
-        delta_ = delta0;
-        delta_(i) = delta_(i) + _delta_noise(i); //noise has been added
+    // We compute all the jacobians wrt deltas and noisy deltas
+    for (int i = 0; i < 3; i++)  // for 3 first and 3 last components we just add to add noise as vector component
+                                 // since it is in the R^3 space
+    {
+        // PQV formulation
+        // Add perturbation in position
+        delta_              = delta0;
+        delta_(i)           = delta_(i) + _delta_noise(i);  // noise has been added
         delta_noisy_vect(i) = delta_;
 
-            //Add perturbation in velocity
-            /*
-            delta_ is size 10 (P Q V),  _delta_noise is size 9 (P O V)
-            */
-        delta_ = delta0;
-        delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added
-        delta_noisy_vect(i+6) = delta_;
+        // Add perturbation in velocity
+        /*
+        delta_ is size 10 (P Q V),  _delta_noise is size 9 (P O V)
+        */
+        delta_                  = delta0;
+        delta_(i + 7)           = delta_(i + 7) + _delta_noise(i + 6);  // noise has been added
+        delta_noisy_vect(i + 6) = delta_;
     }
 
-    for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
-    {   
-        //PQV formulation
-        //fist we need to reset some stuff
+    for (int i = 0; i < 3; i++)  // for noise dtheta, it is in SO3, need to work on quaternions
+    {
+        // PQV formulation
+        // fist we need to reset some stuff
         Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
 
         delta_ = delta0;
-        new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3); //not sure that we need this
-        dtheta(i) +=  _delta_noise(i+3); //introduce perturbation
-        Dq_out_ = Dq_out_ * v2q(dtheta);
-        delta_noisy_vect(i+3) = delta_;
+        new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3);  // not sure that we need this
+        dtheta(i) += _delta_noise(i + 3);                    // introduce perturbation
+        Dq_out_                 = Dq_out_ * v2q(dtheta);
+        delta_noisy_vect(i + 3) = delta_;
     }
 
-    //We compute all the jacobians wrt Deltas and noisy Deltas
-    for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
+    // We compute all the jacobians wrt Deltas and noisy Deltas
+    for (int i = 0; i < 3; i++)  // for 3 first and 3 last components we just add to add noise as vector component
+                                 // since it is in the R^3 space
     {
-        //PQV formulation
-            //Add perturbation in position
-        Delta_ = _Delta0;
-        Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added
+        // PQV formulation
+        // Add perturbation in position
+        Delta_              = _Delta0;
+        Delta_(i)           = Delta_(i) + _Delta_noise(i);  // noise has been added
         Delta_noisy_vect(i) = Delta_;
 
-            //Add perturbation in velocity
-            /*
-            Delta_ is size 10 (P Q V),  _Delta_noise is size 9 (P O V)
-            */
-        Delta_ = _Delta0;
-        Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added
-        Delta_noisy_vect(i+6) = Delta_;
+        // Add perturbation in velocity
+        /*
+        Delta_ is size 10 (P Q V),  _Delta_noise is size 9 (P O V)
+        */
+        Delta_                  = _Delta0;
+        Delta_(i + 7)           = Delta_(i + 7) + _Delta_noise(i + 6);  // noise has been added
+        Delta_noisy_vect(i + 6) = Delta_;
     }
 
-    for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
+    for (int i = 0; i < 3; i++)  // for noise dtheta, it is in SO3, need to work on quaternions
     {
-        //fist we need to reset some stuff
+        // fist we need to reset some stuff
         Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
 
         Delta_ = _Delta0;
         new (&Dq_out_) Map<Quaterniond>(Delta_.data() + 3);
-        dtheta(i) += _Delta_noise(i+3); //introduce perturbation
-        Dq_out_ = Dq_out_ * v2q(dtheta);
-        Delta_noisy_vect(i+3) = Delta_;
+        dtheta(i) += _Delta_noise(i + 3);  // introduce perturbation
+        Dq_out_                 = Dq_out_ * v2q(dtheta);
+        Delta_noisy_vect(i + 3) = Delta_;
     }
-    
-    Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
-    return jac_deltas;
 
+    Imu_jac_deltas jac_deltas(
+        _Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
+    return jac_deltas;
 }
 
-} // namespace wolf
\ No newline at end of file
+}  // namespace wolf
\ No newline at end of file
diff --git a/test/processor_imu_3d_tester.cpp b/test/processor_imu_3d_tester.cpp
index 03328b5a304a6abc22a743358d9b9cdfd752a56f..01fa29421a33cd473dc3561d8b800774111ac04d 100644
--- a/test/processor_imu_3d_tester.cpp
+++ b/test/processor_imu_3d_tester.cpp
@@ -29,5 +29,7 @@ ProcessorImu3dTester::ProcessorImu3dTester(const YAML::Node& _params) :
 
 ProcessorImu3dTester::~ProcessorImu3dTester(){}
 
+// Register in the FactoryProcessor
+WOLF_REGISTER_PROCESSOR(ProcessorImu3dTester)
 } // namespace wolf
 
diff --git a/test/processor_imu_3d_tester.h b/test/processor_imu_3d_tester.h
index 0072b4d1dae820965677686340c8fa76d3bb8d1b..a75ba00ea16cb1060f3c507902371990738a517c 100644
--- a/test/processor_imu_3d_tester.h
+++ b/test/processor_imu_3d_tester.h
@@ -24,190 +24,195 @@
 #include "imu/processor/processor_imu_3d.h"
 #include "core/processor/processor_motion.h"
 
-namespace wolf {
-    struct Imu_jac_bias{ //struct used for checking jacobians by finite difference
-
-        Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
-                     Eigen::VectorXd _Delta0 ,
-                     Eigen::Matrix3d _dDp_dab,
-                     Eigen::Matrix3d _dDv_dab,
-                     Eigen::Matrix3d _dDp_dwb,
-                     Eigen::Matrix3d _dDv_dwb,
-                     Eigen::Matrix3d _dDq_dwb) :
-                         Deltas_noisy_vect_(_Deltas_noisy_vect),
-                         Delta0_(_Delta0) ,
-                         dDp_dab_(_dDp_dab),
-                         dDv_dab_(_dDv_dab),
-                         dDp_dwb_(_dDp_dwb),
-                         dDv_dwb_(_dDv_dwb),
-                         dDq_dwb_(_dDq_dwb)
+namespace wolf
+{
+struct Imu_jac_bias
+{  // struct used for checking jacobians by finite difference
+
+    Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd, 6, 1> _Deltas_noisy_vect,
+                 Eigen::VectorXd                      _Delta0,
+                 Eigen::Matrix3d                      _dDp_dab,
+                 Eigen::Matrix3d                      _dDv_dab,
+                 Eigen::Matrix3d                      _dDp_dwb,
+                 Eigen::Matrix3d                      _dDv_dwb,
+                 Eigen::Matrix3d                      _dDq_dwb)
+        : Deltas_noisy_vect_(_Deltas_noisy_vect),
+          Delta0_(_Delta0),
+          dDp_dab_(_dDp_dab),
+          dDv_dab_(_dDv_dab),
+          dDp_dwb_(_dDp_dwb),
+          dDv_dwb_(_dDv_dwb),
+          dDq_dwb_(_dDq_dwb)
+    {
+        //
+    }
+
+    Imu_jac_bias()
+    {
+        for (int i = 0; i < 6; i++)
         {
-            //
-        }
-                
-        Imu_jac_bias(){
-
-            for (int i=0; i<6; i++){
-                Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-            }
-
-            Delta0_ = Eigen::VectorXd::Zero(1,1);
-            dDp_dab_ = Eigen::Matrix3d::Zero();
-            dDv_dab_ = Eigen::Matrix3d::Zero();
-            dDp_dwb_ = Eigen::Matrix3d::Zero();
-            dDv_dwb_ = Eigen::Matrix3d::Zero();
-            dDq_dwb_ = Eigen::Matrix3d::Zero();
+            Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
         }
 
-        Imu_jac_bias(Imu_jac_bias const & toCopy){
+        Delta0_  = Eigen::VectorXd::Zero(1, 1);
+        dDp_dab_ = Eigen::Matrix3d::Zero();
+        dDv_dab_ = Eigen::Matrix3d::Zero();
+        dDp_dwb_ = Eigen::Matrix3d::Zero();
+        dDv_dwb_ = Eigen::Matrix3d::Zero();
+        dDq_dwb_ = Eigen::Matrix3d::Zero();
+    }
 
-            Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
-            Delta0_  = toCopy.Delta0_;
-            dDp_dab_ = toCopy.dDp_dab_;
-            dDv_dab_ = toCopy.dDv_dab_;
-            dDp_dwb_ = toCopy.dDp_dwb_;
-            dDv_dwb_ = toCopy.dDv_dwb_;
-            dDq_dwb_ = toCopy.dDq_dwb_;
-        }
+    Imu_jac_bias(Imu_jac_bias const& toCopy)
+    {
+        Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
+        Delta0_            = toCopy.Delta0_;
+        dDp_dab_           = toCopy.dDp_dab_;
+        dDv_dab_           = toCopy.dDv_dab_;
+        dDp_dwb_           = toCopy.dDp_dwb_;
+        dDv_dwb_           = toCopy.dDv_dwb_;
+        dDq_dwb_           = toCopy.dDq_dwb_;
+    }
+
+  public:
+    /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+      place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
+      place 4 : added dw_bx in data         place 5 : added dw_by in data       place 6 : added dw_bz in data
+     */
+    Eigen::Matrix<Eigen::VectorXd, 6, 1> Deltas_noisy_vect_;
+    Eigen::VectorXd                      Delta0_;
+    Eigen::Matrix3d                      dDp_dab_;
+    Eigen::Matrix3d                      dDv_dab_;
+    Eigen::Matrix3d                      dDp_dwb_;
+    Eigen::Matrix3d                      dDv_dwb_;
+    Eigen::Matrix3d                      dDq_dwb_;
+
+  public:
+    void copyfrom(Imu_jac_bias const& right)
+    {
+        Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
+        Delta0_            = right.Delta0_;
+        dDp_dab_           = right.dDp_dab_;
+        dDv_dab_           = right.dDv_dab_;
+        dDp_dwb_           = right.dDp_dwb_;
+        dDv_dwb_           = right.dDv_dwb_;
+        dDq_dwb_           = right.dDq_dwb_;
+    }
+};
 
-        public:
-            /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
-              place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
-              place 4 : added dw_bx in data         place 5 : added dw_by in data       place 6 : added dw_bz in data
-             */
-            Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect_;
-            Eigen::VectorXd Delta0_;
-            Eigen::Matrix3d dDp_dab_;
-            Eigen::Matrix3d dDv_dab_;
-            Eigen::Matrix3d dDp_dwb_;
-            Eigen::Matrix3d dDv_dwb_;
-            Eigen::Matrix3d dDq_dwb_;
-
-        public:
-            void copyfrom(Imu_jac_bias const& right){
-
-                Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
-                Delta0_  = right.Delta0_;
-                dDp_dab_ = right.dDp_dab_;
-                dDv_dab_ = right.dDv_dab_;
-                dDp_dwb_ = right.dDp_dwb_;
-                dDv_dwb_ = right.dDv_dwb_;
-                dDq_dwb_ = right.dDq_dwb_;
-            }
-    };
-
-    struct Imu_jac_deltas{
-
-        Imu_jac_deltas(Eigen::VectorXd _Delta0,
-                       Eigen::VectorXd _delta0,
-                       Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
-                       Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
-                       Eigen::MatrixXd _jacobian_delta_preint,
-                       Eigen::MatrixXd _jacobian_delta ) :
-                           Delta0_(_Delta0),
-                           delta0_(_delta0),
-                           Delta_noisy_vect_(_Delta_noisy_vect),
-                           delta_noisy_vect_(_delta_noisy_vect),
-                           jacobian_delta_preint_(_jacobian_delta_preint),
-                           jacobian_delta_(_jacobian_delta)
+struct Imu_jac_deltas
+{
+    Imu_jac_deltas(Eigen::VectorXd                      _Delta0,
+                   Eigen::VectorXd                      _delta0,
+                   Eigen::Matrix<Eigen::VectorXd, 9, 1> _Delta_noisy_vect,
+                   Eigen::Matrix<Eigen::VectorXd, 9, 1> _delta_noisy_vect,
+                   Eigen::MatrixXd                      _jacobian_delta_preint,
+                   Eigen::MatrixXd                      _jacobian_delta)
+        : Delta0_(_Delta0),
+          delta0_(_delta0),
+          Delta_noisy_vect_(_Delta_noisy_vect),
+          delta_noisy_vect_(_delta_noisy_vect),
+          jacobian_delta_preint_(_jacobian_delta_preint),
+          jacobian_delta_(_jacobian_delta)
+    {
+        //
+    }
+
+    Imu_jac_deltas()
+    {
+        for (int i = 0; i < 9; i++)
         {
-            //
+            Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
+            delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1, 1);
         }
 
-        Imu_jac_deltas(){
-            for (int i=0; i<9; i++){
-                Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-                delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
-            }
+        Delta0_                = Eigen::VectorXd::Zero(1, 1);
+        delta0_                = Eigen::VectorXd::Zero(1, 1);
+        jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9, 9);
+        jacobian_delta_        = Eigen::MatrixXd::Zero(9, 9);
+    }
 
-            Delta0_ = Eigen::VectorXd::Zero(1,1);
-            delta0_ = Eigen::VectorXd::Zero(1,1);
-            jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9,9);
-            jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
-        }
+    //        Imu_jac_deltas(Imu_jac_deltas const & toCopy){
+    //
+    //            Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
+    //            delta_noisy_vect_ = toCopy.delta_noisy_vect_;
+    //
+    //            Delta0_ = toCopy.Delta0_;
+    //            delta0_ = toCopy.delta0_;
+    //            jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
+    //            jacobian_delta_ = toCopy.jacobian_delta_;
+    //        }
+
+  public:
+    /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+      Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx,
+      dVx, dvx,..., dOz,doz). Delta_noisy_vect_ delta_noisy_vect_ 0: + 0, 0: + 0 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2:
+      +dpy, 3: +dpz 4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox,
+      5: +doy, 6: +doz 7: +dVx, 8: +dVy, 9: +dVz                                                               7: +
+      dvx, 9: +dvy, +: +dvz
+     */
+    Eigen::VectorXd                      Delta0_;            // this will contain the Delta not affected by noise
+    Eigen::VectorXd                      delta0_;            // this will contain the delta not affected by noise
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> Delta_noisy_vect_;  // this will contain the Deltas affected by noises
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> delta_noisy_vect_;  // this will contain the deltas affected by noises
+    Eigen::MatrixXd                      jacobian_delta_preint_;
+    Eigen::MatrixXd                      jacobian_delta_;
+
+  public:
+    void copyfrom(Imu_jac_deltas const& right)
+    {
+        Delta_noisy_vect_      = right.Delta_noisy_vect_;
+        delta_noisy_vect_      = right.delta_noisy_vect_;
+        Delta0_                = right.Delta0_;
+        delta0_                = right.delta0_;
+        jacobian_delta_preint_ = right.jacobian_delta_preint_;
+        jacobian_delta_        = right.jacobian_delta_;
+    }
+};
 
-//        Imu_jac_deltas(Imu_jac_deltas const & toCopy){
-//
-//            Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
-//            delta_noisy_vect_ = toCopy.delta_noisy_vect_;
-//
-//            Delta0_ = toCopy.Delta0_;
-//            delta0_ = toCopy.delta0_;
-//            jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
-//            jacobian_delta_ = toCopy.jacobian_delta_;
-//        }
-        
-        public:
-            /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
-              Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz).
-                            Delta_noisy_vect_                                                                       delta_noisy_vect_
-                            0: + 0,                                                                                 0: + 0
-                            1: +dPx, 2: +dPy, 3: +dPz                                                               1: + dpx, 2: +dpy, 3: +dpz
-                            4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox, 5: +doy, 6: +doz
-                            7: +dVx, 8: +dVy, 9: +dVz                                                               7: + dvx, 9: +dvy, +: +dvz
-             */
-            Eigen::VectorXd Delta0_; //this will contain the Delta not affected by noise
-            Eigen::VectorXd delta0_; //this will contain the delta not affected by noise
-            Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
-            Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
-            Eigen::MatrixXd jacobian_delta_preint_;
-            Eigen::MatrixXd jacobian_delta_;
-
-        public:
-            void copyfrom(Imu_jac_deltas const& right){
-
-                Delta_noisy_vect_       = right.Delta_noisy_vect_;
-                delta_noisy_vect_       = right.delta_noisy_vect_;
-                Delta0_                 = right.Delta0_;
-                delta0_                 = right.delta0_;
-                jacobian_delta_preint_  = right.jacobian_delta_preint_;
-                jacobian_delta_         = right.jacobian_delta_;
-            }
-    };
-
-    class ProcessorImu3dTester : public ProcessorImu3d{
-
-        public:
-
-        ProcessorImu3dTester(const YAML::Node& _params);
-        ~ProcessorImu3dTester() override;
-
-        //Functions to test jacobians with finite difference method
-
-        /* params :
-            _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 Imu measurements
-            da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one. 
-         */
-        Imu_jac_bias finite_diff_ab(const double _dt,
-                                    Eigen::Vector6d& _data,
-                                    const double& da_b,
-                                    const Eigen::Matrix<double,10,1>& _delta_preint0);
-
-        /* params :
-            _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 Imu measurements
-            _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
-            _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
-         */
-        Imu_jac_deltas finite_diff_noise(const double& _dt,
-                                         Eigen::Vector6d& _data,
-                                         const Eigen::Matrix<double,9,1>& _Delta_noise,
-                                         const Eigen::Matrix<double,9,1>& _delta_noise,
-                                         const Eigen::Matrix<double,10,1>& _Delta0);
-
-        public:
-        // static ProcessorBasePtr create(const std::string& _unique_name,
-        //                                const ParamsProcessorBasePtr _params,
-        //                                const SensorBasePtr = nullptr);
-
-        public:
-        // Maps quat, to be used as temporary
-        Eigen::Map<Eigen::Quaterniond> Dq_out_;
-
-    };
+class ProcessorImu3dTester : public ProcessorImu3d
+{
+  public:
+    ProcessorImu3dTester(const YAML::Node& _params);
+    WOLF_PROCESSOR_CREATE(ProcessorImu3dTester);
+    ~ProcessorImu3dTester() override;
+
+    // Functions to test jacobians with finite difference method
+
+    /* params :
+        _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+        _dt : time interval between 2 Imu measurements
+        da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx,
+       wby, wbz) one by one.
+     */
+    Imu_jac_bias finite_diff_ab(const double                        _dt,
+                                Eigen::Vector6d&                    _data,
+                                const double&                       da_b,
+                                const Eigen::Matrix<double, 10, 1>& _delta_preint0);
+
+    /* params :
+        _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+        _dt : time interval between 2 Imu measurements
+        _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector
+       (R2v(q.matrix())) _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because
+       rotation expressed as a vector (R2v(q.matrix()))
+     */
+    Imu_jac_deltas finite_diff_noise(const double&                       _dt,
+                                     Eigen::Vector6d&                    _data,
+                                     const Eigen::Matrix<double, 9, 1>&  _Delta_noise,
+                                     const Eigen::Matrix<double, 9, 1>&  _delta_noise,
+                                     const Eigen::Matrix<double, 10, 1>& _Delta0);
 
-}
+  public:
+    // static ProcessorBasePtr create(const std::string& _unique_name,
+    //                                const ParamsProcessorBasePtr _params,
+    //                                const SensorBasePtr = nullptr);
+
+  public:
+    // Maps quat, to be used as temporary
+    Eigen::Map<Eigen::Quaterniond> Dq_out_;
+};
+
+}  // namespace wolf
 
 /////////////////////////////////////////////////////////
 // IMPLEMENTATION. Put your implementation includes here
@@ -217,35 +222,36 @@ namespace wolf {
 #include "core/state_block/state_block.h"
 #include "core/math/rotations.h"
 
-namespace wolf{
+namespace wolf
+{
 
-    //Functions to test jacobians with finite difference method
-inline Imu_jac_bias ProcessorImu3dTester::finite_diff_ab(const double _dt,
-                                                            Eigen::Vector6d& _data,
-                                                            const double& da_b,
-                                                            const Eigen::Matrix<double,10,1>& _delta_preint0)
+// Functions to test jacobians with finite difference method
+inline Imu_jac_bias ProcessorImu3dTester::finite_diff_ab(const double                        _dt,
+                                                         Eigen::Vector6d&                    _data,
+                                                         const double&                       da_b,
+                                                         const Eigen::Matrix<double, 10, 1>& _delta_preint0)
 {
-    //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
-    ///Define all the needed variables
-    Eigen::VectorXd Delta0;
-    Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect;
-    Eigen::Vector6d data0;
+    // TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
+    /// Define all the needed variables
+    Eigen::VectorXd                      Delta0;
+    Eigen::Matrix<Eigen::VectorXd, 6, 1> Deltas_noisy_vect;
+    Eigen::Vector6d                      data0;
     data0 = _data;
 
     Eigen::MatrixXd data_cov;
     Eigen::MatrixXd jacobian_delta_preint;
     Eigen::MatrixXd jacobian_delta;
     Eigen::VectorXd delta_preint_plus_delta0;
-    data_cov.resize(6,6);
-    jacobian_delta_preint.resize(9,9);
-    jacobian_delta.resize(9,9);
+    data_cov.resize(6, 6);
+    jacobian_delta_preint.resize(9, 9);
+    jacobian_delta.resize(9, 9);
     delta_preint_plus_delta0.resize(10);
 
-    //set all variables to 0
-    data_cov = Eigen::MatrixXd::Zero(6,6);
-    jacobian_delta_preint = Eigen::MatrixXd::Zero(9,9);
-    jacobian_delta = Eigen::MatrixXd::Zero(9,9);
-    delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
+    // set all variables to 0
+    data_cov              = Eigen::MatrixXd::Zero(6, 6);
+    jacobian_delta_preint = Eigen::MatrixXd::Zero(9, 9);
+    jacobian_delta        = Eigen::MatrixXd::Zero(9, 9);
+    delta_preint_plus_delta0 << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;  // PQV
 
     Vector6d bias = Vector6d::Zero();
 
@@ -256,140 +262,147 @@ inline Imu_jac_bias ProcessorImu3dTester::finite_diff_ab(const double _dt,
 
     Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
 
-    //Deltas without use of da_b
-    computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
+    // Deltas without use of da_b
+    computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
     deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
     MatrixXd jacobian_bias = jacobian_delta * jacobian_delta_calib_;
-    Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
-    dDp_dab = jacobian_bias.block(0,0,3,3);
-    dDp_dwb = jacobian_bias.block(0,3,3,3);
-    dDq_dwb = jacobian_bias.block(3,3,3,3);
-    dDv_dab = jacobian_bias.block(6,0,3,3);
-    dDv_dwb = jacobian_bias.block(6,3,3,3);
-    
+    Delta0 = delta_preint_plus_delta0;  // this is the first preintegrated delta, not affected by any added bias noise
+    dDp_dab = jacobian_bias.block(0, 0, 3, 3);
+    dDp_dwb = jacobian_bias.block(0, 3, 3, 3);
+    dDq_dwb = jacobian_bias.block(3, 3, 3, 3);
+    dDv_dab = jacobian_bias.block(6, 0, 3, 3);
+    dDv_dwb = jacobian_bias.block(6, 3, 3, 3);
 
     // propagate bias noise
-    for(int i=0; i<6; i++){
-        //need to reset stuff here
-        delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0;  //PQV
-        data_cov = Eigen::MatrixXd::Zero(6,6);
+    for (int i = 0; i < 6; i++)
+    {
+        // need to reset stuff here
+        delta_preint_plus_delta0 << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;  // PQV
+        data_cov = Eigen::MatrixXd::Zero(6, 6);
 
         // add da_b
-        _data = data0;
-        _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
-        //data2delta
+        _data    = data0;
+        _data(i) = _data(i) - da_b;  //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
+        // data2delta
         computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
         deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
-        Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
+        Deltas_noisy_vect(i) = delta_preint_plus_delta0;  // preintegrated deltas affected by added bias noise
     }
 
     Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
     return bias_jacobians;
 }
 
-inline Imu_jac_deltas ProcessorImu3dTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu3dTester::finite_diff_noise(const double&                       _dt,
+                                                              Eigen::Vector6d&                    _data,
+                                                              const Eigen::Matrix<double, 9, 1>&  _Delta_noise,
+                                                              const Eigen::Matrix<double, 9, 1>&  _delta_noise,
+                                                              const Eigen::Matrix<double, 10, 1>& _Delta0)
 {
-    //we do not propagate any noise from data vector
-    Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
-    Eigen::VectorXd delta0; //will contain the delta not affected by added noise
+    // we do not propagate any noise from data vector
+    Eigen::VectorXd Delta_;  // will contain the preintegrated Delta affected by added noise
+    Eigen::VectorXd delta0;  // will contain the delta not affected by added noise
     // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
     Eigen::VectorXd delta_preint_plus_delta;
     delta0.resize(10);
     delta_preint_plus_delta.resize(10);
-    delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
-
-    Eigen::MatrixXd jacobian_delta_preint;  //will be used as input for deltaPlusDelta
-    Eigen::MatrixXd jacobian_delta;         //will be used as input for deltaPlusDelta
-    jacobian_delta_preint.resize(9,9);
-    jacobian_delta.resize(9,9);
-    jacobian_delta_preint.setIdentity(9,9);
-    jacobian_delta.setIdentity(9,9);
-    Eigen::MatrixXd jacobian_delta_preint0; //will contain the jacobian we want to check
-    Eigen::MatrixXd jacobian_delta0;        //will contain the jacobian we want to check
-    jacobian_delta_preint0.resize(9,9);
-    jacobian_delta0.resize(9,9);
-    jacobian_delta_preint0.setIdentity(9,9);
-    jacobian_delta0.setIdentity(9,9);
-
-    Eigen::MatrixXd data_cov;   //will be used filled with Zeros as input for data2delta
-    data_cov.resize(6,6);
-    data_cov = Eigen::MatrixXd::Zero(6,6);
-
-    Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
-    Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
+    delta_preint_plus_delta << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+
+    Eigen::MatrixXd jacobian_delta_preint;  // will be used as input for deltaPlusDelta
+    Eigen::MatrixXd jacobian_delta;         // will be used as input for deltaPlusDelta
+    jacobian_delta_preint.resize(9, 9);
+    jacobian_delta.resize(9, 9);
+    jacobian_delta_preint.setIdentity(9, 9);
+    jacobian_delta.setIdentity(9, 9);
+    Eigen::MatrixXd jacobian_delta_preint0;  // will contain the jacobian we want to check
+    Eigen::MatrixXd jacobian_delta0;         // will contain the jacobian we want to check
+    jacobian_delta_preint0.resize(9, 9);
+    jacobian_delta0.resize(9, 9);
+    jacobian_delta_preint0.setIdentity(9, 9);
+    jacobian_delta0.setIdentity(9, 9);
+
+    Eigen::MatrixXd data_cov;  // will be used filled with Zeros as input for data2delta
+    data_cov.resize(6, 6);
+    data_cov = Eigen::MatrixXd::Zero(6, 6);
+
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> Delta_noisy_vect;  // this will contain the Deltas affected by noises
+    Eigen::Matrix<Eigen::VectorXd, 9, 1> delta_noisy_vect;  // this will contain the deltas affected by noises
 
     Vector6d bias = Vector6d::Zero();
 
-    computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
-    delta0 = delta_;        //save the delta that is not affected by noise
-    deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta); 
+    computeCurrentDelta(
+        _data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);  // Affects dp_out, dv_out and dq_out
+    delta0 = delta_;  // save the delta that is not affected by noise
+    deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta);
     jacobian_delta_preint0 = jacobian_delta_preint;
-    jacobian_delta0 = jacobian_delta;
+    jacobian_delta0        = jacobian_delta;
 
-    //We compute all the jacobians wrt deltas and noisy deltas
-    for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
-    {   
-        //PQV formulation
-            //Add perturbation in position
-        delta_ = delta0;
-        delta_(i) = delta_(i) + _delta_noise(i); //noise has been added
+    // We compute all the jacobians wrt deltas and noisy deltas
+    for (int i = 0; i < 3; i++)  // for 3 first and 3 last components we just add to add noise as vector component
+                                 // since it is in the R^3 space
+    {
+        // PQV formulation
+        // Add perturbation in position
+        delta_              = delta0;
+        delta_(i)           = delta_(i) + _delta_noise(i);  // noise has been added
         delta_noisy_vect(i) = delta_;
 
-            //Add perturbation in velocity
-            /*
-            delta_ is size 10 (P Q V),  _delta_noise is size 9 (P O V)
-            */
-        delta_ = delta0;
-        delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added
-        delta_noisy_vect(i+6) = delta_;
+        // Add perturbation in velocity
+        /*
+        delta_ is size 10 (P Q V),  _delta_noise is size 9 (P O V)
+        */
+        delta_                  = delta0;
+        delta_(i + 7)           = delta_(i + 7) + _delta_noise(i + 6);  // noise has been added
+        delta_noisy_vect(i + 6) = delta_;
     }
 
-    for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
-    {   
-        //PQV formulation
-        //fist we need to reset some stuff
+    for (int i = 0; i < 3; i++)  // for noise dtheta, it is in SO3, need to work on quaternions
+    {
+        // PQV formulation
+        // fist we need to reset some stuff
         Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
 
         delta_ = delta0;
-        new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3); //not sure that we need this
-        dtheta(i) +=  _delta_noise(i+3); //introduce perturbation
-        Dq_out_ = Dq_out_ * v2q(dtheta);
-        delta_noisy_vect(i+3) = delta_;
+        new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3);  // not sure that we need this
+        dtheta(i) += _delta_noise(i + 3);                    // introduce perturbation
+        Dq_out_                 = Dq_out_ * v2q(dtheta);
+        delta_noisy_vect(i + 3) = delta_;
     }
 
-    //We compute all the jacobians wrt Deltas and noisy Deltas
-    for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
+    // We compute all the jacobians wrt Deltas and noisy Deltas
+    for (int i = 0; i < 3; i++)  // for 3 first and 3 last components we just add to add noise as vector component
+                                 // since it is in the R^3 space
     {
-        //PQV formulation
-            //Add perturbation in position
-        Delta_ = _Delta0;
-        Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added
+        // PQV formulation
+        // Add perturbation in position
+        Delta_              = _Delta0;
+        Delta_(i)           = Delta_(i) + _Delta_noise(i);  // noise has been added
         Delta_noisy_vect(i) = Delta_;
 
-            //Add perturbation in velocity
-            /*
-            Delta_ is size 10 (P Q V),  _Delta_noise is size 9 (P O V)
-            */
-        Delta_ = _Delta0;
-        Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added
-        Delta_noisy_vect(i+6) = Delta_;
+        // Add perturbation in velocity
+        /*
+        Delta_ is size 10 (P Q V),  _Delta_noise is size 9 (P O V)
+        */
+        Delta_                  = _Delta0;
+        Delta_(i + 7)           = Delta_(i + 7) + _Delta_noise(i + 6);  // noise has been added
+        Delta_noisy_vect(i + 6) = Delta_;
     }
 
-    for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
+    for (int i = 0; i < 3; i++)  // for noise dtheta, it is in SO3, need to work on quaternions
     {
-        //fist we need to reset some stuff
+        // fist we need to reset some stuff
         Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
 
         Delta_ = _Delta0;
         new (&Dq_out_) Map<Quaterniond>(Delta_.data() + 3);
-        dtheta(i) += _Delta_noise(i+3); //introduce perturbation
-        Dq_out_ = Dq_out_ * v2q(dtheta);
-        Delta_noisy_vect(i+3) = Delta_;
+        dtheta(i) += _Delta_noise(i + 3);  // introduce perturbation
+        Dq_out_                 = Dq_out_ * v2q(dtheta);
+        Delta_noisy_vect(i + 3) = Delta_;
     }
-    
-    Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
-    return jac_deltas;
 
+    Imu_jac_deltas jac_deltas(
+        _Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
+    return jac_deltas;
 }
 
-} // namespace wolf
\ No newline at end of file
+}  // namespace wolf
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_imu_2d_static_init.yaml b/test/yaml/imu_2d_static_init/problem_2d.yaml
similarity index 88%
rename from test/yaml/problem_gtest_imu_2d_static_init.yaml
rename to test/yaml/imu_2d_static_init/problem_2d.yaml
index d92a98b8a90a4cc44c7f9f19416bab8a926117b7..51513935b0ab7221736e398efe4b91ff7748f651 100644
--- a/test/yaml/problem_gtest_imu_2d_static_init.yaml
+++ b/test/yaml/imu_2d_static_init/problem_2d.yaml
@@ -20,4 +20,4 @@ sensors:
   - follow: sensor_imu_2d.yaml
     
 processors:
-  - follow: processor_imu_2d_static_init.yaml
\ No newline at end of file
+  - follow: processor_imu_2d.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_imu_2d_static_init.yaml b/test/yaml/imu_2d_static_init/processor_imu_2d.yaml
similarity index 100%
rename from test/yaml/processor_imu_2d_static_init.yaml
rename to test/yaml/imu_2d_static_init/processor_imu_2d.yaml
diff --git a/test/yaml/imu_2d_static_init/sensor_imu_2d.yaml b/test/yaml/imu_2d_static_init/sensor_imu_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f1e85064e06c8744f38e8e1f40ca48b758bda065
--- /dev/null
+++ b/test/yaml/imu_2d_static_init/sensor_imu_2d.yaml
@@ -0,0 +1,24 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu2d
+
+states:
+  P:
+    state: [0, 0]
+    mode: fix
+    dynamic: false
+  O:
+    state: [0]
+    mode: fix
+    dynamic: false
+  I:
+    state: [0, 0, 0]
+    mode: factor
+    dynamic: true
+    noise_std: [0.800, 0.800, 0.350] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
+    drift_std: [0.001, 0.001, 0.0004] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
+
+a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s²
+w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+
+orthogonal_gravity: true # gravity is aligned with Z axis
\ No newline at end of file
diff --git a/test/yaml/imu_3d/problem_3d.yaml b/test/yaml/imu_3d/problem_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d09607b9bba4c8a5f82b66e5c9e82a8d6bf3e972
--- /dev/null
+++ b/test/yaml/imu_3d/problem_3d.yaml
@@ -0,0 +1,23 @@
+problem:
+  dimension: 3
+  first_frame:
+    P:
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    O: 
+      state: [0,0,0,1]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    V: 
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+  tree_manager: 
+    type: "None"
+
+sensors: 
+  - follow: sensor_imu_3d.yaml
+    
+processors:
+  - follow: processor_imu_3d.yaml
\ No newline at end of file
diff --git a/test/yaml/imu_3d/processor_imu_3d.yaml b/test/yaml/imu_3d/processor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cd9cfab8c29bd983842841d242b647b1c67b093d
--- /dev/null
+++ b/test/yaml/imu_3d/processor_imu_3d.yaml
@@ -0,0 +1,21 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu3d
+sensor_name: "cool sensor imu"
+
+time_tolerance: 0.005        # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+apply_loss_function: false
+
+keyframe_vote:
+  voting_active:      false
+  max_time_span:      2.0   # seconds
+  max_buff_length:    20000    # motion deltas
+  max_dist_traveled:  2.0   # meters
+  max_angle_turned:   0.2   # radians (1 rad approx 57 deg, approx 60 deg)
+
+state_provider: true
+state_provider_order: 1
+
+bootstrap:
+  enabled: false
\ No newline at end of file
diff --git a/test/yaml/imu_3d/processor_odom_3d.yaml b/test/yaml/imu_3d/processor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eb3790e9bf57d13bb362e7f8d06bcaa1a617ef53
--- /dev/null
+++ b/test/yaml/imu_3d/processor_odom_3d.yaml
@@ -0,0 +1,19 @@
+name: "cool processor odom"
+plugin: core
+type: ProcessorOdom3d
+sensor_name: cool sensor odom
+
+time_tolerance:         0.005  # seconds
+
+keyframe_vote:
+  voting_active:        false
+  max_time_span:      0.0099   # seconds
+  max_buff_length:    1000    # motion deltas
+  max_dist_traveled:      1000   # meters
+  max_angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
+
+unmeasured_perturbation_std: 0.001
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 2
diff --git a/test/yaml/imu_3d/sensor_imu_3d.yaml b/test/yaml/imu_3d/sensor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..19bd9b31b43c2f12842c563ac6512cc408ccccde
--- /dev/null
+++ b/test/yaml/imu_3d/sensor_imu_3d.yaml
@@ -0,0 +1,22 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu3d
+
+states:
+  P:
+    state: [0, 0, 0]
+    mode: fix
+    dynamic: false
+  O:
+    state: [0, 0, 0, 1]
+    mode: fix
+    dynamic: false
+  I:
+    state: [0, 0, 0, 0, 0, 0]
+    mode: factor
+    dynamic: true
+    noise_std: [0.8, 0.8, 0.8, 0.35, 0.35, 0.35] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
+    drift_std: [0.1, 0.1, 0.1, 0.04, 0.04, 0.04] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
+
+a_noise: 0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s²
+w_noise: 0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
diff --git a/test/yaml/imu_3d/sensor_odom_3d.yaml b/test/yaml/imu_3d/sensor_odom_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..63675654b6ddba29e67a27c1196a8fed6059267f
--- /dev/null
+++ b/test/yaml/imu_3d/sensor_odom_3d.yaml
@@ -0,0 +1,19 @@
+name: "cool sensor odom"
+plugin: core
+type: SensorOdom3d
+
+states:
+  P:
+    state: [0, 0, 0]
+    mode: fix
+    dynamic: false
+  O:
+    state: [0, 0, 0, 1]
+    mode: fix
+    dynamic: false
+    
+k_disp_to_disp:   0.02  # m^2   / m
+k_disp_to_rot:    0.02  # rad^2 / m
+k_rot_to_rot:     0.01  # rad^2 / rad
+min_disp_var:     0.01  # m^2
+min_rot_var:      0.01  # rad^2
diff --git a/test/yaml/imu_mocap_fusion/problem_3d.yaml b/test/yaml/imu_3d_mocap_fusion/problem_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/problem_3d.yaml
rename to test/yaml/imu_3d_mocap_fusion/problem_3d.yaml
diff --git a/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml b/test/yaml/imu_3d_mocap_fusion/processor_imu_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
rename to test/yaml/imu_3d_mocap_fusion/processor_imu_3d.yaml
diff --git a/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml b/test/yaml/imu_3d_mocap_fusion/processor_pose_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
rename to test/yaml/imu_3d_mocap_fusion/processor_pose_3d.yaml
diff --git a/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml b/test/yaml/imu_3d_mocap_fusion/sensor_imu_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
rename to test/yaml/imu_3d_mocap_fusion/sensor_imu_3d.yaml
diff --git a/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml b/test/yaml/imu_3d_mocap_fusion/sensor_pose_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
rename to test/yaml/imu_3d_mocap_fusion/sensor_pose_3d.yaml
diff --git a/test/yaml/problem_gtest_imu_3d_static_init.yaml b/test/yaml/imu_3d_static_init/problem_3d.yaml
similarity index 91%
rename from test/yaml/problem_gtest_imu_3d_static_init.yaml
rename to test/yaml/imu_3d_static_init/problem_3d.yaml
index d25288ebcc5ed8bb568c46576e52fa0f02e27285..352236ec6889ad7537926d59b01bc8c2d7c270d9 100644
--- a/test/yaml/problem_gtest_imu_3d_static_init.yaml
+++ b/test/yaml/imu_3d_static_init/problem_3d.yaml
@@ -4,7 +4,7 @@ problem:
     P:
       state: [0, 0, 0]
       mode: "factor"
-      noise_std: [.001, .001]
+      noise_std: [.001, .001, .001]
     O: 
       state: [0, 0, 0, 1]
       mode: "factor"
diff --git a/test/yaml/processor_imu_3d_static_init.yaml b/test/yaml/imu_3d_static_init/processor_imu_3d_static_init.yaml
similarity index 67%
rename from test/yaml/processor_imu_3d_static_init.yaml
rename to test/yaml/imu_3d_static_init/processor_imu_3d_static_init.yaml
index 42e5310a2829b7e855f43ee00e7742831cf232ce..1efe61ced0b44035a495c83350652ed74a11c6a6 100644
--- a/test/yaml/processor_imu_3d_static_init.yaml
+++ b/test/yaml/imu_3d_static_init/processor_imu_3d_static_init.yaml
@@ -11,13 +11,11 @@ keyframe_vote:
     voting_active:      true
     max_time_span:      0.9999   # seconds
     max_buff_length:    1000000000   # motion deltas
-    max_dist_traveled:      100000000000   # meters
-    max_angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)
+    max_dist_traveled:  100000000000 # meters
+    max_angle_turned:   10000000000  # radians (1 rad approx 57 deg, approx 60 deg)
 
 state_provider: true
 state_provider_order: 1
 
 bootstrap:
-    enabled: true
-    method: STATIC
-    averaging_length: 10
\ No newline at end of file
+    enabled: false
\ No newline at end of file
diff --git a/test/yaml/imu_3d_static_init/sensor_imu_3d.yaml b/test/yaml/imu_3d_static_init/sensor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f224166679da9814d0b00e7b21abcdc7f976dfa1
--- /dev/null
+++ b/test/yaml/imu_3d_static_init/sensor_imu_3d.yaml
@@ -0,0 +1,22 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu3d
+
+states:
+  P:
+    state: [0, 0, 0]
+    mode: fix
+    dynamic: false
+  O:
+    state: [0, 0, 0, 1]
+    mode: fix
+    dynamic: false
+  I:
+    state: [0, 0, 0, 0, 0, 0]
+    mode: factor
+    dynamic: false
+    noise_std: [0.8, 0.8, 0.8, 0.35, 0.35, 0.35] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
+    drift_std: [0.1, 0.1, 0.1, 0.04, 0.04, 0.04] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
+
+a_noise: 0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s²
+w_noise: 0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
diff --git a/test/yaml/problem_3d_gtest_feature_imu.yaml b/test/yaml/problem_3d_gtest_feature_imu.yaml
index e8da703dda9950e16951363c130d16ae405cac66..d09607b9bba4c8a5f82b66e5c9e82a8d6bf3e972 100644
--- a/test/yaml/problem_3d_gtest_feature_imu.yaml
+++ b/test/yaml/problem_3d_gtest_feature_imu.yaml
@@ -16,13 +16,8 @@ problem:
   tree_manager: 
     type: "None"
 
-map:
-  type: "MapBase"
-  plugin: "core"
-
 sensors: 
   - follow: sensor_imu_3d.yaml
     
 processors:
-  - follow: processor_imu_3d.yaml
-    sensor_name: "cool sensor imu"
\ No newline at end of file
+  - follow: processor_imu_3d.yaml
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_processor_imu_2d.yaml b/test/yaml/problem_gtest_processor_imu_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..51513935b0ab7221736e398efe4b91ff7748f651
--- /dev/null
+++ b/test/yaml/problem_gtest_processor_imu_2d.yaml
@@ -0,0 +1,23 @@
+problem:
+  dimension: 2
+  first_frame:
+    P:
+      state: [0,0]
+      mode: "factor"
+      noise_std: [.001, .001]
+    O: 
+      state: [0]
+      mode: "factor"
+      noise_std: [.001]
+    V: 
+      state: [0,0]
+      mode: "factor"
+      noise_std: [.001, .001]
+  tree_manager: 
+    type: "None"
+
+sensors: 
+  - follow: sensor_imu_2d.yaml
+    
+processors:
+  - follow: processor_imu_2d.yaml
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_processor_imu_2d_with_gravity.yaml b/test/yaml/problem_gtest_processor_imu_2d_with_gravity.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1a79abec3da2099994288c95a0181411d2d5de8d
--- /dev/null
+++ b/test/yaml/problem_gtest_processor_imu_2d_with_gravity.yaml
@@ -0,0 +1,23 @@
+problem:
+  dimension: 2
+  first_frame:
+    P:
+      state: [0,0]
+      mode: "factor"
+      noise_std: [.001, .001]
+    O: 
+      state: [0]
+      mode: "factor"
+      noise_std: [.001]
+    V: 
+      state: [0,0]
+      mode: "factor"
+      noise_std: [.001, .001]
+  tree_manager: 
+    type: "None"
+
+sensors: 
+  - follow: sensor_imu_2d_with_gravity.yaml
+    
+processors:
+  - follow: processor_imu_2d.yaml
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_processor_imu_3d.yaml b/test/yaml/problem_gtest_processor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d09607b9bba4c8a5f82b66e5c9e82a8d6bf3e972
--- /dev/null
+++ b/test/yaml/problem_gtest_processor_imu_3d.yaml
@@ -0,0 +1,23 @@
+problem:
+  dimension: 3
+  first_frame:
+    P:
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    O: 
+      state: [0,0,0,1]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    V: 
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+  tree_manager: 
+    type: "None"
+
+sensors: 
+  - follow: sensor_imu_3d.yaml
+    
+processors:
+  - follow: processor_imu_3d.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_imu_2d.yaml b/test/yaml/processor_imu_2d.yaml
index 7705ed4c0d53c111f70e879bd339623cab81fd3c..88960c997c90cb2888f9bf20a7a52be74861b699 100644
--- a/test/yaml/processor_imu_2d.yaml
+++ b/test/yaml/processor_imu_2d.yaml
@@ -1,17 +1,18 @@
-name: "cool processor odom"
+name: "cool processor imu"
 plugin: imu
 type: ProcessorImu2d
+sensor_name: "cool sensor imu"
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.00001
 apply_loss_function: false
 
 keyframe_vote:
-    voting_active:      false
+    voting_active:      true
     max_time_span:      2.0   # seconds
-    max_buff_length:  20000    # motion deltas
-    max_dist_traveled:      2.0   # meters
-    max_angle_turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)
+    max_buff_length:    20000    # motion deltas
+    max_dist_traveled:  2.0   # meters
+    max_angle_turned:   0.2   # radians (1 rad approx 57 deg, approx 60 deg)
 
 state_provider: true
 state_provider_order: 1
diff --git a/test/yaml/processor_imu_3d.yaml b/test/yaml/processor_imu_3d.yaml
index bcc77ad5c352c8ef97ac10ce0814d77eb35365f7..ec6e194fad17a0373441684609515053568cd8f3 100644
--- a/test/yaml/processor_imu_3d.yaml
+++ b/test/yaml/processor_imu_3d.yaml
@@ -1,13 +1,14 @@
 name: "cool processor imu"
 plugin: imu
 type: ProcessorImu3d
+sensor_name: "cool sensor imu"
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.00001
 apply_loss_function: false
 
 keyframe_vote:
-  voting_active:      false
+  voting_active:      true
   max_time_span:      2.0   # seconds
   max_buff_length:    20000    # motion deltas
   max_dist_traveled:  2.0   # meters
diff --git a/test/yaml/processor_motion_intrinsics_update/problem_3d.yaml b/test/yaml/processor_motion_intrinsics_update/problem_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7bafd288d3b48f5b4c1fd00f4ffbc907a9a93da4
--- /dev/null
+++ b/test/yaml/processor_motion_intrinsics_update/problem_3d.yaml
@@ -0,0 +1,23 @@
+problem:
+  dimension: 3
+  first_frame:
+    P:
+      state: [0, 0, 0]
+      mode: "factor"
+      noise_std: [.001, .001, .001]
+    O: 
+      state: [0, 0, 0, 1]
+      mode: "factor"
+      noise_std: [.001, .001, .001]
+    V: 
+      state: [0, 0, 0]
+      mode: "factor"
+      noise_std: [.001, .001, .001]
+  tree_manager: 
+    type: "None"
+
+sensors: 
+  - follow: sensor_imu_3d.yaml
+    
+processors:
+  - follow: processor_imu_3d.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_motion_intrinsics_update/processor_imu_3d.yaml b/test/yaml/processor_motion_intrinsics_update/processor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1efe61ced0b44035a495c83350652ed74a11c6a6
--- /dev/null
+++ b/test/yaml/processor_motion_intrinsics_update/processor_imu_3d.yaml
@@ -0,0 +1,21 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu3d
+sensor_name: "cool sensor imu"
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+apply_loss_function: false
+
+keyframe_vote:
+    voting_active:      true
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    max_dist_traveled:  100000000000 # meters
+    max_angle_turned:   10000000000  # radians (1 rad approx 57 deg, approx 60 deg)
+
+state_provider: true
+state_provider_order: 1
+
+bootstrap:
+    enabled: false
\ No newline at end of file
diff --git a/test/yaml/processor_motion_intrinsics_update/sensor_imu_3d.yaml b/test/yaml/processor_motion_intrinsics_update/sensor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b09274d94683984160ffb9ae26676a5fb94409c3
--- /dev/null
+++ b/test/yaml/processor_motion_intrinsics_update/sensor_imu_3d.yaml
@@ -0,0 +1,22 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu3d
+
+states:
+  P:
+    state: [0, 0, 0]
+    mode: fix
+    dynamic: false
+  O:
+    state: [0, 0, 0, 1]
+    mode: fix
+    dynamic: false
+  I:
+    state: [0, 0, 0, 0, 0, 0]
+    mode: factor
+    dynamic: false
+    noise_std: [0.8, 0.8, 0.8, 0.35, 0.35, 0.35] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
+    drift_std: [0.1, 0.1, 0.1, 0.04, 0.04, 0.04] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
+
+a_noise: 0.001    # standard deviation of Acceleration noise (same for all the axis) in m/s²
+w_noise: 0.001    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 5fa752180d1e36a0f5eb0f633f0ee7d62d54b86c..0f97c643764cae0b08a1c8442a25ad81cff43056 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,15 +1,16 @@
 name: "cool processor odom"
 plugin: core
 type: ProcessorOdom3d
+sensor_name: cool sensor odom
 
-time_tolerance:         0.01  # seconds
+time_tolerance:         0.0005  # seconds
 
 keyframe_vote:
-  voting_active:        false
-  max_time_span:      0.2   # seconds
-  max_buff_length:    10    # motion deltas
-  max_dist_traveled:      0.5   # meters
-  max_angle_turned:       0.1   # radians (1 rad approx 57 deg, approx 60 deg)
+  voting_active:        trueº
+  max_time_span:      0.0099   # seconds
+  max_buff_length:    1000    # motion deltas
+  max_dist_traveled:      1000   # meters
+  max_angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
 
 unmeasured_perturbation_std: 0.001
 apply_loss_function: false