Skip to content
Snippets Groups Projects
Commit 40da132f authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix test ICim all unfixed 

Check for keyframes
Remove 0.5*ang_acc*dt^2
Put orientation at the end
parent a0901eb7
No related branches found
No related tags found
2 merge requests!31devel->main,!30Complete UAV identification setup
Hide whitespace changes
Inline Side-by-side
test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+ 60
− 38
View file @ 40da132f
......@@ -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);
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment