add asserts and pass

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -764,33 +764,38 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
 
             report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
 
-            WOLF_INFO("Total angle turned: ", angle_true.transpose(), " rad.");
-            WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+            WOLF_INFO("Total angle turned      : ", angle_true.transpose(), " rad.");
+            WOLF_INFO("Estimated IMU bias      : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
             WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
-            WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
-            WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+            WOLF_INFO("Estimated inertia       : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+            WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
             WOLF_INFO("-----------------------------");
         }
     }
 
-    WOLF_INFO("True IMU bias     : ", bias_true.transpose(), " m/s2 | rad/s.");
-    WOLF_INFO("Guess IMU bias    : ", bias_guess.transpose(), " m/s2 | rad/s.");
-    WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+    WOLF_INFO("True IMU bias           : ", bias_true.transpose(), " m/s2 | rad/s.");
+    WOLF_INFO("Guess IMU bias          : ", bias_guess.transpose(), " m/s2 | rad/s.");
+    WOLF_INFO("Estimated IMU bias      : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
     WOLF_INFO("True center of mass     : ", cdm_true.transpose(), " m.");
     WOLF_INFO("Guess center of mass    : ", cdm_guess.transpose(), " m.");
     WOLF_INFO("Estimated cneter of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
-    WOLF_INFO("True inertia     : ", inertia_true.transpose(), " m^2 Kg.");
-    WOLF_INFO("Guess inertia    : ", inertia_guess.transpose(), " m^2 Kg.");
-    WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
-    WOLF_INFO("True mass     : ", mass_true, " Kg.");
-    WOLF_INFO("Guess mass    : ", mass_guess, " Kg.");
-    WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+    WOLF_INFO("True inertia            : ", inertia_true.transpose(), " m^2 Kg.");
+    WOLF_INFO("Guess inertia           : ", inertia_guess.transpose(), " m^2 Kg.");
+    WOLF_INFO("Estimated inertia       : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+    WOLF_INFO("True mass               : ", mass_true, " Kg.");
+    WOLF_INFO("Guess mass              : ", mass_guess, " Kg.");
+    WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
     WOLF_INFO("-----------------------------");
 
-    // P->print(2,1,1,1);
-
+    auto bias_estimated    = S->getStateBlock('I')->getState();
+    auto cdm_estimated     = S->getStateBlock('C')->getState();
     auto inertia_estimated = S->getStateBlock('i')->getState();
-    ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
+    auto mass_estimated    = S->getStateBlock('m')->getState()(0);
+
+    ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+    ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6);  // cdm_z is not observable while hovering
+    ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);             // Ix and Iy not observable in this test
+    ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
 }
 
 int main(int argc, char **argv)