Fix m and com tests with more KFs.

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -135,6 +135,22 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
     WOLF_INFO("True mass     : ", mass_true, " Kg.");
     WOLF_INFO("Guess mass    : ", mass_guess, " Kg.");
     WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+
+
+    CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
+    {
+        C->setTimeStamp(t);
+        C->process();
+        p->getOrigin()->getFrame()->fix();
+        report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+        WOLF_INFO("-----------------------------");
+    }
+
+
+
+    ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
 }
 
 TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
@@ -175,6 +191,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
     CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
 
     C_KF0->getFrame()->fix();
+    C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
     C_KF1->getFrame()->fix();
 
     P->print(4, 1, 1, 1);
@@ -188,6 +205,23 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
     WOLF_INFO("True cdm     : ", cdm_true.transpose(), " m.");
     WOLF_INFO("Guess cdm    : ", cdm_guess.transpose(), " m.");
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+
+
+
+    CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
+    {
+        C->setTimeStamp(t);
+        C->process();
+        p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+        p->getOrigin()->getFrame()->fix();
+        report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+        WOLF_INFO("-----------------------------");
+    }
+
+
+    ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
 }
 
 TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
@@ -229,7 +263,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
 
     C_KF0->getFrame()->fix();
-    C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero());
+    C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
     C_KF1->getFrame()->fix();
 
     P->print(4, 1, 1, 1);
@@ -239,10 +273,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
 
     WOLF_INFO("True mass     : ", mass_true, " Kg.");
-    WOLF_INFO("Guess mass    : ", mass_guess, " Kg.");
-    WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
     WOLF_INFO("True cdm      : ", cdm_true.transpose(), " m.");
+    WOLF_INFO("Guess mass    : ", mass_guess, " Kg.");
     WOLF_INFO("Guess cdm     : ", cdm_guess.transpose(), " m.");
+    WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
     WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
@@ -258,7 +292,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     {
         C->setTimeStamp(t);
         C->process();
-        p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero());
+        p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
         p->getOrigin()->getFrame()->fix();
         report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
         WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
@@ -277,6 +311,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.mass_and_cdm_hovering";
+    // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
     return RUN_ALL_TESTS();
 }