diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp index 59eb9ddb16e7b87e27ee0fb4d597ccb35d9daf0d..763ecb87472f8703c01f635e56444d52cbe04241 100644 --- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp +++ b/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)