From baa6b127caf97294d87275f613b43126df45f9d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 20 Aug 2022 00:45:28 +0200
Subject: [PATCH] add asserts and pass
---
...problem_force_torque_inertial_dynamics.cpp | 37 +++++++++++--------
1 file changed, 21 insertions(+), 16 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 5e0d5c7..0cae99e 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -764,33 +764,38 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO("Total angle turned: ", angle_true.transpose(), " rad.");
- WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
- WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
- WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
}
}
- WOLF_INFO("True IMU bias : ", bias_true.transpose(), " m/s2 | rad/s.");
- WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
- WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("True IMU bias : ", bias_true.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
+ WOLF_INFO("Estimated IMU bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated cneter of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
- WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
- WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
- WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
- WOLF_INFO("True mass : ", mass_true, " Kg.");
- WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
- WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
- // P->print(2,1,1,1);
-
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
auto inertia_estimated = S->getStateBlock('i')->getState();
- ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ ASSERT_MATRIX_APPROX(bias_estimated, bias_true, 1e-4);
+ ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+ ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6); // Ix and Iy not observable in this test
+ ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
}
int main(int argc, char **argv)
--
GitLab