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

Use Sensor/ProcessorPose to account for pose measurements

parent 1413ed82
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
+ 44
− 46
View file @ a6221b11
......@@ -38,6 +38,8 @@
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/tree_manager/tree_manager_sliding_window.h>
#include <core/capture/capture_pose.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
......@@ -56,17 +58,27 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
{
public:
ProblemPtr P;
SolverCeresPtr solver;
// FTI sensor
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
SolverCeresPtr solver;
Vector6d bias_true;
Vector3d cdm_true;
Vector3d inertia_true;
double mass_true;
std::vector<double> time_stamp;
double dt;
// Pose sensor
SensorBasePtr SP;
Vector7d pose;
// Reading csv data
std::vector<double> time_stamp;
std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
std::vector<Quaterniond> quaternion;
// Fitxer CSV
std::fstream fout;
......@@ -218,9 +230,12 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
// solver->getSolverOptions().gradient_tolerance = 1e-15;
// solver->getSolverOptions().parameter_tolerance = 1e-15;
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
// FTI sensor and processor
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->findSensor("sensor FTI"));
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
// Pose sensor
SP = P->findSensor("sensor Pose");
}
};
......@@ -380,11 +395,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
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());
// Pose
pose << position[i_init] , quaternion[i_init].coeffs();
CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
CP->process();
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
......@@ -402,25 +416,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
data.segment<3>(9) = torque[i];
MatrixXd data_cov = S->getNoiseCov();
pose << position[i] , quaternion[i].coeffs();
// ESTIMATOR
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
// check if new KF
CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
CP->process();
// check if new KF and solve
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());
// solve!
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
// results of this iteration
WOLF_INFO("Time : ", t, " s.");
......@@ -434,12 +446,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
}
}
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
// FINAL RESULTS
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.");
......@@ -455,7 +462,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
WOLF_INFO("-----------------------------");
ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.11);
ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-3);
ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-2);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 2e-2);
}
......@@ -493,27 +500,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
// S->addPriorParameter('m', // mass
// Vector1d(mass_guess), // mass
// 0.1 * 0.1 * Matrix1d::Identity()); // cov
// 0.01 * 0.01 * Matrix1d::Identity()); // cov
// Force processor to make a KF at t=0
int i_init = 0;
// Force FTI 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();
// Pose
pose << position[i_init] , quaternion[i_init].coeffs();
CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
CP->process();
for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
{
// SIMULATOR
TimeStamp t = time_stamp[i];
// Data
......@@ -523,23 +526,18 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
data.segment<3>(6) = force[i];
data.segment<3>(9) = torque[i];
MatrixXd data_cov = S->getNoiseCov();
pose << position[i] , quaternion[i].coeffs();
// ESTIMATOR
// FTI
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());
}
// Pose
CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
CP->process();
}
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
......@@ -565,7 +563,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.21);
ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 2e-3);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 3e-2);
ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 4e-2);
}
int main(int argc, char** argv)
......
test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+ 24
− 0
View file @ a6221b11
......@@ -49,6 +49,19 @@ config:
inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2.00 # mass [kg]
-
name: sensor Pose
type: SensorPose
plugin: core
extrinsic:
pose: [0,0,0, 0,0,0,1]
std_px: 0.001
std_py: 0.001
std_pz: 0.001
std_ox: 0.001
std_oy: 0.001
std_oz: 0.001
processors:
-
name: "proc FTID"
......@@ -67,3 +80,14 @@ config:
max_buff_length: 999 # motion deltas
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
-
name: prc Pose
type: ProcessorPose
plugin: core
sensor_name: sensor Pose
time_tolerance: 0.01
keyframe_vote:
voting_active: true
apply_loss_function: false
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