From 989e74b861781756544d8e4fa63641e933786b92 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Mon, 29 Aug 2022 17:12:10 +0200
Subject: [PATCH] new gtest testing noise
---
src/sensor/sensor_force_torque_inertial.cpp | 8 +
...problem_force_torque_inertial_dynamics.cpp | 255 +++++++++++++++---
...e_torque_inertial_dynamics_solve_test.yaml | 16 +-
3 files changed, 229 insertions(+), 50 deletions(-)
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index aa38e8a..2540ece 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -48,6 +48,14 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
+
+ VectorXd noise_std_vector(12);
+ Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std;
+ Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std;
+ Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
+ Vector3d torque_noise_std_vector =Vector3d::Ones() * params_fti_->torque_noise_std;
+ noise_std_vector << acc_noise_std_vector, gyro_noise_std_vector, force_noise_std_vector, torque_noise_std_vector;
+ setNoiseStd(noise_std_vector);
}
// TODO: Adapt this API to that of MR !448
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 4f043af..68d82b5 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -69,17 +69,17 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
P = Problem::autoSetup(server);
// CERES WRAPPER
solver = std::make_shared<SolverCeres>(P);
- // solver->getSolverOptions().max_num_iterations = 1e4;
- // solver->getSolverOptions().function_tolerance = 1e-15;
- // solver->getSolverOptions().gradient_tolerance = 1e-15;
- // solver->getSolverOptions().parameter_tolerance = 1e-15;
+ solver->getSolverOptions().max_num_iterations = 1e4;
+ solver->getSolverOptions().function_tolerance = 1e-15;
+ solver->getSolverOptions().gradient_tolerance = 1e-15;
+ solver->getSolverOptions().parameter_tolerance = 1e-15;
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0.0341};
- inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {0.017598, 0.017957, 0.029599}
+ inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {0.017598, 0.017957, 0.029599}
mass_true = 1.952;
}
};
@@ -157,10 +157,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
{
- Vector3d cdm_guess (0.01, 0.01, 0.033);
+ Vector3d cdm_guess(0.01, 0.01, 0.033);
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_true));
-
// Data
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
@@ -227,7 +226,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
{
- Vector3d cdm_guess (0.005, 0.005, 0.05);
+ Vector3d cdm_guess(0.005, 0.005, 0.05);
double mass_guess = 1.9;
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_guess));
@@ -311,7 +310,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
{
- Vector3d cdm_guess (0.02, 0.02, 0.033);
+ Vector3d cdm_guess(0.02, 0.02, 0.033);
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_true));
@@ -383,7 +382,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_mom_hovering)
{
- Vector3d cdm_guess (0.05, 0.05, 0.05);
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
double mass_guess = 1.50;
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_guess));
@@ -472,7 +471,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
// Here we only fix P and O from each key frame
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
{
- Vector3d cdm_guess (0.05, 0.05, 0.05);
+ Vector3d cdm_guess(0.05, 0.05, 0.05);
double mass_guess = 1.50;
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('m')->setState(Vector1d(mass_guess));
@@ -504,7 +503,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
0, // start: X coordinate
2); // size: 2
-
CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
@@ -533,7 +531,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
WOLF_INFO("======== SOLVE PROBLEM =======")
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_INFO(report);
+ WOLF_INFO(report);
WOLF_INFO("True mass : ", mass_true, " Kg.");
WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
@@ -577,7 +575,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
// Here we only fix P and O from each key frame
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
{
- Vector3d inertia_guess (0.01, 0.01, 0.02);
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
S->getStateBlock('i')->setState(inertia_guess);
// Data
@@ -640,7 +638,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
ang_acc_true(2) = torque_true(2) / inertia_true(2);
angle_true = ang_vel_true * dt + 0.5 * ang_acc_true * dt * dt;
- quat_true = quat_true * exp_q(angle_true);
+ quat_true = quat_true * exp_q(angle_true);
// ESTIMATOR
@@ -679,7 +677,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
Vector3d L_true(0, 0, 0);
Vector3d position_true(0, 0, 0);
Quaterniond quat_true(1, 0, 0, 0);
- Vector3d ang_acc_true;// = inertia_true.asDiagonal().inverse() * torque_true; // alpha = I.inv * torque is constant
+ Vector3d
+ ang_acc_true; // = inertia_true.asDiagonal().inverse() * torque_true; // alpha = I.inv * torque is constant
// Vector3d ang_acc_true = (torque_true.array() / inertia_true.array()).matrix();
ang_acc_true(0) = torque_true(0) / inertia_true(0);
ang_acc_true(1) = torque_true(1) / inertia_true(1);
@@ -698,11 +697,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
Vector4d orient_data = quat_true.coeffs();
// Calibration params
- Vector6d bias_guess; bias_guess << 0,0,0, 0,0,0;
+ Vector6d bias_guess;
+ bias_guess << 0, 0, 0, 0, 0, 0;
S->getStateBlock('I')->setState(bias_guess);
- Vector3d cdm_guess (0.005, 0.005, 0.1);
+ Vector3d cdm_guess(0.005, 0.005, 0.1);
S->getStateBlock('C')->setState(cdm_guess);
- Vector3d inertia_guess (0.01, 0.01, 0.02);
+ Vector3d inertia_guess(0.01, 0.01, 0.02);
S->getStateBlock('i')->setState(inertia_guess);
double mass_guess = 2.0;
S->getStateBlock('m')->setState(Vector1d(mass_guess));
@@ -721,7 +721,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
// UAV states
- C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
C_KF0->getFrame()->getStateBlock('P')->fix();
C_KF0->getFrame()->getStateBlock('P')->setState(position_data);
C_KF0->getFrame()->getStateBlock('O')->fix();
@@ -741,13 +741,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
// ang velocity
- ang_vel_true += ang_acc_true * dt;
+ ang_vel_true += ang_acc_true * dt;
- // orientation (quaternion and total angle)
+ // orientation (quaternion and total angle)
// FIXME JS: this should be on top but it works better here, weird...
- 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*/;
-
+ 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);
@@ -771,8 +771,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// check if new KF
if (last_kf_id != p->getOrigin()->getFrame()->id())
{
- last_kf_id = p->getOrigin()->getFrame()->id();
-
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
// fix measured position and orientation
p->getOrigin()->getFrame()->unfix();
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
@@ -814,7 +814,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
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);
-
}
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__nonzero_bias)
@@ -830,7 +829,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
Vector3d angle_true = Vector3d(0, 0, 0);
Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
- bias_true <<acc_bias_true, gyro_bias_true;
+ bias_true << acc_bias_true, gyro_bias_true;
// bias_true.setZero();
// Data -- hovering and constant torque in yaw
@@ -845,7 +844,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
Vector4d orient_data = quat_true.coeffs();
// Calibration params
- Vector6d bias_guess = Vector6d::Zero();
+ Vector6d bias_guess = Vector6d::Zero();
// Vector6d bias_guess = bias_true;
Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
@@ -888,8 +887,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
// initialiation of all the variables needed in the following iteration process
- double t_final = 8.0; // total run time
- double dt = 0.1; // time increment
+ double t_final = 8.0; // total run time
+ double dt = 0.1; // time increment
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
std::string report;
@@ -898,15 +897,15 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// SIMULATOR
// ang velocity
- ang_vel_true += ang_acc_true * dt;
-
- // orientation (quaternion and total angle)
- 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*/;
-
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ 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*/;
+
// FTI measurements
data.segment<3>(0) = acc_true + acc_bias_true;
- data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
data.segment<3>(6) = force_true;
data.segment<3>(9) = torque_true;
@@ -922,8 +921,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// check if new KF
if (last_kf_id != p->getOrigin()->getFrame()->id())
{
- last_kf_id = p->getOrigin()->getFrame()->id();
-
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
// fix measured position and orientation
// p->getOrigin()->getFrame()->unfix();
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
@@ -944,7 +943,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
}
}
-
// Final results
WOLF_INFO("True IMU bias * : ", bias_true.transpose(), " m/s2 | rad/s.");
WOLF_INFO("Guess IMU bias : ", bias_guess.transpose(), " m/s2 | rad/s.");
@@ -971,9 +969,180 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
ASSERT_NEAR(mass_estimated, mass_true, 1e-5);
}
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
+ torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise)
+{
+ // Simulation data -- hovering and constant torque in yaw
+ Vector3d acc_true = -gravity();
+ Vector3d ang_vel_true = Vector3d(0, 0, 0);
+ Vector3d force_true = -mass_true * gravity();
+ Vector3d torque_true = Vector3d(0, 0, 0.01);
+ Vector3d position_true = Vector3d(0, 0, 0);
+ Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
+ Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
+ Vector3d angle_true = Vector3d(0, 0, 0);
+ Vector3d acc_bias_true = 0.001 * Vector3d(1, 1, 1);
+ Vector3d gyro_bias_true = 0.001 * Vector3d(1, 1, 1);
+ bias_true << acc_bias_true, gyro_bias_true;
+ // bias_true.setZero();
+
+ // Noise
+ Vector3d acc_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ Vector3d gyro_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ Vector3d force_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ Vector3d torque_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = -mass_true * gravity() + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+ MatrixXd data_cov = S->getNoiseCov();
+
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ Vector6d bias_guess = Vector6d::Zero();
+ // Vector6d bias_guess = bias_true;
+ Vector3d cdm_guess = Vector3d(0.005, 0.005, 0.1);
+ Vector3d inertia_guess = Vector3d(0.01, 0.01, 0.02);
+ double mass_guess = 2.0;
+ S->getStateBlock('I')->setState(bias_guess);
+ S->getStateBlock('C')->setState(cdm_guess);
+ S->getStateBlock('i')->setState(inertia_guess);
+ S->getStateBlock('m')->setState(Vector1d(mass_guess));
+
+ 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 to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * .1, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * .01, // cov XY
+ 0, // start: X coordinate
+ 2); // size: 2
+
+ // Process origin
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ // UAV states
+ // C_KF0->getFrame()->unfix(); // JS: this is unnecessary, all states are unfixed by default
+ 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(orient_data);
+
+ // initialiation of all the variables needed in the following iteration process
+ double t_final = 24.0; // total run time
+ double dt = 0.1; // time increment
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
+
+ for (TimeStamp t = dt; t <= t_final; t += dt)
+ {
+ // SIMULATOR
+
+ // ang velocity
+ ang_vel_true += ang_acc_true * dt;
+
+ // orientation (quaternion and total angle)
+ 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*/;
+
+ // noises
+ acc_noise = Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
+ gyro_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
+ force_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
+ torque_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true + acc_noise;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
+ data.segment<3>(6) = force_true + force_noise;
+ data.segment<3>(9) = torque_true + torque_noise;
+
+ // Pose measurements (simulate motion capture)
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
+
+ // 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()->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);
+
+ // solve!
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ //WOLF_INFO(report);
+ P->print(4,1,1,1);
+
+ // results of this iteration
+ 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("-----------------------------");
+ }
+ }
+
+ // Final results
+ 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 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("-----------------------------");
+
+ auto bias_estimated = S->getStateBlock('I')->getState();
+ auto cdm_estimated = S->getStateBlock('C')->getState();
+ auto inertia_estimated = S->getStateBlock('i')->getState();
+ auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+ EXPECT_MATRIX_APPROX(bias_estimated, bias_true, 1e-3);
+ EXPECT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-3); // cdm_z is not observable while hovering
+ EXPECT_NEAR(inertia_estimated(2), inertia_true(2), 1e-3); // Ix and Iy not observable in this test
+ EXPECT_NEAR(mass_estimated, mass_true, 1e-3);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.hovering_test_without_fixing";
+ ::testing::GTEST_FLAG(filter) =
+ "Test_SolveProblemForceTorqueInertialDynamics_yaml.torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_"
+ "noise";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 5552009..5a4d3b5 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -19,7 +19,7 @@ config:
type: "TreeManagerSlidingWindow"
plugin: "core"
n_frames: 3
- n_fix_first_frames: 0
+ n_fix_first_frames: 1
viral_remove_empty_parent: true
map:
type: "MapBase"
@@ -33,12 +33,14 @@ config:
pose: [0,0,0, 0,0,0,1]
# IMU
- force_noise_std: 0.1 # std dev of force noise in N
- torque_noise_std: 0.1 # std dev of torque noise in N/m
- acc_noise_std: 0.01 # std dev of acc noise in m/s2
- gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
- acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
- gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
# Dynamics
com: [0,0,0.0341] # center of mass [m]
--
GitLab