added TODOs to try and fix the problem

test/gtest_imu_static_init.cpp

@@ -81,6 +81,7 @@ TEST_F(ProcessorImuStaticInitTest, static)
    data_cov *= 1e-3;
    data << 1, 2, 3, 4, 5, 6;
    Vector6d bias_groundtruth;
+   //TODO: Fix this, it works because initial position and extrinsics are 0, but gravity has to be preceded by the quaternion that takes it to the IMU reference
    bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
    bias_groundtruth.tail(3) = data.tail(3);
 
@@ -123,6 +124,8 @@ TEST_F(ProcessorImuStaticInitTest, static)
              n_frames++;
              last_frame_ = processor_motion_->getOrigin()->getFrame();
 
+             // TODO: We have to impose that the position is the same as for the first frame and fix it, and do the same for the orientation
+
              // impose zero velocity
              last_frame_->getV()->setZero();
              last_frame_->getV()->fix();