diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 6aafb68474cf2c95f12544b18f50bc0e809b284d..7437f2df480b4834adc33c6942db41e5e5d54ddc 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -113,7 +113,8 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f1)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"),  x1.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -153,7 +154,8 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f0)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm0->getStateVector("V"),  x0.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -244,7 +246,7 @@ TEST_F(FactorImu2d_test, solve_b0)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+        ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -292,7 +294,8 @@ TEST_F(FactorImu2d_test, solve_f0)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm0->getStateVector("V"),  x0.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -340,7 +343,8 @@ TEST_F(FactorImu2d_test, solve_f1)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"),  x1.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 51ee79d6ee34e460d02c6022a016828d1bdfd627..97eb0687a1b946a74876a285b4bb63425c582068 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -125,7 +125,8 @@ TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f1)
         std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
         // std::cout << report << std::endl;
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"),  x1.tail(2), 1e-6);
         // WOLF_INFO(frm1->getStateVector());
 
         // remove feature (and factor) for the next loop
@@ -172,7 +173,8 @@ TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f0)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm0->getStateVector("V"),  x0.tail(2), 1e-6);
         // WOLF_INFO(frm1->getStateVector());
 
         // remove feature (and factor) for the next loop
@@ -331,7 +333,8 @@ TEST_F(FactorImu2dWithGravity_test, solve_f0)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm0->getStateVector("V"),  x0.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -385,7 +388,8 @@ TEST_F(FactorImu2dWithGravity_test, solve_f1)
         // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"),  x1.tail(2), 1e-6);
 
         // remove feature (and factor) for the next loop
         fea1->remove();