diff --git a/.gitignore b/.gitignore
index b4e53703e896e0c36e288c8c4e1e13e71c42ab2d..4d9c52e82fe8dee50c99c9a1c8548575962ce658 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,4 +39,5 @@ src/examples/map_apriltag_save.yaml
 .ccls-root
 compile_commands.json
 .vimspector.json
-est.csv
+est*.csv
+.clang-format
diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
index e1b0c2e86eb5212679e7c75db2ba2f25fc039348..1afbb00a6ab5112cbad1555098985ae1f0687f8b 100644
--- a/demos/processor_imu.yaml
+++ b/demos/processor_imu.yaml
@@ -9,3 +9,8 @@ keyframe_vote:
     max_buff_length:  20000    # motion deltas
     dist_traveled:      2.0   # meters
     angle_turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+    enable: false 
+    method: "G"
+    averaging_length: 0.10 # seconds
\ No newline at end of file
diff --git a/demos/processor_imu_bootstrap.yaml b/demos/processor_imu_bootstrap.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3a6c6c9587ab23aeb66ae95495c8054be46e0c96
--- /dev/null
+++ b/demos/processor_imu_bootstrap.yaml
@@ -0,0 +1,17 @@
+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.00001
+
+keyframe_vote:
+    voting_active:      false
+    max_time_span:      2.0   # seconds
+    max_buff_length:  20000    # motion deltas
+    dist_traveled:      2.0   # meters
+    angle_turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+    enable: true 
+    method: "G"             # methods: "G", "STATIC" or "V0_G"
+    averaging_length: 0.10  # seconds
+    keyframe_provider_processor_name: "processor_other_name" # Not yet implemented
\ No newline at end of file
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index f579ae2dc44fc2f7cd5a6d1b70ba49df21dfdcb7..316405e50afa3ce2a620766e6c3a910615797536 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -257,7 +257,7 @@ inline void compose(const MatrixBase<D1>& d1,
 //    Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First  Delta, DR
 //    Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
 //
-//    // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
+//    // Jac wrt first delta 
 //    J_sum_d1.clear();
 //    J_sum_d1.emplace('P','P', Matrix3d::Identity());        // dDp'/dDp = I
 //    J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ;   // dDp'/dDo
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 7101c04eb11afd5285fd2f77dd2f9c281780c5fc..c5744fbae31fea609aad91e70da9cf55cf83bb66 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -282,7 +282,7 @@ inline void compose(const VectorComposite& d1,
     Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First  Delta, DR
     Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
 
-    // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
+    // Jac wrt first delta 
     J_sum_d1.clear();
     J_sum_d1.emplace('P','P', Matrix3d::Identity());        // dDp'/dDp = I
     J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ;   // dDp'/dDo
@@ -868,6 +868,65 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
     return  data;
 }
 
+/** extract local g and v0 from three keyframes and deltas
+ * @param p0 position of kf0
+ * @param q0 orientation of kf0
+ * @param p1 position of kf0
+ * @param q1 orientation of kf0
+ * @param p2 position of kf0
+ * @param Dt01 time delta between kf0 and kf1
+ * @param Dp01 position delta between kf0 and kf1
+ * @param Dv01 orientation delta between kf0 and kf1
+ * @param Dt12 time delta between kf1 and kf2
+ * @param Dp12 position delta between kf1 and kf2
+ * @param Dv12 orientation delta between kf1 and kf2
+ * 
+ * This method needs 3 kfs to solve. Inspired on Lupton-11 with a revised 
+ * implementation by jsola-22, see https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
+ */
+template <typename TP0,
+          typename TQ0,
+          typename TP1,
+          typename TQ1,
+          typename TP2,
+          typename TDT01,
+          typename TDP01,
+          typename TDV01,
+          typename TDT12,
+          typename TDP12,
+          typename TV0,
+          typename TG>
+void extract_v0_g(const MatrixBase<TP0> &p0,
+                  const QuaternionBase<TQ0> &q0,
+                  const MatrixBase<TP1> &p1,
+                  const QuaternionBase<TQ1> &q1,
+                  const MatrixBase<TP2> &p2,
+                  const TDT01 &Dt01,
+                  const MatrixBase<TDP01> &Dp01,
+                  const MatrixBase<TDV01> &Dv01,
+                  const TDT12 &Dt12,
+                  const MatrixBase<TDP12> &Dp12,
+                  MatrixBase<TV0> &v0,
+                  MatrixBase<TG> &g)
+{
+    // full time period between kfs 0 and 2
+    const TDT01 Dt02 = Dt01 + Dt12;
+
+    // Scalar coefficients of entries of 6x6 matrix A.inv
+    const TDT01 ai_v0 = (Dt02 / (Dt01 * Dt12));
+    const TDT01 ai_v1 = (-Dt01 / (Dt12 * Dt02));
+    const TDT01 ai_g0 = (-2 / (Dt01 * Dt12));
+    const TDT01 ai_g1 = (2 / (Dt12 * Dt02));
+
+    // 3-vector entries of 6-vector b
+    Matrix<typename TV0::Scalar, 3, 1> b0 = p1 - p0 - q0 * Dp01;
+    Matrix<typename TV0::Scalar, 3, 1> b1 = p2 - p0 - q0 * (Dp01 + Dv01 * Dt12) - q1 * Dp12;
+
+    // output vectors -- this is x = A.inv * b
+    v0 = ai_v0 * b0 + ai_v1 * b1;
+    g  = ai_g0 * b0 + ai_g1 * b1;
+}
+
 } // namespace imu
 } // namespace wolf
 
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index d8efc7cbd3221654da4721ff8bc21e6c15bc4353..94388550eedf13e50e2a16be7d3ca060159e21c4 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -32,11 +32,41 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu);
 
 struct ParamsProcessorImu : public ParamsProcessorMotion
 {
+    bool bootstrap_enable;
+    enum BootstrapMethod 
+    {
+        BOOTSTRAP_STATIC,
+        BOOTSTRAP_G,
+        BOOTSTRAP_V0_G
+    } bootstrap_method;
+    double bootstrap_averaging_length;
+
     ParamsProcessorImu() = default;
     ParamsProcessorImu(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorMotion(_unique_name, _server)
     {
-        //
+        bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap/enable");
+        if (_server.hasParam(prefix + _unique_name + "/bootstrap/method"))
+        {
+            string str = _server.getParam<string>(prefix + _unique_name + "/bootstrap/method");
+            std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+            if (str == "STATIC" /**/)
+            {
+                bootstrap_method = BOOTSTRAP_STATIC;
+                bootstrap_averaging_length =
+                    _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+            }
+            if (str == "G" /*     */)
+            {
+                bootstrap_method = BOOTSTRAP_G;
+                bootstrap_averaging_length =
+                    _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+            }
+            if (str == "V0_G" /*  */)
+            {
+                bootstrap_method = BOOTSTRAP_V0_G;
+            }
+        }
     }
     std::string print() const override
     {
@@ -51,55 +81,60 @@ class ProcessorImu : public ProcessorMotion{
     public:
         ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu);
         ~ProcessorImu() override;
+        WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
         void configure(SensorBasePtr _sensor) override { };
 
-        WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
         void preProcess() override;
+        void bootstrapEnable(bool _bootstrap_enable = true);
 
-    protected:
-        void computeCurrentDelta(const Eigen::VectorXd& _data,
-                                         const Eigen::MatrixXd& _data_cov,
-                                         const Eigen::VectorXd& _calib,
-                                         const double _dt,
-                                         Eigen::VectorXd& _delta,
-                                         Eigen::MatrixXd& _delta_cov,
-                                         Eigen::MatrixXd& _jacobian_calib) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _dt,
-                                    Eigen::VectorXd& _delta_preint_plus_delta) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _dt,
-                                    Eigen::VectorXd& _delta_preint_plus_delta,
-                                    Eigen::MatrixXd& _jacobian_delta_preint,
-                                    Eigen::MatrixXd& _jacobian_delta) const override;
-        void statePlusDelta(const VectorComposite& _x,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _Dt,
-                                    VectorComposite& _x_plus_delta) const override;
-        Eigen::VectorXd deltaZero() const override;
-        Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
-                                             const Eigen::VectorXd& delta_step) const override;
-        VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
-        void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
-        bool voteForKeyFrame() const override;
-        CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                                const SensorBasePtr& _sensor,
-                                                const TimeStamp& _ts,
-                                                const VectorXd& _data,
-                                                const MatrixXd& _data_cov,
-                                                const VectorXd& _calib,
-                                                const VectorXd& _calib_preint,
-                                                const CaptureBasePtr& _capture_origin) override;
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override;
+      protected:
+        void             computeCurrentDelta(const Eigen::VectorXd& _data,
+                                             const Eigen::MatrixXd& _data_cov,
+                                             const Eigen::VectorXd& _calib,
+                                             const double           _dt,
+                                             Eigen::VectorXd&       _delta,
+                                             Eigen::MatrixXd&       _delta_cov,
+                                             Eigen::MatrixXd&       _jacobian_calib) const override;
+        void             deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _dt,
+                                        Eigen::VectorXd&       _delta_preint_plus_delta) const override;
+        void             deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _dt,
+                                        Eigen::VectorXd&       _delta_preint_plus_delta,
+                                        Eigen::MatrixXd&       _jacobian_delta_preint,
+                                        Eigen::MatrixXd&       _jacobian_delta) const override;
+        void             statePlusDelta(const VectorComposite& _x,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _Dt,
+                                        VectorComposite&       _x_plus_delta) const override;
+        Eigen::VectorXd  deltaZero() const override;
+        Eigen::VectorXd  correctDelta(const Eigen::VectorXd& delta_preint,
+                                      const Eigen::VectorXd& delta_step) const override;
+        VectorXd         getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+        void             setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+        bool             voteForKeyFrame() const override;
+        CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                        const SensorBasePtr&  _sensor,
+                                        const TimeStamp&      _ts,
+                                        const VectorXd&       _data,
+                                        const MatrixXd&       _data_cov,
+                                        const VectorXd&       _calib,
+                                        const VectorXd&       _calib_preint,
+                                        const CaptureBasePtr& _capture_origin) override;
+        FeatureBasePtr   emplaceFeature(CaptureMotionPtr _capture_motion) override;
+        FactorBasePtr    emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
 
-    protected:
-        ParamsProcessorImuPtr params_motion_Imu_;
-};
+        virtual void     bootstrap() override;
+        CaptureBasePtr   bootstrapOrigin() const;
+        VectorXd         bootstrapDelta() const;
+        bool             recomputeStates() const;
 
+      protected:
+        ParamsProcessorImuPtr    params_motion_Imu_;
+        std::list<FactorBasePtr> list_fac_inactive_bootstrap_;
+};
 }
 
 /////////////////////////////////////////////////////////
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index f8a4b0b4118c299fda57c59e77df1b922b2e292a..14a604c9261613207b5f73c1a899bcf6f782136d 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -21,43 +21,51 @@
 //--------LICENSE_END--------
 #include "imu/capture/capture_imu.h"
 #include "imu/sensor/sensor_imu.h"
-#include "core/state_block/state_quaternion.h"
+
+#include <core/state_block/state_quaternion.h>
+#include <core/state_block/state_block_derived.h>
 
 namespace wolf {
 
-CaptureImu::CaptureImu(const TimeStamp& _init_ts,
-                       SensorBasePtr _sensor_ptr,
+CaptureImu::CaptureImu(const TimeStamp&       _init_ts,
+                       SensorBasePtr          _sensor_ptr,
                        const Eigen::Vector6d& _acc_gyro_data,
                        const Eigen::MatrixXd& _data_cov,
-                       CaptureBasePtr _capture_origin_ptr) :
-                CaptureMotion("CaptureImu",
-                              _init_ts,
-                              _sensor_ptr,
-                              _acc_gyro_data,
-                              _data_cov,
-                              _capture_origin_ptr,
-                              nullptr,
-                              nullptr,
-                              std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false))
+                       CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion(
+          "CaptureImu",
+          _init_ts,
+          _sensor_ptr,
+          _acc_gyro_data,
+          _data_cov,
+          _capture_origin_ptr,
+          nullptr,
+          nullptr,
+          (_sensor_ptr->getProblem()->getDim() == 2)
+              ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
+              : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
 {
     //
 }
 
-CaptureImu::CaptureImu(const TimeStamp& _init_ts,
-                       SensorBasePtr _sensor_ptr,
+CaptureImu::CaptureImu(const TimeStamp&       _init_ts,
+                       SensorBasePtr          _sensor_ptr,
                        const Eigen::Vector6d& _acc_gyro_data,
                        const Eigen::MatrixXd& _data_cov,
-                       const VectorXd& _bias,
-                       CaptureBasePtr _capture_origin_ptr) :
-                CaptureMotion("CaptureImu",
-                              _init_ts,
-                              _sensor_ptr,
-                              _acc_gyro_data,
-                              _data_cov,
-                              _capture_origin_ptr,
-                              nullptr,
-                              nullptr,
-                              std::make_shared<StateBlock>(_bias, false))
+                       const VectorXd&        _bias,
+                       CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion(
+          "CaptureImu",
+          _init_ts,
+          _sensor_ptr,
+          _acc_gyro_data,
+          _data_cov,
+          _capture_origin_ptr,
+          nullptr,
+          nullptr,
+          (_bias.size() == 3)
+              ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
+              : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
 {
     assert((_bias.size() == 3) or (_bias.size() == 6));
 }
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 5e429fd80355bddf587eaccafec7bf4db8476b02..b72182fcdda85f359e9da9f3a67a52b0c6ccd4f3 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -31,9 +31,10 @@ namespace wolf {
 
 ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) :
         ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu),
-        params_motion_Imu_(std::make_shared<ParamsProcessorImu>(*_params_motion_imu))
+        params_motion_Imu_(_params_motion_imu)
 {
-    //
+    bootstrapping_ = params_motion_Imu_->bootstrap_enable;
+    list_fac_inactive_bootstrap_.clear();
 }
 
 ProcessorImu::~ProcessorImu()
@@ -48,13 +49,13 @@ void ProcessorImu::preProcess()
 bool ProcessorImu::voteForKeyFrame() const
 {
     // time span
-    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
+    if (getBuffer().back().ts_ - getBuffer().front().ts_ >= params_motion_Imu_->max_time_span - Constants::EPS)
     {
         WOLF_DEBUG( "PM: vote: time span" );
         return true;
     }
     // buffer length
-    if (getBuffer().size() > params_motion_Imu_->max_buff_length)
+    if (getBuffer().size() -1 > params_motion_Imu_->max_buff_length)
     {
         WOLF_DEBUG( "PM: vote: buffer length" );
         return true;
@@ -122,6 +123,12 @@ FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, Captur
 
     auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
 
+    if (bootstrapping_) 
+    {
+        fac_imu->setStatus(FAC_INACTIVE);
+        list_fac_inactive_bootstrap_.push_back(fac_imu);
+    }
+
     return fac_imu;
 }
 
@@ -240,9 +247,195 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
     return imu::plus(delta_preint, delta_step);
 }
 
+void ProcessorImu::bootstrap()
+{
+    // TODO bootstrap strategies.
+    // See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
+    // frames:
+    //   w: world global ( where g = [0,0,-9.806] );
+    //   l: world local;
+    //   r: robot;
+    //   s: sensor (IMU)
+
+    CaptureBasePtr  first_capture = bootstrapOrigin();
+    TimeStamp       t_current     = last_ptr_->getBuffer().back().ts_;
+    VectorComposite transformation("PO");
+    switch (params_motion_Imu_->bootstrap_method)
+    {
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
+            
+            // Implementation of static strategy
+            if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+            {
+                // get initial IMU frame 's' expressed in local world frame 'l'
+                Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data());
+                Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data());
+                const auto&            q_l_s = q_l_r * q_r_s;
+
+                // Compute total integrated delta during bootstrap period
+                VectorXd delta_int = bootstrapDelta();
+
+                // compute local g and transformation to global g
+                double      dt         = t_current - first_capture->getTimeStamp();  //
+                const auto& dv         = delta_int.segment(7, 3);                    //
+                Vector3d    g_l        = -(q_l_s * dv / dt);                         // See eq. (20)
+                const auto& g_w        = gravity();                                  //
+                const auto& p_w_l      = Vector3d::Zero();                           // will pivot around the origin
+                Quaterniond q_w_l      = Quaterniond::FromTwoVectors(g_l, g_w);      //
+                transformation.at('P') = p_w_l;                                      //
+                transformation.at('O') = q_w_l.coeffs();                             //
+
+                // Transform problem to new reference
+                getProblem()->transform(transformation);
+
+                // Recompute states at keyframes if they were provided by this processor
+                bool recomputed = recomputeStates();
+                if (recomputed)
+                {
+                    WOLF_DEBUG("IMU Keyframe states have been recomputed!");
+                }
+
+                // TODO: add factors for the STATIC strategy:
+                // - zero-velocity factors (at each KF)
+                // - zero-displaecement odom3d factors (between KFs)
+
+                // Activate factors that were inactive during bootstrap
+                while (not list_fac_inactive_bootstrap_.empty())
+                {
+                    list_fac_inactive_bootstrap_.front()->setStatus(FAC_ACTIVE);
+                    list_fac_inactive_bootstrap_.pop_front();
+                }
+
+                // Clear bootstrapping flag. This marks the end of the bootstrapping process
+                bootstrapping_ = false;
+            }
+            break;
+
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G:
+            
+            // Implementation of G strategy.
+            if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+            {
+                // get initial IMU frame 's' expressed in local world frame 'l'
+                Map<const Quaterniond> q_l_r(first_capture->getFrame()->getStateVector("O").data());
+                Map<const Quaterniond> q_r_s(first_capture->getSensor()->getStateVector("O").data());
+                const auto&            q_l_s = q_l_r * q_r_s;
+
+                // Compute total integrated delta during bootstrap period
+                VectorXd delta_int = bootstrapDelta();
+
+                // compute local g and transformation to global g
+                double      dt         = t_current - first_capture->getTimeStamp();  //
+                const auto& dv_l       = delta_int.segment(7, 3);                    //
+                Vector3d    g_l        = -(q_l_s * dv_l / dt);                       // See eq. (20)
+                const auto& g_w        = gravity();                                  //
+                const auto& p_w_l      = Vector3d::Zero();                           // will pivot around the origin
+                Quaterniond q_w_l      = Quaterniond::FromTwoVectors(g_l, g_w);      //
+                transformation.at('P') = p_w_l;                                      //
+                transformation.at('O') = q_w_l.coeffs();                             //
+
+                // Transform problem to new reference
+                getProblem()->transform(transformation);
+
+                // Recompute states at keyframes if they were provided by this processor
+                bool recomputed = recomputeStates();
+                if (recomputed)
+                {
+                    WOLF_DEBUG("IMU Keyframe states have been recomputed!");
+                }
+
+                // Activate factors that were inactive during bootstrap
+                while (not list_fac_inactive_bootstrap_.empty())
+                {
+                    list_fac_inactive_bootstrap_.front()->setStatus(FAC_ACTIVE);
+                    list_fac_inactive_bootstrap_.pop_front();
+                }
+
+                // Clear bootstrapping flag. This marks the end of the bootstrapping process
+                bootstrapping_ = false;
+            }
+            break;
+
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
+        
+            // TODO implement v0-g strategy
+            WOLF_WARN("Bootstrapping strategy BOOTSTRAP_V0_G not yet implemented. Disabling bootstrap!");
+            bootstrapping_ = false;
+            break;
+
+        default:
+
+            // No strategy provided: clear bootstrapping flag and warn
+            WOLF_WARN("Bootstrapping enabled but no viable strategy detected. Disabling bootstrap!");
+            bootstrapping_ = false;
+            break;
+    }
+}
+
+void ProcessorImu::bootstrapEnable(bool _bootstrap_enable)
+{
+    params_motion_Imu_->bootstrap_enable = _bootstrap_enable;
+    bootstrapping_                       = _bootstrap_enable;
+};
+
+CaptureBasePtr ProcessorImu::bootstrapOrigin() const
+{
+    if (list_fac_inactive_bootstrap_.empty())
+        return origin_ptr_;
+    else
+        return std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
+            ->getOriginCapture();
+}
+
+VectorXd ProcessorImu::bootstrapDelta() const
+{
+    // Compute total integrated delta during bootstrap period
+    // first, integrate all deltas in previous factors
+    VectorXd delta_int = deltaZero();
+    double   dt;
+    for (const auto& fac : list_fac_inactive_bootstrap_)
+    // here, we take advantage of the list of IMU factors to recover all deltas
+    {
+        dt                = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+        const auto& delta = fac->getFeature()->getMeasurement();  // In FeatImu, delta = measurement
+        delta_int         = imu::compose(delta_int, delta, dt);
+    }
+    // now compose with delta in last_ptr_
+    dt                = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
+    const auto& delta = last_ptr_->getBuffer().back().delta_integr_;
+    delta_int         = imu::compose(delta_int, delta, dt);
 
+    return delta_int;
+}
+
+bool ProcessorImu::recomputeStates() const
+{
+    const auto& mp = getProblem()->getMotionProviderMap();
+    if (not mp.empty() and
+        mp.begin()->second == std::static_pointer_cast<const MotionProvider>(
+                                  std::static_pointer_cast<const ProcessorMotion>(shared_from_this())))
+    {
+        WOLF_DEBUG("Recomputing IMU keyframe states...");
+        for (const auto& fac : list_fac_inactive_bootstrap_)
+        {
+            const auto& ftr        = fac->getFeature();
+            const auto& cap        = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
+            const auto& frm        = cap->getFrame();
+            const auto& cap_origin = cap->getOriginCapture();
+            const auto& frm_origin = cap_origin->getFrame();
+            const auto& delta      = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
+            const auto& x_origin   = frm_origin->getState();
+            auto        dt         = cap->getTimeStamp() - cap_origin->getTimeStamp();
+            auto        x          = imu::composeOverState(x_origin, delta, dt);
+            frm->setState(x);
+        }
+        return true;
+    }
+    else
+        return false;
+}
 
-} // namespace wolf
+}  // namespace wolf
 
 // Register in the FactoryProcessor
 #include "core/processor/factory_processor.h"
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index bb6ec35e0061202819385fe04b5b2c58c54a3160..fba6cdf5e008b6dbe12dc1c7df5875ffadef59d3 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/src/sensor/sensor_imu.cpp
@@ -20,8 +20,9 @@
 //
 //--------LICENSE_END--------
 #include "imu/sensor/sensor_imu.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
+
+#include <core/state_block/state_quaternion.h>
+#include <core/state_block/state_block_derived.h>
 
 namespace wolf {
 
@@ -33,9 +34,9 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _par
 
 SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) :
         SensorBase("SensorImu",
-                   std::make_shared<StateBlock>(_extrinsics.head(3), true),
-                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
-                   std::make_shared<StateBlock>(6, false, nullptr),
+                   std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false),
+                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
+                   std::make_shared<StateParams6>(Vector6d::Zero(), false),
                    (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(),
                    false, false, true),
         a_noise(_params.a_noise),
diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index f74b3baa23383ea5265d7d288abc8d97d225a0ff..ca6130465acdac333330b3c97c656ac12a038778 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -19,8 +19,9 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include <imu/sensor/sensor_imu2d.h>
-#include <core/state_block/state_block.h>
+#include "imu/sensor/sensor_imu2d.h"
+
+#include <core/state_block/state_block_derived.h>
 #include <core/state_block/state_angle.h>
 
 namespace wolf {
@@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt
     //
 }
 
-SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) :
-        SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true),
-        a_noise(_params.a_noise),
-        w_noise(_params.w_noise),
-        ab_initial_stdev(_params.ab_initial_stdev),
-        wb_initial_stdev(_params.wb_initial_stdev),
-        ab_rate_stdev(_params.ab_rate_stdev),
-        wb_rate_stdev(_params.wb_rate_stdev),
-        orthogonal_gravity(_params.orthogonal_gravity)
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params)
+    : SensorBase("SensorImu2d",
+                 std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false),
+                 std::make_shared<StateAngle>(_extrinsics(2), true, false),
+                 std::make_shared<StateParams3>(Vector3d::Zero(3), false),
+                 (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(),
+                 false,
+                 false,
+                 true),
+      a_noise(_params.a_noise),
+      w_noise(_params.w_noise),
+      ab_initial_stdev(_params.ab_initial_stdev),
+      wb_initial_stdev(_params.wb_initial_stdev),
+      ab_rate_stdev(_params.ab_rate_stdev),
+      wb_rate_stdev(_params.wb_rate_stdev),
+      orthogonal_gravity(_params.orthogonal_gravity)
 {
     assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
-    if(!orthogonal_gravity)
+    if (!orthogonal_gravity)
     {
-        addStateBlock('G', std::make_shared<StateBlock>(2, false), false);
+        addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false);
     }
 }
 
diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
index 199f59af44bd61d5dcf60ce1a815def2e3cb950a..c5fd0333e06d6732128516081705339840100665 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -48,17 +48,41 @@ static ParamsProcessorBasePtr createProcessorImuParams(const std::string & _file
 
     if (config["type"].as<std::string>() == "ProcessorImu")
     {
-        YAML::Node kf_vote = config["keyframe_vote"];
 
         ParamsProcessorImuPtr params = std::make_shared<ParamsProcessorImu>();
         params->time_tolerance = config["time_tolerance"]           .as<double>();
         params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
 
-        params->max_time_span       = kf_vote["max_time_span"]      .as<double>();
-        params->max_buff_length     = kf_vote["max_buff_length"]    .as<SizeEigen>();
-        params->dist_traveled       = kf_vote["dist_traveled"]      .as<double>();
-        params->angle_turned        = kf_vote["angle_turned"]       .as<double>();
-        params->voting_active       = kf_vote["voting_active"]      .as<bool>();
+        YAML::Node kf_vote      = config["keyframe_vote"];
+        params->max_time_span   = kf_vote["max_time_span"].as<double>();
+        params->max_buff_length = kf_vote["max_buff_length"].as<SizeEigen>();
+        params->dist_traveled   = kf_vote["dist_traveled"].as<double>();
+        params->angle_turned    = kf_vote["angle_turned"].as<double>();
+        params->voting_active   = kf_vote["voting_active"].as<bool>();
+
+        YAML::Node bootstrap     = config["bootstrap"];
+        params->bootstrap_enable = bootstrap["enable"].as<bool>();
+        if (bootstrap["method"])
+        // if (params->bootstrap_enable)
+        {
+            string str = bootstrap["method"].as<string>();
+            std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+            if (str == "STATIC" /**/)
+            {
+                params->bootstrap_method           = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC;
+                params->bootstrap_averaging_length = bootstrap["averaging_length"].as<int>();
+            }
+            if (str == "G" /*     */)
+            {
+                params->bootstrap_method           = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G;
+                params->bootstrap_averaging_length = bootstrap["averaging_length"].as<double>();
+            }
+            if (str == "V0_G" /*  */)
+            {
+                params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G;
+            }
+        }
+
         return params;
     }
 
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 6e63ec0bba182b5b0e163ed3c7ff094896f4a296..90d12807fe10a24496ab97c4c1e7a532ea566797 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -1539,10 +1539,44 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
 
 }
 
+TEST_F(Process_Factor_Imu, bootstrap)
+{
+    processor_imu->setVotingActive(true);
+    processor_imu->setMaxTimeSpan(0.04);
+    processor_imu->bootstrapEnable(true);
+
+    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
+
+    capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());
+
+    for (t = 0; t < 0.14; t += dt)
+    {
+        capture_imu->setTimeStamp(t);
+        capture_imu->process();
+    }
+
+    problem->print(4, 0, 1, 1);
+
+    Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX()));  // turn of +90deg over X
+    for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap())
+    {
+        ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10);
+        ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10);
+        ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10);
+    }
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*";
+    // ::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";
 
diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_tools.cpp
index 4053b15f912ef7e5cb4b39181e7a5b81395d0c6a..12c6c97f966c2a78dc961d2bec6f39f4698adadf 100644
--- a/test/gtest_imu_tools.cpp
+++ b/test/gtest_imu_tools.cpp
@@ -641,6 +641,116 @@ TEST(Imu_tools, full_delta_residual)
 //    WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
 }
 
+TEST(extract_v0_g, all_zeros)
+{
+    Vector3d    p0(0, 0, 0);
+    Quaterniond q0(1, 0, 0, 0);
+    Vector3d    p1(0, 0, 0);
+    Quaterniond q1(1, 0, 0, 0);
+    Vector3d    p2(0, 0, 0);
+    double      Dt01(1), Dt12(1);
+    Vector3d    Dp01(0, 0, 0);
+    Vector3d    Dv01(0, 0, 0);
+    Vector3d    Dp12(0, 0, 0);
+    Vector3d    v0, g;
+
+    extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+    ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15);
+    ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, 0), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation)
+{
+    /** free fall v0 = 0
+     *
+     *    X p0 = 0, v0 = 0
+     *    .
+     *    X p1 = -4.9m
+     *    .
+     *    .
+     *    .
+     *    .
+     *    X p2 = -19,6m
+     */
+
+    Vector3d    p0(0, 0, 0);
+    Quaterniond q0(1, 0, 0, 0);
+    Vector3d    p1(0, 0, -9.8 / 2);
+    Quaterniond q1(1, 0, 0, 0);
+    Vector3d    p2(0, 0, -9.8 * 4 / 2);
+    double      Dt01(1), Dt12(1);
+    Vector3d    Dp01(0, 0, 0);
+    Vector3d    Dv01(0, 0, 0);
+    Vector3d    Dp12(0, 0, 0);
+    Vector3d    v0, g;
+
+    extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+    ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15);
+    ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation_hor_vel)
+{
+    /** free fall v0 = 1m/s horizontally --> parabolic fall
+     *
+     *    X p0 = (0,0)m, v0 = (1,0)m/s
+     *        .
+     *          X p1 = (1,-4.9)m
+     *            .
+     *             .
+     *              .
+     *              .
+     *              X p2 = (2,-19,6)m
+     */
+    Vector3d    p0(0, 0, 0);
+    Quaterniond q0(1, 0, 0, 0);
+    Vector3d    p1(1, 0, -9.8 / 2);
+    Quaterniond q1(1, 0, 0, 0);
+    Vector3d    p2(2, 0, -9.8 * 4 / 2);
+    double      Dt01(1), Dt12(1);
+    Vector3d    Dp01(0, 0, 0);
+    Vector3d    Dv01(0, 0, 0);
+    Vector3d    Dp12(0, 0, 0);
+    Vector3d    v0, g;
+
+    extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+    ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 0), 1e-15);
+    ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation_diag_vel)
+{
+    /** free fall v0 = 1m/s horizontally and 9.8 vert --> parabolic ballistic flight
+     * 
+     * 
+     *             X p1 = (1, +4.9)m
+     *        .         .
+     *      .             .
+     *     .               .
+     *    X p0 = (0,0)m     X p2 = (2, 0)m
+     *      v0 = (1, +9.8)m/s
+     */
+
+    Vector3d    p0(0, 0, 0);
+    Quaterniond q0(1, 0, 0, 0);
+    Vector3d    p1(1, 0, +9.8 / 2);
+    Quaterniond q1(1, 0, 0, 0);
+    Vector3d    p2(2, 0, 0);
+    double      Dt01(1), Dt12(1);
+    Vector3d    Dp01(0, 0, 0);
+    Vector3d    Dv01(0, 0, 0);
+    Vector3d    Dp12(0, 0, 0);
+    Vector3d    v0, g;
+
+    extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+    ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 9.8), 1e-15);
+    ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml
index 46393f30b8f24bf645bfb1272764e6c8915e5dc2..cc07835915541bd4243b33370f3cf45c496cda32 100644
--- a/test/yaml/imu_static_init.yaml
+++ b/test/yaml/imu_static_init.yaml
@@ -10,3 +10,8 @@ keyframe_vote:
     max_buff_length:    1000000000   # motion deltas
     dist_traveled:      100000000000   # meters
     angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+    enable: false 
+    method: "G"
+    averaging_length: 10
\ No newline at end of file