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

Make batch test and online tests. Both work.

parent 82c5839e
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_simulation_problem_force_torque_inertial_dynamics.cpp
+ 132
− 59
View file @ fb4d8183
......@@ -80,8 +80,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
string line_data;
char delimiter = ',';
std::getline(data_simulation, line_data);
// this acceleration is just to fill a gap, it is not going to be used and it is not true
// a_meas.push_back(Vector3d(0, 0, 0));
int counter = 0;
......@@ -187,31 +185,10 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
torque.push_back(torque_i);
// acceleration -- need to compute from other data
a_meas_i = force_i/mass_true;
a_meas.push_back(a_meas_i);
// if (counter != 0)
// {
// dt = time_stamp[counter] - time_stamp[counter - 1];
// a_meas_i = force_i/mass_true;
// //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity();
// a_meas.push_back(a_meas_i);
// // We have to be careful with the index
// }
// if (counter != 0)
// {
// dt = time_stamp[counter] - time_stamp[counter - 1];
// WOLF_INFO("Massa : ", mass_true);
// WOLF_INFO("Forces : ", (force_i).transpose());
// WOLF_INFO("Acc mesurada amb forces: ", (force_i/mass_true).transpose());
// WOLF_INFO("Acc mesurada amb la vel: ", ((vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity()).transpose())
// //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity();
// // a_meas.push_back(a_meas_i);
// // We have to be careful with the index
// }
counter++;
}
......@@ -235,7 +212,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
P = Problem::autoSetup(server);
solver = std::make_shared<SolverCeres>(P);
// auto options = solver->getSolverOptions();
auto options = solver->getSolverOptions();
// solver->getSolverOptions().max_num_iterations = 1e4;
// solver->getSolverOptions().function_tolerance = 1e-15;
// solver->getSolverOptions().gradient_tolerance = 1e-15;
......@@ -359,7 +336,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
fout.close();
}
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online)
{
// Calibration params
Vector6d bias_guess = S->getStateBlock('I')->getState();
......@@ -376,25 +353,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
S->setStateBlockDynamic('I', false);
// // add regularization, uncomment if the parameter is not fixed
// add regularization, uncomment if the parameter is not fixed
// S->addPriorParameter('C', // cdm
// cdm_guess, // cdm
// 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate
// 3); // size
S->addPriorParameter('I', // cdm
bias_guess, // cdm
0.1*0.1*Matrix6d::Identity()); // cov
// S->addPriorParameter('i', // inertia
// inertia_guess, // inertia
// 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate
// 3);
S->addPriorParameter('C', // cdm
cdm_guess, // cdm
0.1*0.1*Matrix3d::Identity()); // cov
// S->addPriorParameter('m', // mass
// Vector1d(mass_guess), // mass
// 0.1 * 0.1 * Matrix1d::Identity(), // cov
// 0, // start
// 1);
S->addPriorParameter('i', // inertia
inertia_guess, // inertia
0.01*0.01*Matrix3d::Identity()); // cov
S->addPriorParameter('m', // mass
Vector1d(mass_guess), // mass
0.1 * 0.1 * Matrix1d::Identity()); // cov
std::string report;
......@@ -410,10 +385,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
// p->getOrigin()->getFrame()->getStateBlock('V')->fix();
// p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i_init].conjugate() * vlin[i_init]);
P->print(4, 1, 1, 1);
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
......@@ -436,8 +407,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
P->print(4, 1, 1, 1);
// check if new KF
if (last_kf_id != p->getOrigin()->getFrame()->id())
{
......@@ -448,16 +417,15 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
// p->getOrigin()->getFrame()->getStateBlock('V')->fix();
// p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i].conjugate() * vlin[i_init]);
// solve!
// report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
// WOLF_INFO(report);
P->print(4, 1, 1, 1);
// results of this iteration
WOLF_INFO("Time : ", t, " s.");
WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
......@@ -468,18 +436,123 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
WOLF_INFO(report);
// FINAL RESULTS
WOLF_INFO("True bias : ", bias_true.transpose());
WOLF_INFO("Guess bias : ", bias_guess.transpose());
WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose());
WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Estimated 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 center 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("-----------------------------");
}
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
{
// Calibration params
Vector6d bias_guess = S->getStateBlock('I')->getState();
Vector3d cdm_guess = S->getStateBlock('C')->getState();
Vector3d inertia_guess = S->getStateBlock('i')->getState();
double mass_guess = S->getStateBlock('m')->getState()(0);
S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix();
S->getStateBlock('I')->unfix();
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->unfix();
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
// // add regularization, uncomment if the parameter is not fixed
// S->addPriorParameter('I', // bias
// bias_guess, // bias
// 0.1 * 0.1 * Matrix3d::Identity()); // cov
// S->addPriorParameter('C', // cdm
// cdm_guess, // cdm
// 0.01 * 0.01 * Matrix3d::Identity()); // cov
// S->addPriorParameter('i', // inertia
// inertia_guess, // inertia
// 0.01 * 0.01 * Matrix3d::Identity()); // cov
// S->addPriorParameter('m', // mass
// Vector1d(mass_guess), // mass
// 0.1 * 0.1 * Matrix1d::Identity()); // cov
// Force processor to make a KF at t=0
CaptureMotionPtr C0 =
std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
C0->process();
int i_init = 0;
// fix measured position and orientation
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
{
// SIMULATOR
TimeStamp t = time_stamp[i];
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = a_meas[i];
data.segment<3>(3) = vang[i];
data.segment<3>(6) = force[i];
data.segment<3>(9) = torque[i];
MatrixXd data_cov = S->getNoiseCov();
// ESTIMATOR
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
// check if new KF
if (last_kf_id != p->getOrigin()->getFrame()->id())
{
last_kf_id = p->getOrigin()->getFrame()->id();
// fix measured position and orientation
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
}
}
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
// FINAL RESULTS
WOLF_INFO("-----------------------------");
WOLF_INFO(report);
WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
WOLF_INFO("Estimated 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 center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 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("True mass * : ", mass_true, " Kg.");
WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
......@@ -491,7 +564,7 @@ int main(int argc, char** argv)
// ::testing::GTEST_FLAG(filter) =
// "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
::testing::GTEST_FLAG(filter) =
"Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation";
"Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation*";
return RUN_ALL_TESTS();
}
test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+ 1
− 1
View file @ fb4d8183
......@@ -46,7 +46,7 @@ config:
# Dynamics
com: [0.005,0.005,0.01] # center of mass [m]
inertia: [0.013,0.013,0.024] # moments of inertia i_xx, i_yy, i_zz [kg m2]
inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2.00 # mass [kg]
processors:
......
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