Run solver for more KFs to allow convergence

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -192,8 +192,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
 
 TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
 {
-    S->getStateBlock('C')->setState(Vector3d(0.0, 0.0, 0.02));
-    S->getStateBlock('m')->setState(Vector1d(1.91));
+    S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
+    S->getStateBlock('m')->setState(Vector1d(1.900));
     Vector3d cdm_guess  = S->getStateBlock('C')->getState();
     double   mass_guess = S->getStateBlock('m')->getState()(0);
 
@@ -238,19 +238,45 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
 
-    P->print(4, 1, 1, 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 cdm    : ", cdm_guess.transpose(), " m.");
-    WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+    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.");
+    WOLF_INFO("-----------------------------");
+
+
+    // Do a number of iterations with more keyframes, solving so that the calibration gets better and better
+    // Here we take advantage of the sliding window, thereby getting rid progressively 
+    // of the old factors, which contained calibration parameters far from the converged values,
+    // which if not eliminated contaminate the overall solution.
+    // This is due to these first factors relying on the linearization Jacobian to correct the
+    // poorly preintegrated delta.
+    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()->getStateBlock('L')->setState(Vector3d::Zero());
+        p->getOrigin()->getFrame()->fix();
+        report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+        WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+        WOLF_INFO("-----------------------------");
+    }
+
+    auto cdm_estimated  = S->getStateBlock('C')->getState();
+    auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+
+    ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
+    ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable
 }
 
 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.mass_and_cdm_hovering";
     return RUN_ALL_TESTS();
 }
\ No newline at end of file