From 192e99b68eee6ea81ef8d9c2f8e59494d6d89dff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 6 Aug 2022 21:56:38 +0200
Subject: [PATCH] Run solver for more KFs to allow convergence
---
...problem_force_torque_inertial_dynamics.cpp | 42 +++++++++++++++----
1 file changed, 34 insertions(+), 8 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 783bd2e..c8b2497 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/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
--
GitLab