diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index d979c00807b07ab9fa29212a77791ae847f506ea..6e46c370d6804d350580d24c982f8e5eace448e5 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -135,8 +135,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("-----------------------------");
-
- for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 7.0; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
@@ -148,7 +147,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
P->print(4, 1, 1, 1);
-
ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
}
@@ -173,12 +171,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
S->getStateBlock('m')->fix();
// 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);
+ 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());
@@ -191,7 +189,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
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()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -205,19 +203,20 @@ 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 <= 22.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);
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
- p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ 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
+ 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_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
@@ -260,7 +259,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
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()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -277,19 +276,19 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
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
+ // 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 <= 50.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 50.0; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
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()->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.");
@@ -300,9 +299,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
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
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
}
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
@@ -326,12 +324,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
S->getStateBlock('m')->fix();
// 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);
+ 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());
@@ -359,20 +357,22 @@ 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 <= 30.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);
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
p->getOrigin()->getFrame()->fix();
p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
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("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
WOLF_INFO("-----------------------------");
}
-
- ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
+ 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_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_mom_hovering)
@@ -433,14 +433,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_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
+ // 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 <= 26.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);
@@ -450,7 +449,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
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("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
WOLF_INFO("-----------------------------");
}
@@ -458,12 +459,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
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
+ 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
+// 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));
@@ -524,25 +524,26 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
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
+ // 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)
+ 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
+ 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("Estimated ang mom : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " m^2 kg/s.");
WOLF_INFO("-----------------------------");
}
@@ -550,15 +551,87 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
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
+ 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, rotation_around_z_axis_test)
+{
+ S->getStateBlock('i')->setState(Vector3d(0.01, 0.01, 0.01));
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ Vector3d initial_ang_vel(0, 0, 0);
+ data.segment<3>(3) = initial_ang_vel;
+ data.segment<3>(6) = -mass_true * gravity();
+ Vector3d initial_torque(0, 0, 1);
+ data.segment<3>(9) = initial_torque;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->fix();
+ // S->setStateBlockDynamic('I');
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->getStateBlock('P')->fix();
+ C_KF0->getFrame()->getStateBlock('O')->fix();
+
+ Vector3d dL = data.segment<3>(9);
+ Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState();
+ ang_mom = ang_mom + dL;
+
+ 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 inertia : ", inertia_true, " m^2 kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess, " m^2 kg.");
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 kg.");
+ WOLF_INFO("-----------------------------");
+
+ Vector3d ang_vel;
+
+ for (TimeStamp t = 0.0; t <= 50.0; t += 1.0)
+ {
+ // w = i_inv*L we actualize the data //Això segur que es pot millorar
+ ang_vel(0) = ang_mom(0) /
+ (p->getSensor()->getStateBlock('i')->getState()(0));
+ ang_vel(1) = ang_mom(1) /
+ (p->getSensor()->getStateBlock('i')->getState()(1));
+ ang_vel(2) = ang_mom(2) /
+ (p->getSensor()->getStateBlock('i')->getState()(2));
+ data.segment<3>(3) = ang_vel;
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+ // increment de L
+ //dL és tota l'estona la mateixa (?) diria que si dL = torque*dt
+ ang_mom = ang_mom + dL;
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated inertia: ", S->getStateBlock('m')->getState(), " m^2 Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
+}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.hovering_test_without_fixing";
+ ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.rotation_around_z_axis_test";
return RUN_ALL_TESTS();
}