From d5bce2763bec81773beda02033109b22e39c4766 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 23 Aug 2022 12:16:05 +0200
Subject: [PATCH] Add test with nonzero IMU bias and PASS ALL TESTS
---
...problem_force_torque_inertial_dynamics.cpp | 253 +++++++++++++++---
...e_torque_inertial_dynamics_solve_test.yaml | 2 +-
2 files changed, 214 insertions(+), 41 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 8c300b7..4f043af 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -21,6 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
@@ -76,9 +77,9 @@ 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;
+ bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0.0341};
- inertia_true = {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;
}
};
@@ -400,7 +401,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->fix();
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);
CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
@@ -485,11 +486,24 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
// We fix the parameters of the sensor except for the cdm and the mass
S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix();
- S->getStateBlock('I')->unfix();
+ S->getStateBlock('I')->fix();
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->fix();
S->getStateBlock('m')->unfix();
- // S->setStateBlockDynamic('I');
+ S->setStateBlockDynamic('I', false);
+
+ // add regularization to unobservable states
+ S->addPriorParameter('C', // cdm
+ Vector1d(cdm_true(2)), // cdm Z
+ Matrix1d::Identity() * 1e2, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 1e2, // cov XY
+ 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);
@@ -519,7 +533,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); // should show a very low iteration number (possibly 1)
+ WOLF_INFO(report);
WOLF_INFO("True mass : ", mass_true, " Kg.");
WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
@@ -540,7 +554,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
C->process(); // En aquesta lÃnia el programa s'aborta
- p->getOrigin()->getFrame()->unfix();
+ // p->getOrigin()->getFrame()->unfix();
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
@@ -561,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, rotation_around_z_axis_test)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
{
Vector3d inertia_guess (0.01, 0.01, 0.02);
S->getStateBlock('i')->setState(inertia_guess);
@@ -655,33 +669,24 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
ASSERT_NEAR(inertia_estimated(2), inertia_true(2), 1e-6);
}
-TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__zero_bias)
{
- 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);
- S->getStateBlock('i')->setState(inertia_guess);
- double mass_guess = 2.0;
- S->getStateBlock('m')->setState(Vector1d(mass_guess));
-
+ // Simulation data -- hovering and constant torque in yaw
Vector3d acc_true = -gravity();
- Vector3d ang_vel_true (0, 0, 0);
+ Vector3d ang_vel_true(0, 0, 0);
Vector3d force_true = -mass_true * gravity();
- Vector3d torque_true (0, 0, 0.01);
- Vector3d L_true (0, 0, 0);
- Vector3d position_true (0, 0, 0);
- Quaterniond quat_true (1, 0, 0, 0);
- Vector3d ang_acc_true;
+ Vector3d torque_true(0, 0, 0.01);
+ 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 = (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);
ang_acc_true(2) = torque_true(2) / inertia_true(2);
- Vector3d angle_true(0,0,0);
+ Vector3d angle_true(0, 0, 0);
- Vector3d position_data = position_true;
- Vector4d orient_data = quat_true.coeffs();
-
- // Data
+ // Data -- hovering and constant torque in yaw
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
data.segment<3>(3) = ang_vel_true;
@@ -689,6 +694,19 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
data.segment<3>(9) = torque_true;
MatrixXd data_cov = 1 * MatrixXd::Identity(12, 12);
+ Vector3d position_data = position_true;
+ Vector4d orient_data = quat_true.coeffs();
+
+ // Calibration params
+ 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);
+ S->getStateBlock('C')->setState(cdm_guess);
+ 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));
+
S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix();
S->getStateBlock('I')->unfix();
@@ -697,29 +715,30 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
+ // 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());
- C_KF0->getFrame()->unfix();
+ // 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
-
- Vector3d dL;
- double dt = 0.1; // time increment
- std::string report;
+ // Vector3d dL;
+ double dt = 0.2; // time increment
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+ std::string report;
for (TimeStamp t = dt; t <= 8.0; t += dt)
{
// SIMULATOR
// angular momentum
- L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
+ // L_true += torque_true * dt; // FIXME JS: it seems this is not needed!
// ang velocity
ang_vel_true += ang_acc_true * dt;
@@ -735,15 +754,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
// 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>(0) = acc_true; // TODO: add acc bias
+ data.segment<3>(3) = ang_vel_true; // TODO: add gyro bias
data.segment<3>(6) = force_true;
data.segment<3>(9) = torque_true;
// Pose measurements (simulate motion capture)
- position_data = position_true;
- orient_data = quat_true.coeffs();
-
+ position_data = position_true;
+ orient_data = quat_true.coeffs();
// ESTIMATOR
@@ -792,6 +810,161 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
auto inertia_estimated = S->getStateBlock('i')->getState();
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);
+
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__unfix_all__nonzero_bias)
+{
+ // 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();
+
+ // Data -- hovering and constant torque in yaw
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity() + acc_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(6) = -mass_true * gravity();
+ data.segment<3>(9) = torque_true;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ 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() * 1e2, // cov Z
+ 2, // start: Z coordinate
+ 1); // size
+ S->addPriorParameter('i', // inertia
+ Vector2d(inertia_true.segment<2>(0)), // inertia XY
+ Matrix2d::Identity() * 1e2, // 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 = 8.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*/;
+
+ // FTI measurements
+ data.segment<3>(0) = acc_true + acc_bias_true;
+ data.segment<3>(3) = ang_vel_true + gyro_bias_true;
+ data.segment<3>(6) = force_true;
+ data.segment<3>(9) = torque_true;
+
+ // 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);
+
+ // 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);
+
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
@@ -801,6 +974,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.rotation_test_unfixing";
+ // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.hovering_test_without_fixing";
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 12fd236..5552009 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: 1
+ n_fix_first_frames: 0
viral_remove_empty_parent: true
map:
type: "MapBase"
--
GitLab