diff --git a/demos/imu2d_static_init.yaml b/demos/imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c45b9666696c20010a8c967ac6608c1ee3f7a98d
--- /dev/null
+++ b/demos/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.00001
+
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 1.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)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d16b4a3857c88316a48fa5cc79f77a6bc4f7b40d..70ad9b2240b17448306eb5579c9313d212301a70 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,6 +37,9 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
target_link_libraries(gtest_factor_imu2d ${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_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9283b56e3160fd128dbf2a2e5f12f4b2f00c224f
--- /dev/null
+++ b/test/gtest_imu2d_static_init.cpp
@@ -0,0 +1,277 @@
+#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 mti_clock, tmp;
+ double dt;
+ bool second_frame;
+ Vector6d data;
+ Matrix6d data_cov;
+ VectorComposite x0c; // initial state composite
+ VectorComposite s0c; // initial sigmas composite
+ std::shared_ptr<wolf::CaptureImu> C;
+ FrameBasePtr last_frame_;
+
+ //a new of this will be created for each new test
+ void SetUp() override
+ {
+ using namespace Eigen;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using namespace wolf::Constants;
+
+ std::string wolf_root = _WOLF_IMU_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 + "/demos/sensor_imu2d.yaml");
+ ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu2d_static_init.yaml");
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+ // Time and data variables
+ second_frame = false;
+ dt = 0.01;
+ t0.set(0);
+ t = t0;
+ data = Vector6d::Random();
+ data_cov = Matrix6d::Identity();
+ last_frame_ = nullptr;
+
+ // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+ C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+ }
+
+};
+
+//// Input odometry data and covariance
+//Matrix6d data_cov = 0.1*Matrix6d::Identity();
+//Matrix5d delta_cov = 0.1*Matrix5d::Identity();
+//
+//// Jacobian of the bias
+//MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
+// 0, 1, 0,
+// 0, 0, 1,
+// 1, 0, 0,
+// 0, 1, 0 ).finished());
+////preintegration bias
+//Vector3d b0_preint = Vector3d::Zero();
+//
+//// Problem and solver
+//ProblemPtr _problem_ptr_ptr = Problem::create("POV", 2);
+//SolverCeres solver(problem_ptr);
+//
+//// Two frames
+//FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+//FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+//
+//// Imu2d sensor
+//SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
+//
+////capture from frm1 to frm0
+//auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+//auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+//
+//TEST(imu2d_static_init, check_tree)
+//{
+// ASSERT_TRUE(problem_ptr->check(0));
+//}
+//
+//
+//TEST(imu2d_static_init, solve_b0)
+//{
+// for(int i = 0; i < 50; i++){
+// // Random delta
+// Vector5d delta_biased = Vector5d::Random() * 10;
+// delta_biased(2) = pi2pi(delta_biased(2));
+//
+// // Random initial pose
+// Vector5d x0 = Vector5d::Random() * 10;
+// x0(2) = pi2pi(x0(2));
+//
+// //Random Initial bias
+// Vector3d b0 = Vector3d::Random();
+//
+// //Corrected delta
+// Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+// Vector5d delta = imu2d::plus(delta_biased, delta_step);
+//
+// // x1 groundtruth
+// Vector5d x1;
+// x1 = imu2d::compose(x0, delta, 1);
+//
+// // Set states
+// frm0->setState(x0);
+// frm1->setState(x1);
+// frm0->getV()->setZero();
+// frm0->getV()->fix();
+// frm1->getV()->setZero();
+// frm1->getV()->fix();
+// cap0->getStateBlock('I')->setState(b0);
+// cap1->getStateBlock('I')->setState(b0);
+// //WOLF_INFO("states set");
+//
+// // feature & factor with delta measurement
+// auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+// FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+//
+// // Fix frm1, perturb frm0
+// frm0->fix();
+// cap0->unfix();
+// frm1->fix();
+// cap1->fix();
+// cap0->perturb(0.1);
+//
+// //do static init stuff on frm1
+// Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(3);
+// //WOLF_INFO("zero_motion created");
+// for (auto frm_pair = problem_ptr->getTrajectory()->begin();
+// frm_pair != problem_ptr->getTrajectory()->end();
+// frm_pair++)
+// {
+// //WOLF_INFO("entered for");
+// if (frm_pair->second == frm1)
+// break;
+// auto capture_zero = CaptureBase::emplace<CaptureVoid>(frm1, frm1->getTimeStamp(), nullptr);
+// auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+// "FeatureZeroOdom",
+// zero_motion,
+// Eigen::MatrixXd::Identity(3,3) * 0.01);
+// FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+// feature_zero,
+// frm_pair->second,
+// nullptr,
+// false,
+// TOP_MOTION);
+//
+// }
+//
+// // solve
+// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+//
+// ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
+// //WOLF_INFO(cap0->getStateVector());
+//
+// // remove feature (and factor) for the next loop
+// fea1->remove();
+// }
+//}
+
+TEST_F(ProcessorImu2dStaticInitTest, static)
+{
+ Vector2d pos;
+ // Set the origin
+ x0c['P'] = Vector2d(0, 0);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(0, 0);
+
+ data_cov *= 1e-3;
+ dt = 0.5;
+ auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
+ processor_motion->setOrigin(KF0);
+
+ data = Vector6d::Random();
+ WOLF_INFO("Data is: ", data);
+ int size = 3;
+ second_frame = false;
+ FrameBasePtr first_frame = nullptr;
+
+
+ for(t = t0+dt; t <= t0 + 4 + dt/2; t+=dt){
+ if(first_frame) WOLF_INFO("The first frame is ", first_frame, " and it's currently estimated bias is: ", first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
+ WOLF_INFO("Last frame is: ", last_frame_);
+ WOLF_INFO("Current state is: ", processor_motion->getState().vector("POV"));
+ WOLF_INFO("second_frame is: ", second_frame);
+ WOLF_INFO("Currently estimated bias is: ", C->getStateBlock('I')->getState());
+ WOLF_INFO("The current problem is:");
+ _problem_ptr->print(4);
+ C->setTimeStamp(t);
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ if(second_frame){
+ //ASSERT_MATRIX_APPROX(processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
+ }
+ if (not last_frame_)
+ {
+ last_frame_ = processor_motion->getOrigin()->getFrame();
+ first_frame = last_frame_;
+
+ // impose zero velocity
+ last_frame_->getV()->setZero();
+ last_frame_->getV()->fix();
+
+ // impose zero odometry
+ //processor_motion_->setOdometry(sensor_ptr->getProblem()->stateZero(processor_motion_->getStateStructure()));
+ }
+ else
+ {
+ assert(processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
+ assert(processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
+
+ // new frame
+ if (last_frame_ != processor_motion->getOrigin()->getFrame())
+ {
+ second_frame = true;
+ last_frame_ = processor_motion->getOrigin()->getFrame();
+
+ // impose zero velocity
+ last_frame_->getV()->setZero();
+ last_frame_->getV()->fix();
+
+ // 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());
+ Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
+ 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",
+ zero_motion,
+ Eigen::MatrixXd::Identity(size,size) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
+ }
+ }
+ }
+ }
+}
+
+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();
+}