diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index a0d944df42c4f67641a8b15599dcb3de9c4da59b..711906457a384d0548abb3a722108a841c0557c6 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -53,6 +53,7 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
SolverCeresPtr solver;
+ Vector6d bias_true;
Vector3d cdm_true;
Vector3d inertia_true;
double mass_true;
@@ -75,6 +76,7 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
+ bias_true << 0,0,0, 0,0,0;
cdm_true = {0, 0, 0.0341};
inertia_true = {0.017598, 0.017957, 0.029599};
mass_true = 1.952;
@@ -599,7 +601,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
Vector3d ang_vel_true(0, 0, 0);
Vector3d ang_acc_true;
Vector3d angle_true;
- Quaterniond q_true(1, 0, 0, 0);
+ Quaterniond quat_true(1, 0, 0, 0);
double dt = 1; // time increment
Vector3d position_data(0, 0, 0);
Quaterniond q_data;
@@ -624,7 +626,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
ang_acc_true(2) = torque_true(2) / inertia_true(2);
angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
- q_true = q_true * exp_q(angle_true);
+ quat_true = quat_true * exp_q(angle_true);
// ESTIMATOR
@@ -635,7 +637,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
p->getOrigin()->getFrame()->unfix();
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
- q_data = q_true;
+ q_data = quat_true;
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
@@ -655,7 +657,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing)
{
-
+ Vector6d bias_guess; bias_guess << 0,0,0, 0,0,0;
Vector3d cdm_guess (0.005, 0.005, 0.1);
S->getStateBlock('C')->setState(cdm_guess);
Vector3d inertia_guess (0.01, 0.01, 0.02);
@@ -666,14 +668,18 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
Vector3d acc_true = -gravity();
Vector3d ang_vel_true (0, 0, 0);
Vector3d force_true = -mass_true * gravity();
- Vector3d torque_true (0, 0, 0.01);
+ Vector3d torque_true (0, 0, 0.001);
Vector3d L_true (0, 0, 0);
Vector3d position_true (0, 0, 0);
- Quaterniond q_true (1, 0, 0, 0);
+ Quaterniond quat_true (1, 0, 0, 0);
Vector3d ang_acc_true;
- Vector3d angle_true;
+ ang_acc_true(0) = torque_true(0) / inertia_true(0);
+ ang_acc_true(1) = torque_true(1) / inertia_true(1);
+ ang_acc_true(2) = torque_true(2) / inertia_true(2);
+ Vector3d angle_true(0,0,0);
+
Vector3d position_data = position_true;
- Quaterniond q_data = q_true;
+ Vector4d orient_data = quat_true.coeffs();
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -681,7 +687,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
data.segment<3>(3) = ang_vel_true;
data.segment<3>(6) = -mass_true * gravity();
data.segment<3>(9) = torque_true;
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ MatrixXd data_cov = 1 * MatrixXd::Identity(12, 12);
S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix();
@@ -689,7 +695,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->unfix();
S->getStateBlock('m')->unfix();
- // S->setStateBlockDynamic('I');
+ S->setStateBlockDynamic('I', false);
CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
C0_0->process();
@@ -699,62 +705,76 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
C_KF0->getFrame()->getStateBlock('P')->fix();
C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
C_KF0->getFrame()->getStateBlock('O')->fix();
- C_KF0->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
+ C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
// initialiation of all the variables needed in the following iteration process
Vector3d dL;
- double dt = 0.25; // time increment
+ double dt = 0.5; // time increment
std::string report;
+ unsigned int num_kfs = P->getTrajectory()->getFrameMap().size();
- for (TimeStamp t = 1.0; t <= 75.0; t += 0.25)
+ for (TimeStamp t = dt; t <= 20.0; t += dt)
{
// SIMULATOR
- data.segment<3>(0) = acc_true;
+ // angular momentum
+ L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
- // calculate dL = torque*dt
- dL = torque_true * dt;
- // actualize L = L + dt
- L_true = L_true + dL; // L = L + dt
- // w = L*i_inv we actualize the data
- ang_vel_true(0) = L_true(0) / inertia_true(0);
- ang_vel_true(1) = L_true(1) / inertia_true(1);
- ang_vel_true(2) = L_true(2) / inertia_true(2);
- data.segment<3>(3) = ang_vel_true;
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
- data.segment<3>(6) = force_true;
+ // orientation (quaternion and total angle)
+ // FIXME JS: this should be on top but it works better here, weird...
+ quat_true = quat_true * exp_q(ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/);
+ angle_true += ang_vel_true * dt /*+ 0.5 * ang_acc_true * dt * dt*/;
+
+ // // w = L*i_inv we actualize the data
+ // ang_vel_true(0) = L_true(0) / inertia_true(0);
+ // ang_vel_true(1) = L_true(1) / inertia_true(1);
+ // ang_vel_true(2) = L_true(2) / inertia_true(2);
+ // FTI measurements
+ data.segment<3>(0) = acc_true;
+ data.segment<3>(3) = ang_vel_true;
+ data.segment<3>(6) = force_true;
data.segment<3>(9) = torque_true;
- ang_acc_true(0) = torque_true(0) / inertia_true(0);
- ang_acc_true(1) = torque_true(1) / inertia_true(1);
- ang_acc_true(2) = torque_true(2) / inertia_true(2);
-
- position_data = position_true;
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
- angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
- q_true = q_true * exp_q(angle_true);
- q_data = q_true;
// ESTIMATOR
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
- p->getOrigin()->getFrame()->unfix();
- p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
- p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(q_data.coeffs());
+ // check if new KF
+ if (P->getTrajectory()->getFrameMap().size() > num_kfs)
+ {
+ num_kfs = P->getTrajectory()->getFrameMap().size();
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->unfix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
+ }
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("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("-----------------------------");
}
+ 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.");
@@ -765,8 +785,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
- auto inertia_estimated = S->getStateBlock('i')->getState();
+ // P->print(2,1,1,1);
+
+ auto inertia_estimated = S->getStateBlock('i')->getState();
ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
}