diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index d176b3066b5e09dd442ac2bf1a8219f98ccc6d4a..f6cf9647c1066e2786b9ba02b07f51abf328ad4c 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,107 +1,76 @@
-image: labrobotica/ceres:1.14
-
-before_script:
-  ##
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_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
 
-  ##
-  ## Create the SSH directory and give it the right permissions
-  ##
-  - ls
+  # update apt
   - apt-get update
-  - apt-get install -y build-essential cmake 
 
-# SPDLOG
-#  - apt-get install -y libspdlog-dev
-  - if [ -d spdlog ]; then
-  -   echo "directory exists" 
-  -   if [ "$(ls -A ./spdlog)" ]; then 
-  -     echo "directory not empty" 
-  -     cd spdlog
-  -     git pull
-  -   else 
-  -     echo "directory empty" 
-  -     git clone https://github.com/gabime/spdlog.git
-  -     cd spdlog
-  -   fi
+.install_wolf_template: &install_wolf_definition
+  - if [ -d wolf ]; then
+  -   echo "directory wolf exists"
+  -   cd wolf
+  -   git pull
   - else
-  -   echo "directory inexistent" 
-  -   git clone https://github.com/gabime/spdlog.git
-  -   cd spdlog
+  -   git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+  -   cd wolf
   - fi
-  - git fetch
-  - git checkout v0.17.0
   - mkdir -pv build
   - cd build
-  - ls
-  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+  - make -j2
+  - ctest -j2
   - make install
   - cd ../..
 
-# YAML
-#  - apt-get install -y libyaml-cpp-dev
-  - if [ -d yaml-cpp ]; then
-  -   echo "directory exists" 
-  -   if [ "$(ls -A ./yaml-cpp)" ]; then 
-  -     echo "directory not empty" 
-  -     cd yaml-cpp
-  -     git pull
-  -   else 
-  -     echo "directory empty" 
-  -     git clone https://github.com/jbeder/yaml-cpp.git
-  -     cd yaml-cpp
-  -   fi
-  - else
-  -   echo "directory inexistent" 
-  -   git clone https://github.com/jbeder/yaml-cpp.git
-  -   cd yaml-cpp
-  - fi
-  - mkdir -pv build
-  - cd build
-  - ls
-  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
-  - make install
-  - cd ../..
-#Wolf core
-  - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
-  - cd wolf
+.build_and_test_template: &build_and_test_definition
   - mkdir -pv build
   - cd build
   - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
-  - make -j$(nproc)
-  - ctest -j$(nproc)
+  - make -j2
+  - ctest -j2
   - make install
-  - cd ../..
 
-wolf_build_and_test:
-  stage: build
+############ UBUNTU 16.04 TESTS ############
+wolf_build_and_test:xenial:
+  image: labrobotica/wolf_deps:16.04
+  cache:
+    - key: wolf-xenial
+      paths:
+      - wolf/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+  script:
+    - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+wolf_build_and_test:bionic:
+  image: labrobotica/wolf_deps:18.04
+  cache:
+    - key: wolf-bionic
+      paths:
+      - wolf/
   except:
     - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
   script:
-    - mkdir -pv build
-    - cd build
-    - ls # we can check whether the directory was already full
-    - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
-    - make -j$(nproc)
-    - ctest -j$(nproc)
-    - make install
+    - *build_and_test_definition
diff --git a/README.md b/README.md
index dd79d5e177365518822ce675cf4fce278e65e45f..2345eb53e24a8f4179ea43625f727662fba7df26 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
 WOLF - Windowed Localization Frames | IMU Plugin
 ===================================
 
-For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
\ No newline at end of file
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 17c80316246bdd977ed59b4edc0aea6f34dca4f7..0d52b68b7a005597b958d90a334a620819f581fe 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -4,6 +4,7 @@
 //Wolf includes
 #include "imu/feature/feature_imu.h"
 #include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
 #include "core/factor/factor_autodiff.h"
 #include "core/math/rotations.h"
 
@@ -235,9 +236,9 @@ Eigen::Vector9d FactorImu::error()
     Map<const Vector3d > acc_bias(bias.data());
     Map<const Vector3d > gyro_bias(bias.data() + 3);
 
-    Eigen::Vector9d delta_exp = expectation();
+    Eigen::Vector10d delta_exp = expectation();
 
-    Eigen::Vector9d delta_preint = getMeasurement();
+    Eigen::Vector10d delta_preint = getMeasurement();
 
     Eigen::Vector9d delta_step;
 
@@ -247,9 +248,9 @@ Eigen::Vector9d FactorImu::error()
 
     Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
 
-    Eigen::Vector9d res = imu::diff(delta_exp, delta_corr);
+    Eigen::Vector9d err = imu::diff(delta_exp, delta_corr);
 
-    return res;
+    return err;
 }
 
 template<typename D1, typename D2, typename D3>
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index d9b0b58454132c8abaa11d50a5847d18b829337f..ac93e3fe2228f39fab603cec3b889e598309086f 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -566,7 +566,7 @@ inline void plus(const MatrixBase<D1>& dp1, const MatrixBase<D2>& dq1, const Mat
 template<typename D1, typename D2, typename D3>
 inline void plus(const MatrixBase<D1>& d1,
                  const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& d_pert)
+                 MatrixBase<D3>& d)
 {
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
@@ -574,11 +574,11 @@ inline void plus(const MatrixBase<D1>& d1,
     Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
     Map<const Matrix<typename D2::Scalar, 3, 1> >   do2    ( & d2(3) );
     Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & d2(6) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         dp_p ( & d_pert(0) );
-    Map<Quaternion<typename D3::Scalar> >           dq_p ( & d_pert(3) );
-    Map<Matrix<typename D3::Scalar, 3, 1> >         dv_p ( & d_pert(7) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dp     ( & d (0) );
+    Map<Quaternion<typename D3::Scalar> >           dq     ( & d (3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dv     ( & d (7) );
 
-    plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
+    plus(dp1, dq1, dv1, dp2, do2, dv2, dp, dq, dv);
 }
 
 template<typename D1, typename D2>
diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp
index 87201a4699e047e1873fe12de0ce121bcd96d1db..f2631e901d8d0e27d7564ef523f660820ef6d524 100644
--- a/src/yaml/sensor_imu_yaml.cpp
+++ b/src/yaml/sensor_imu_yaml.cpp
@@ -28,7 +28,6 @@ static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_d
     if (config["type"].as<std::string>() == "SensorImu")
     {
         YAML::Node variances        = config["motion_variances"];
-        YAML::Node kf_vote          = config["keyframe_vote"];
 
         ParamsSensorImuPtr params = std::make_shared<ParamsSensorImu>();
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f89523a4bc2ac3904b444ecce442177311cf48c2..f5e97ef62c6f51996e654b0c4db3c6d7730fffa1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -43,6 +43,12 @@ target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
 wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp)
 target_link_libraries(gtest_factor_imu2d_with_gravity ${PLUGIN_NAME} ${wolf_LIBRARY})
 
+wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
+target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
 wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
 target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY})
 
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 161157b8e079a5dd5f3dd4c873e754e021003c6f..d666285f444465fa01dc5d4c20fdc182efea9863 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -475,28 +475,10 @@ class Process_Factor_Imu : public testing::Test
         {
             // This perturbs states to estimate around the exact value, then assigns to the keyframe
             // Perturbations are applied only if the state block is unfixed
-
-            VectorXd x_pert(10);
-
-            // KF 0
-            x_pert = x0;
-            if (!p0_fixed)
-                x_pert.head(3)      += Vector3d::Random() * 0.01;
-            if (!q0_fixed)
-                x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
-            if (!v0_fixed)
-                x_pert.tail(3)      += Vector3d::Random() * 0.01;
-            KF_0->setState(x_pert);
-
-            // KF 1
-            x_pert = x1_exact;
-            if (!p1_fixed)
-                x_pert.head(3)      += Vector3d::Random() * 0.01;
-            if (!q1_fixed)
-                x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
-            if (!v1_fixed)
-                x_pert.tail(3)      += Vector3d::Random() * 0.01;
-            KF_1->setState(x_pert);
+            KF_0->setState(x0);
+            KF_0->perturb();
+            KF_1->setState(x1_exact);
+            KF_1->perturb();
         }
 
         virtual void buildProblem()
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..863147d289cf33dfe4dc272df02d65a9f3dd70b3
--- /dev/null
+++ b/test/gtest_imu2d_static_init.cpp
@@ -0,0 +1,534 @@
+/*
+ * gtest_imu2d_static_init.cpp
+ *
+ *  Created on: Sept 2021
+ *      Author: igeer
+ */
+
+#include <fstream>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu2d.h"
+#include "imu/math/imu2d_tools.h"
+#include "imu/sensor/sensor_imu2d.h"
+#include "core/capture/capture_void.h"
+#include <core/factor/factor_relative_pose_2d.h>
+
+using namespace Eigen;
+using namespace wolf;
+
+class ProcessorImu2dStaticInitTest : public testing::Test
+{
+
+    public: //These can be accessed in fixtures
+        wolf::ProblemPtr problem_ptr_;
+        wolf::SensorBasePtr sensor_ptr_;
+        wolf::ProcessorMotionPtr processor_motion_;
+        wolf::TimeStamp t;
+        wolf::TimeStamp t0;
+        double dt;
+        Vector6d data;
+        Matrix6d data_cov;
+        FrameBasePtr KF0_;
+        FrameBasePtr first_frame_;
+        FrameBasePtr last_frame_;
+        SolverCeresPtr solver_;
+
+    //a new of this will be created for each new test
+    void SetUp() override
+    {
+        WOLF_INFO("Doing setup...");
+        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_ROOT_DIR;
+
+        // Wolf problem
+        problem_ptr_ = Problem::create("POV", 2);
+        Vector3d extrinsics = (Vector3d() << 0,0,0).finished();
+        sensor_ptr_ = problem_ptr_->installSensor("SensorImu2d", "Main Imu", extrinsics,  wolf_root + "/test/yaml/sensor_imu2d_static_init.yaml");
+        ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu2d_static_init.yaml");
+        processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+        // Time and data variables
+        dt = 0.1;
+        t0.set(0);
+        t = t0;
+        data = Vector6d::Random();
+        data_cov = Matrix6d::Identity();
+        last_frame_ = nullptr;
+        first_frame_ = nullptr;
+
+        // Create the solver
+        solver_ = make_shared<SolverCeres>(problem_ptr_);
+        
+        // Set the origin
+        VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+        VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
+        //KF0_ = problem_ptr_->setPriorFix(x_origin, t0, dt/2);
+        KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+    }
+
+    void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+    {
+      //Data
+      data.head(2) = body_magnitudes.head(2);
+      data(5) = body_magnitudes(2);
+      data.head(2) += bias_groundtruth.head(2);
+      data(5) += bias_groundtruth(2);
+
+      //Set bias initial guess
+      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+      processor_motion_->setOrigin(KF0_);
+
+#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";
+      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 == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+      file_est << header_est;
+#endif
+
+
+      int n_frames = 0;
+      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+        WOLF_INFO("\n------------------------------------------------------------------------");
+
+        //Create and process capture  
+        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+        C->process();
+
+        auto state = problem_ptr_->getState();
+        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+        // pre-solve print to CSV
+        if(testing_var == 3){
+        file_est << std::fixed << t << ";"
+            << state['P'].norm() << ";"
+            << state['V'].norm() << ";"
+            << state['O'] << ";"
+            << bias_est.head(2).norm() << ";"
+            << bias_est(2) << ";"
+            << bias_preint.head(2).norm() << ";"
+            << bias_preint(2) << "\n";
+        }
+        else
+        {
+        file_est << std::fixed << t << ";"
+            << state['P'](testing_var) << ";"
+            << state['V'](testing_var) << ";"
+            << state['O'] << ";"
+            << bias_est(testing_var) << ";"
+            << bias_est(2) << ";"
+            << bias_preint(testing_var) << ";"
+            << bias_preint(2) << "\n";
+
+        }
+#endif
+
+        // new frame
+        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+        {
+          n_frames++;
+          last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+          // impose static
+          last_frame_->getP()->setState(KF0_->getP()->getState());
+          last_frame_->getO()->setState(KF0_->getO()->getState());
+          last_frame_->getV()->setZero();
+
+          //Fix frame
+          last_frame_->fix();
+
+          //Solve
+          if(n_frames > 0)
+          {
+            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("Solver Report: ", report);
+
+            state = problem_ptr_->getState();
+            bias_est = sensor_ptr_->getIntrinsic()->getState();
+            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+            
+            WOLF_INFO("The current problem is:");
+            problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            if(testing_var == 3){
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'].norm() << ";"
+                << state['V'].norm() << ";"
+                << state['O'] << ";"
+                << bias_est.head(2).norm() << ";"
+                << bias_est(2) << ";"
+                << bias_preint.head(2).norm() << ";"
+                << bias_preint(2) << "\n";
+            }
+            else
+            {
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](testing_var) << ";"
+                << state['V'](testing_var) << ";"
+                << state['O'] << ";"
+                << bias_est(testing_var) << ";"
+                << bias_est(2) << ";"
+                << bias_preint(testing_var) << ";"
+                << bias_preint(2) << "\n";
+
+            }
+#endif
+          }
+
+        }
+
+
+        WOLF_INFO("Number of frames is ", n_frames);
+        WOLF_INFO("The state is: ", state);
+        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+        if(small_tol)
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+          }
+        }
+        else
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+          }
+        }
+      }
+
+#if WRITE_CSV_FILE
+      file_est.close();
+#endif
+
+    }
+
+    void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+    {
+      //Data
+      data.head(2) = body_magnitudes.head(2);
+      data(5) = body_magnitudes(2);
+      data.head(2) += bias_groundtruth.head(2);
+      data(5) += bias_groundtruth(2);
+
+      //Set bias initial guess
+      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+      processor_motion_->setOrigin(KF0_);
+
+#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";
+      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 == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+      file_est << header_est;
+#endif
+
+      int n_frames = 0;
+      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+        WOLF_INFO("\n------------------------------------------------------------------------");
+
+        //Create and process capture  
+        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+        C->process();
+
+        auto state = problem_ptr_->getState();
+        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+        // pre-solve print to CSV
+        if(testing_var == 3){
+        file_est << std::fixed << t << ";"
+            << state['P'].norm() << ";"
+            << state['V'].norm() << ";"
+            << state['O'] << ";"
+            << bias_est.head(2).norm() << ";"
+            << bias_est(2) << ";"
+            << bias_preint.head(2).norm() << ";"
+            << bias_preint(2) << "\n";
+        }
+        else
+        {
+        file_est << std::fixed << t << ";"
+            << state['P'](testing_var) << ";"
+            << state['V'](testing_var) << ";"
+            << state['O'] << ";"
+            << bias_est(testing_var) << ";"
+            << bias_est(2) << ";"
+            << bias_preint(testing_var) << ";"
+            << bias_preint(2) << "\n";
+
+        }
+#endif
+
+        // new frame
+        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+        {
+          n_frames++;
+          last_frame_ = processor_motion_->getOrigin()->getFrame();
+          
+          // impose zero odometry
+          processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
+
+          // add zero displacement and rotation capture & feature & factor with all previous frames
+          assert(sensor_ptr_->getProblem());
+          for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
+              frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
+              frm_pair++)
+          {
+            if (frm_pair->second == last_frame_)
+              break;
+            auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+            auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+                "FeatureZeroOdom",
+                Vector3d::Zero(),
+                Eigen::MatrixXd::Identity(3,3) * 0.01);
+
+            FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+                feature_zero,
+                frm_pair->second,
+                nullptr,
+                false,
+                TOP_MOTION);
+
+          }
+
+          // impose static
+          last_frame_->getV()->setZero();
+
+          //Fix frame
+          last_frame_->getV()->fix();
+
+          //Solve
+          if(n_frames > 0)
+          {
+            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("Solver Report: ", report);
+
+            state = problem_ptr_->getState();
+            bias_est = sensor_ptr_->getIntrinsic()->getState();
+            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+            
+            WOLF_INFO("The current problem is:");
+            problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            if(testing_var == 3){
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'].norm() << ";"
+                << state['V'].norm() << ";"
+                << state['O'] << ";"
+                << bias_est.head(2).norm() << ";"
+                << bias_est(2) << ";"
+                << bias_preint.head(2).norm() << ";"
+                << bias_preint(2) << "\n";
+            }
+            else
+            {
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](testing_var) << ";"
+                << state['V'](testing_var) << ";"
+                << state['O'] << ";"
+                << bias_est(testing_var) << ";"
+                << bias_est(2) << ";"
+                << bias_preint(testing_var) << ";"
+                << bias_preint(2) << "\n";
+
+            }
+#endif
+          }
+
+        }
+
+
+
+        WOLF_INFO("Number of frames is ", n_frames);
+        WOLF_INFO("The state is: ", state);
+        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+        if(small_tol)
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+          }
+        }
+        else
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+          }
+        }
+      }
+
+#if WRITE_CSV_FILE
+      file_est.close();
+#endif
+
+    }
+};
+
+
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); 
+   Vector3d bias_initial_guess = Vector3d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); 
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); 
+   Vector3d bias_initial_guess = Vector3d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); 
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); 
+   Vector3d bias_initial_guess = Vector3d::Zero();
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); 
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = (Vector3d() <<  0, 0, 0.01).finished(); 
+   Vector3d bias_initial_guess = Vector3d::Zero();
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); 
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
+}
+
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = Vector3d::Random()*100;
+   bias_initial_guess(2) = 0;
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = Vector3d::Zero();
+   Vector3d bias_initial_guess = Vector3d::Random()*0.01;
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); 
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, realistic_test)
+{
+   Vector3d body_magnitudes = Vector3d::Zero();
+   Vector3d bias_groundtruth = (Vector3d() << -0.529550648247,
+                                               0.278316717683,
+                                              -0.00122491355676).finished();
+   Vector3d bias_initial_guess = Vector3d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); 
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    //    ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+    return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ad48b186079c12af08b6db42cc014207bc7bc38
--- /dev/null
+++ b/test/gtest_imu_static_init.cpp
@@ -0,0 +1,533 @@
+/*
+ * gtest_imu_static_init.cpp
+ *
+ *  Created on: Sept 2021
+ *      Author: igeer
+ */
+#include <fstream>
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu.h"
+#include "imu/math/imu_tools.h"
+#include "imu/sensor/sensor_imu.h"
+#include "core/capture/capture_void.h"
+#include <core/factor/factor_relative_pose_3d.h>
+
+using namespace Eigen;
+using namespace wolf;
+
+/**
+ *  SET TO TRUE  TO PRODUCE CSV FILE
+ *  SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE  true
+
+class ProcessorImuStaticInitTest : public testing::Test
+{
+
+    public: //These can be accessed in fixtures
+        wolf::ProblemPtr problem_ptr_;
+        wolf::SensorBasePtr sensor_ptr_;
+        wolf::ProcessorMotionPtr processor_motion_;
+        wolf::TimeStamp t;
+        wolf::TimeStamp t0;
+        double dt;
+        Vector6d data;
+        Matrix6d data_cov;
+        FrameBasePtr KF0_;
+        FrameBasePtr first_frame_;
+        FrameBasePtr last_frame_;
+        SolverCeresPtr solver_;
+
+    //a new of this will be created for each new test
+    void SetUp() override
+    {
+        WOLF_INFO("Doing setup...");
+        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_ROOT_DIR;
+
+        // Wolf problem
+        problem_ptr_ = Problem::create("POV", 3);
+        Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished();
+        sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics,  wolf_root + "/test/yaml/sensor_imu_static_init.yaml");
+        ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu_static_init.yaml");
+        processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+        // Time and data variables
+        dt = 0.1;
+        t0.set(0);
+        t = t0;
+        data = Vector6d::Random();
+        data_cov = Matrix6d::Identity();
+        last_frame_ = nullptr;
+        first_frame_ = nullptr;
+
+        // Create the solver
+        solver_ = make_shared<SolverCeres>(problem_ptr_);
+        
+        // Set the origin
+        VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()});
+        WOLF_INFO("x0c is: \n", x0c);
+        //KF0_ = problem_ptr_->setPriorFix(x0c, t0, dt/2);
+
+        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_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+    }
+
+    void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+    {
+      //Data
+      data = body_magnitudes;
+      data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+      data += bias_groundtruth;
+
+      //Set bias initial guess
+      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+      processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+      std::fstream file_est; 
+      file_est.open("./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";
+      if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_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;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+      file_est << header_est;
+#endif
+
+
+      int n_frames = 0;
+      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+        WOLF_INFO("\n------------------------------------------------------------------------");
+
+        //Create and process capture  
+        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+        C->process();
+
+        auto state = problem_ptr_->getState();
+        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+        // pre-solve print to CSV
+        if(testing_var == 3){
+        file_est << std::fixed << t << ";"
+            << state['P'].norm() << ";"
+            << state['V'].norm() << ";"
+            << state['O'].norm() << ";"
+            << bias_est.head(3).norm() << ";"
+            << bias_est.tail(3).norm() << ";"
+            << bias_preint.head(3).norm() << ";"
+            << bias_preint.tail(3).norm() << "\n";
+        }
+        else
+        {
+        file_est << std::fixed << t << ";"
+            << state['P'](testing_var) << ";"
+            << state['V'](testing_var) << ";"
+            << state['O'](testing_var) << ";"
+            << bias_est(testing_var) << ";"
+            << bias_est(testing_var+3) << ";"
+            << bias_preint(testing_var) << ";"
+            << bias_preint(testing_var+3) << "\n";
+
+        }
+#endif
+
+        // new frame
+        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+        {
+          n_frames++;
+          last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+          // impose static
+          last_frame_->getP()->setState(KF0_->getP()->getState());
+          last_frame_->getO()->setState(KF0_->getO()->getState());
+          last_frame_->getV()->setZero();
+
+          //Fix frame
+          last_frame_->fix();
+
+          //Solve
+          if(n_frames > 0)
+          {
+            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("Solver Report: ", report);
+
+            state = problem_ptr_->getState();
+            bias_est = sensor_ptr_->getIntrinsic()->getState();
+            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+            
+            WOLF_INFO("The current problem is:");
+            problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            if(testing_var == 3){
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'].norm() << ";"
+                << state['V'].norm() << ";"
+                << state['O'].norm() << ";"
+                << bias_est.head(3).norm() << ";"
+                << bias_est.tail(3).norm() << ";"
+                << bias_preint.head(3).norm() << ";"
+                << bias_preint.tail(3).norm() << "\n";
+            }
+            else
+            {
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](testing_var) << ";"
+                << state['V'](testing_var) << ";"
+                << state['O'](testing_var) << ";"
+                << bias_est(testing_var) << ";"
+                << bias_est(testing_var+3) << ";"
+                << bias_preint(testing_var) << ";"
+                << bias_preint(testing_var+3) << "\n";
+
+            }
+#endif
+          }
+
+        }
+
+
+
+        WOLF_INFO("Number of frames is ", n_frames);
+        WOLF_INFO("The state is: ", state);
+        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+        if(small_tol)
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+          }
+        }
+        else
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+          }
+        }
+      }
+
+#if WRITE_CSV_FILE
+      file_est.close();
+#endif
+
+    }
+
+    void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+    {
+      //Data
+      data = body_magnitudes;
+      data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+      data += bias_groundtruth;
+
+      //Set bias initial guess
+      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+      processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+      std::fstream file_est; 
+      file_est.open("./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";
+      if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_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;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+      file_est << header_est;
+#endif
+
+
+      int n_frames = 0;
+      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+        WOLF_INFO("\n------------------------------------------------------------------------");
+
+        //Create and process capture  
+        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+        C->process();
+
+        auto state = problem_ptr_->getState();
+        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+        // pre-solve print to CSV
+        if(testing_var == 3){
+        file_est << std::fixed << t << ";"
+            << state['P'].norm() << ";"
+            << state['V'].norm() << ";"
+            << state['O'].norm() << ";"
+            << bias_est.head(3).norm() << ";"
+            << bias_est.tail(3).norm() << ";"
+            << bias_preint.head(3).norm() << ";"
+            << bias_preint.tail(3).norm() << "\n";
+        }
+        else
+        {
+        file_est << std::fixed << t << ";"
+            << state['P'](testing_var) << ";"
+            << state['V'](testing_var) << ";"
+            << state['O'](testing_var) << ";"
+            << bias_est(testing_var) << ";"
+            << bias_est(testing_var+3) << ";"
+            << bias_preint(testing_var) << ";"
+            << bias_preint(testing_var+3) << "\n";
+
+        }
+#endif
+
+        // new frame
+        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+        {
+          n_frames++;
+          last_frame_ = processor_motion_->getOrigin()->getFrame();
+          
+          // impose zero odometry
+          processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
+
+          // add zero displacement and rotation capture & feature & factor with all previous frames
+          assert(sensor_ptr_->getProblem());
+          for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
+              frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
+              frm_pair++)
+          {
+            if (frm_pair->second == last_frame_)
+              break;
+            auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+            auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+                "FeatureZeroOdom",
+                Vector7d::Zero(),
+                Eigen::MatrixXd::Identity(6,6) * 0.01);
+
+            FactorBase::emplace<FactorRelativePose3d>(feature_zero,
+                feature_zero,
+                frm_pair->second,
+                nullptr,
+                false,
+                TOP_MOTION);
+
+          }
+
+          // impose static
+          last_frame_->getV()->setZero();
+
+          //Fix frame
+          last_frame_->getV()->fix();
+
+          //Solve
+          if(n_frames > 0)
+          {
+            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("Solver Report: ", report);
+
+            state = problem_ptr_->getState();
+            bias_est = sensor_ptr_->getIntrinsic()->getState();
+            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+            
+            WOLF_INFO("The current problem is:");
+            problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            if(testing_var == 3){
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'].norm() << ";"
+                << state['V'].norm() << ";"
+                << state['O'].norm() << ";"
+                << bias_est.head(3).norm() << ";"
+                << bias_est.tail(3).norm() << ";"
+                << bias_preint.head(3).norm() << ";"
+                << bias_preint.tail(3).norm() << "\n";
+            }
+            else
+            {
+              file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](testing_var) << ";"
+                << state['V'](testing_var) << ";"
+                << state['O'](testing_var) << ";"
+                << bias_est(testing_var) << ";"
+                << bias_est(testing_var+3) << ";"
+                << bias_preint(testing_var) << ";"
+                << bias_preint(testing_var+3) << "\n";
+
+            }
+#endif
+          }
+
+        }
+
+
+
+        WOLF_INFO("Number of frames is ", n_frames);
+        WOLF_INFO("The state is: ", state);
+        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+        if(small_tol)
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+          }
+        }
+        else
+        {
+          if(n_frames == 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+          }
+          else if (n_frames > 2)
+          {
+            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+          }
+        }
+      }
+
+#if WRITE_CSV_FILE
+      file_est.close();
+#endif
+
+    }
+};
+
+
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
+   Vector6d bias_initial_guess = Vector6d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); 
+   Vector6d bias_initial_guess = Vector6d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); 
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
+   Vector6d bias_initial_guess = Vector6d::Zero();
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); 
+   Vector6d bias_initial_guess = Vector6d::Zero();
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); 
+
+   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
+}
+
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = Vector6d::Random()*100;
+   bias_initial_guess.tail(3) = Vector3d::Zero();
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); 
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random)
+{
+   Vector6d body_magnitudes = Vector6d::Zero();
+   Vector6d bias_groundtruth = Vector6d::Zero();
+   Vector6d bias_initial_guess = Vector6d::Random()*0.01;
+
+   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); 
+}
+
+
+int main(int argc, char **argv)
+{
+    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 4dbac962133c7d8da9f76e152a09e0749c1f2e71..a013072abf3064c5a2ac26302a1fcfe145f1a0b5 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -155,7 +155,7 @@ TEST_F(ProcessorImuTest, test1)
  *  SET TO TRUE  TO PRODUCE CSV FILE
  *  SET TO FALSE TO STOP PRODUCING CSV FILE
  */
-#define WRITE_CSV_FILE  false
+#define WRITE_CSV_FILE  true
 
 TEST_F(ProcessorImuTest, getState)
 {
diff --git a/test/processor_imu_UnitTester.h b/test/processor_imu_UnitTester.h
index f9990707cb56f5ec6d1357683aa749503343ca1b..e816349ecccd892f0ca3f1d7a1a4edbbb043eded 100644
--- a/test/processor_imu_UnitTester.h
+++ b/test/processor_imu_UnitTester.h
@@ -108,16 +108,16 @@ namespace wolf {
             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_;
-        }
+//        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.
diff --git a/test/yaml/imu2d_static_init.yaml b/test/yaml/imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4978c8e2863908df72a336da1225667411d3022a
--- /dev/null
+++ b/test/yaml/imu2d_static_init.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu2d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+    voting_active:      true
+    voting_aux_active:  false
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    dist_traveled:      100000000000   # meters
+    angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..46393f30b8f24bf645bfb1272764e6c8915e5dc2
--- /dev/null
+++ b/test/yaml/imu_static_init.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+    voting_active:      true
+    voting_aux_active:  false
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    dist_traveled:      100000000000   # meters
+    angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3c78a00d35c785fe73381d8f6ce99a705d27ce77
--- /dev/null
+++ b/test/yaml/sensor_imu.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.800     # m/s2    - initial bias 
+    wb_initial_stdev:       0.350     # rad/sec - initial bias 
+    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)
diff --git a/test/yaml/sensor_imu2d_static_init.yaml b/test/yaml/sensor_imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936
--- /dev/null
+++ b/test/yaml/sensor_imu2d_static_init.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu2d"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.800     # m/s2    - initial bias 
+    wb_initial_stdev:       0.350     # rad/sec - initial bias 
+    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)
diff --git a/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..700b8d6762b91b38e96391f6032b2c7c4221eabc
--- /dev/null
+++ b/test/yaml/sensor_imu_static_init.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.001     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.001    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.001     # m/s2    - initial bias 
+    wb_initial_stdev:       0.001     # rad/sec - initial bias 
+    ab_rate_stdev:          0.001       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.001    # rad/s/sqrt(s)