Add (loose) asserts to pass tests

test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp

@@ -453,6 +453,11 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
     WOLF_INFO("Guess mass              : ", mass_guess, " Kg.");
     WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
     WOLF_INFO("-----------------------------");
+
+    ASSERT_MATRIX_APPROX(bias_true   , S->getStateBlock('I')->getState(), 0.11);
+    ASSERT_MATRIX_APPROX(cdm_true    , S->getStateBlock('C')->getState(), 1e-3);
+    ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+    ASSERT_NEAR         (mass_true   , S->getStateBlock('m')->getState()(0), 2e-2);
 }
 
 TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
@@ -556,6 +561,11 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
     WOLF_INFO("Guess mass              : ", mass_guess, " Kg.");
     WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
     WOLF_INFO("-----------------------------");
+
+    ASSERT_MATRIX_APPROX(bias_true   , S->getStateBlock('I')->getState(), 0.21);
+    ASSERT_MATRIX_APPROX(cdm_true    , S->getStateBlock('C')->getState(), 2e-3);
+    ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
+    ASSERT_NEAR         (mass_true   , S->getStateBlock('m')->getState()(0), 3e-2);
 }
 
 int main(int argc, char** argv)