diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 4146484e33e7fb7295c97bda1de29ae5e68da6ac..0bf9d5cacaeeaa946e6ca9a849d2ea7be50650ef 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -91,345 +91,56 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
 }
 }
 
-//TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
-//{
-//  for(int i = 0; i < 50; i++){
-//    // Random delta
-//    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
-//    delta(2) = pi2pi(delta(2));
-//
-//    // Random initial pose
-//    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
-//    x0(2) = pi2pi(x0(2));
-//
-//    // x1 groundtruth
-//    Vector5d x1;
-//    x1 = imu2d::composeOverState(x0, delta, 1);
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm0 and biases, perturb frm1
-//    frm0->unfix();
-//    cap0->fix();
-//    frm1->fix();
-//    cap1->fix();
-//    frm0->perturb(0.01);
-//
-//    // solve 
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
-//    //WOLF_INFO(frm1->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//}
-//}
-//
-//TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
-//{
-//  for(int i = 0; i < 50; i++){
-//    // Random delta
-//    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
-//    delta(2) = pi2pi(delta(2));
-//
-//    // Random initial pose
-//    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
-//    x0(2) = pi2pi(x0(2));
-//
-//    // x1 groundtruth
-//    Vector5d x1;
-//    x1 = imu2d::composeOverState(x0, delta, 1);
-//    
-//    //0 Initial bias
-//    Vector3d b0 = Vector3d::Zero();
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
-//    frm0->fix();
-//    cap0->fix();
-//    frm1->fix();
-//    cap1->unfix();
-//    cap1->perturb(0.01);
-//
-//    // solve  to estimate bias at cap1
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
-//    //WOLF_INFO(cap1->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//}
-//}
-//
-//TEST(FactorImu2dWithGravity, 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::composeOverState(x0, delta, 1);
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm1, perturb frm0
-//    frm0->fix();
-//    cap0->unfix();
-//    frm1->fix();
-//    cap1->fix();
-//    cap0->perturb(0.1);
-//
-//    // 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(FactorImu2dWithGravity, solve_b1)
-//{
-//  for(int i = 0; i < 50; i++){
-//    // Random delta
-//    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
-//    delta(2) = pi2pi(delta(2));
-//
-//    // Random initial pose
-//    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
-//    x0(2) = pi2pi(x0(2));
-//
-//    // x1 groundtruth
-//    Vector5d x1;
-//    x1 = imu2d::composeOverState(x0, delta, 1);
-//    
-//    //0 Initial bias
-//    Vector3d b0 = Vector3d::Random();
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm0 and frm1, unfix bias, perturb cap1
-//    frm0->fix();
-//    cap0->fix();
-//    frm1->fix();
-//    cap1->unfix();
-//    cap1->perturb(0.01);
-//
-//    // solve  to estimate bias at cap1
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
-//    //WOLF_INFO(cap1->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//}
-//}
-//
-//TEST(FactorImu2dWithGravity, solve_f0)
-//{
-//  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::composeOverState(x0, delta, 1);
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm1, perturb frm0
-//    frm0->unfix();
-//    cap0->fix();
-//    frm1->fix();
-//    cap1->fix();
-//    frm0->perturb(0.1);
-//
-//    // solve 
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
-//    //WOLF_INFO(frm0->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//}
-//}
-//
-//TEST(FactorImu2dWithGravity, solve_f1)
-//{
-//  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::composeOverState(x0, delta, 1);
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm1, perturb frm0
-//    frm0->fix();
-//    cap0->fix();
-//    frm1->unfix();
-//    cap1->fix();
-//    frm1->perturb(0.1);
-//
-//    // solve 
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
-//    //WOLF_INFO(frm0->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//}
-//}
-//
-//TEST(FactorImu2dWithGravity, solve_f1_b1)
-//{
-//  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::composeOverState(x0, delta, 1);
-//
-//    // Set states
-//    frm0->setState(x0);
-//    frm1->setState(x1);
-//    cap0->getStateBlock('I')->setState(b0);
-//    cap1->getStateBlock('I')->setState(b0);
-//
-//    // feature & factor with delta measurement
-//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-//    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-//
-//    // Fix frm1, perturb frm0
-//    frm0->fix();
-//    cap0->fix();
-//    frm1->unfix();
-//    cap1->unfix();
-//    frm1->perturb(0.1);
-//    cap1->perturb(0.1);
-//
-//    // solve 
-//     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-//    ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
-//    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
-//    //WOLF_INFO(frm0->getStateVector());
-//
-//    // remove feature (and factor) for the next loop
-//    fea1->remove();
-//  }
-//}
+TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
+{
+  for(int i = 0; i < 50; i++){
+    // Random delta
+    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+    delta(2) = pi2pi(delta(2));
 
-TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+    // Random initial pose
+    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+    x0(2) = pi2pi(x0(2));
+
+    //Random gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    // x1 groundtruth
+    Vector5d x1;
+    x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm1 and biases, perturb frm0
+    frm0->unfix();
+    cap0->fix();
+    frm1->fix();
+    cap1->fix();
+    sensor->getStateBlock('G')->fix();
+    frm0->perturb(0.01);
+
+    // solve 
+     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+    //WOLF_INFO(frm1->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
 {
   for(int i = 0; i < 50; i++){
-    //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
     // Random delta
     Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
     delta(2) = pi2pi(delta(2));
@@ -445,17 +156,353 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_G)
     // x1 groundtruth
     Vector5d x1;
     x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+    
+    //0 Initial bias
+    Vector3d b0 = Vector3d::Zero();
 
     // Set states
     frm0->setState(x0);
     frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
     sensor->getStateBlock('G')->setState(g);
 
     // feature & factor with delta measurement
     auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
     FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-    // Fix frm0 and biases, perturb frm1
+    // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+    frm0->fix();
+    cap0->fix();
+    frm1->fix();
+    sensor->getStateBlock('G')->fix();
+    cap1->unfix();
+    cap1->perturb(0.01);
+
+    // solve  to estimate bias at cap1
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+    //WOLF_INFO(cap1->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+  }
+}
+
+TEST(FactorImu2dWithGravity, 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 gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    //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::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm1, perturb frm0
+    frm0->fix();
+    cap0->unfix();
+    frm1->fix();
+    cap1->fix();
+    sensor->getStateBlock('G')->fix();
+    cap0->perturb(0.1);
+
+    // solve 
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
+    //WOLF_INFO(cap0->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+  }
+}
+
+TEST(FactorImu2dWithGravity, solve_b1)
+{
+  for(int i = 0; i < 50; i++){
+    // Random delta
+    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+    delta(2) = pi2pi(delta(2));
+
+    // Random initial pose
+    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+    x0(2) = pi2pi(x0(2));
+
+    //Random gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    // x1 groundtruth
+    Vector5d x1;
+    x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+    
+    //0 Initial bias
+    Vector3d b0 = Vector3d::Random();
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm0 and frm1, unfix bias, perturb cap1
+    frm0->fix();
+    cap0->fix();
+    frm1->fix();
+    sensor->getStateBlock('G')->fix();
+    cap1->unfix();
+    cap1->perturb(0.01);
+
+    // solve  to estimate bias at cap1
+     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+    //WOLF_INFO(cap1->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, solve_f0)
+{
+  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 gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    //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::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm1, perturb frm0
+    frm0->unfix();
+    cap0->fix();
+    frm1->fix();
+    cap1->fix();
+    sensor->getStateBlock('G')->fix();
+    frm0->perturb(0.1);
+
+    // solve 
+     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+    //WOLF_INFO(frm0->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, solve_f1)
+{
+  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 gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    //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::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm1, perturb frm0
+    frm0->fix();
+    cap0->fix();
+    frm1->unfix();
+    cap1->fix();
+    sensor->getStateBlock('G')->fix();
+    frm1->perturb(0.1);
+
+    // solve 
+     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+    //WOLF_INFO(frm0->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+  }
+}
+
+TEST(FactorImu2dWithGravity, solve_f1_b1)
+{
+  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 gravity
+    Vector2d g = Vector2d::Random()*5;
+    //Vector2d g = Vector2d::Zero();
+
+    //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::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frm1, perturb frm0
+    frm0->fix();
+    cap0->fix();
+    sensor->getStateBlock('G')->fix();
+    frm1->unfix();
+    cap1->unfix();
+    frm1->perturb(0.1);
+    cap1->perturb(0.1);
+
+    // solve 
+     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+    ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+    //WOLF_INFO(frm0->getStateVector());
+
+    // remove feature (and factor) for the next loop
+    fea1->remove();
+  }
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+{
+  for(int i = 0; i < 50; i++){
+    //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+    // Random delta
+    Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+    delta(2) = pi2pi(delta(2));
+
+    // Random initial pose
+    Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+    x0(2) = pi2pi(x0(2));
+
+    //Random gravity
+    //Vector2d g = Vector2d::Random()*5;
+    Vector2d g = Vector2d::Zero();
+
+    //Zero bias
+    Vector3d b0 = Vector3d::Zero();
+
+    // x1 groundtruth
+    Vector5d x1;
+    x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+    // Set states
+    frm0->setState(x0);
+    frm1->setState(x1);
+    cap0->getStateBlock('I')->setState(b0);
+    cap1->getStateBlock('I')->setState(b0);
+    sensor->getStateBlock('G')->setState(g);
+
+    // feature & factor with delta measurement
+    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+    FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+    // Fix frames and biases, perturb g
     frm0->fix();
     cap0->fix();
     frm1->fix();
@@ -465,14 +512,13 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_G)
 
     // solve 
     std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-    //std::cout << report << std::endl;
 
     ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
     //WOLF_INFO(frm1->getStateVector());
 
     // remove feature (and factor) for the next loop
     fea1->remove();
-}
+  }
 }
 TEST(FactorImu2dWithGravity, solve_G)
 {
@@ -485,13 +531,13 @@ TEST(FactorImu2dWithGravity, solve_G)
     Vector5d x0 = Vector5d::Random() * 10;
     x0(2) = pi2pi(x0(2));
 
-    //Random Initial bias
-    Vector3d b0 = Vector3d::Random();
-
     //Random gravity
     Vector2d g = Vector2d::Random()*5;
     //Vector2d g = Vector2d::Zero();
 
+    //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);
@@ -511,7 +557,7 @@ TEST(FactorImu2dWithGravity, solve_G)
     auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
     FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-    // Fix frm1, perturb frm0
+    // Fix frames and captures, perturb g
     frm0->fix();
     cap0->fix();
     frm1->fix();
@@ -520,14 +566,14 @@ TEST(FactorImu2dWithGravity, solve_G)
     sensor->getStateBlock('G')->perturb(1);
 
     // solve 
-     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
     //WOLF_INFO(cap0->getStateVector());
 
     // remove feature (and factor) for the next loop
     fea1->remove();
-}
+  }
 }
 
 int main(int argc, char **argv)