diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 9c4a5981605da4b2f6cb769f819cea4d4d284acd..d979c00807b07ab9fa29212a77791ae847f506ea 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -205,7 +205,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 22.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
C->process();
@@ -359,7 +359,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 30.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
C->process();
@@ -440,7 +440,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
// 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.
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 26.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
@@ -463,9 +463,102 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
}
+//Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.05, 0.05, 0.05));
+ S->getStateBlock('m')->setState(Vector1d(1.50));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm and the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->unfix();
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+ C_KF1->getFrame()->unfix();
+ C_KF1->getFrame()->getStateBlock('P')->fix();
+ C_KF1->getFrame()->getStateBlock('O')->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+
+ WOLF_INFO("True mass : ", mass_true, " 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("-----------------------------");
+
+
+ // 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.
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 30.0 ; t += 1.0)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process(); //En aquesta lÃnia el programa s'aborta
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->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("Estimated ang mom : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), " m^2 kg/s.");
+
+ 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 while hovering
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.mass_and_cdm_and_ang_mom_hovering";
+ //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.hovering_test_without_fixing";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index ca0697e4c1714cd93ec5cdb41b672598dc8916f8..8eb8204126f94bc008bcb01587ae2fa5482fec6b 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -18,7 +18,7 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
plugin: "core"
- n_frames: 10
+ n_frames: 3
n_fix_first_frames: 1
viral_remove_empty_parent: true
map: